Root/usrp/fft.c

1#include <stdlib.h>
2#include <stdio.h>
3#include <unistd.h>
4#include <string.h>
5#include <math.h>
6#include <sys/types.h>
7
8#include <fftw3.h>
9
10
11#define DEFAULT_THRESHOLD 100
12
13static int alg = 0;
14
15
16static double window_rectangle(int i, int n)
17{
18    return 1;
19}
20
21
22static double window_hann(int i, int n)
23{
24    return 0.5-0.5*cos(M_PI*2*i/(n-1));
25}
26
27
28static double window_hamming(int i, int n)
29{
30    return 0.54-0.46*cos(M_PI*2*i/(n-1));
31}
32
33
34static double window_blackman(int i, int n)
35{
36    return 0.42-0.5*cos(M_PI*2*i/(n-1))+0.08*cos(M_PI*4*i/(n-1));
37}
38
39
40static double (*window)(int i, int n) = window_rectangle;
41
42
43static void fft_complex(int n, const float *re, const float *im, double *res)
44{
45    fftw_complex *in, *out;
46    fftw_plan plan;
47    int i;
48    double a;
49
50    in = fftw_malloc(sizeof(fftw_complex)*n);
51    out = fftw_malloc(sizeof(fftw_complex)*n);
52
53    for (i = 0; i != n; i++) {
54        double w = window(i, n);
55
56        in[i][0] = re[i]*w;
57        in[i][1] = im[i]*w;
58    }
59
60    plan = fftw_plan_dft_1d(n, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
61    fftw_execute(plan);
62
63    for (i = 0; i != n; i++) {
64        a = hypot(out[i][0], out[i][1]); // /n;
65        a = a*a/n;
66        res[i] = a;
67    }
68}
69
70
71static void fft_real(int n, const float *re, const float *im, double *res)
72{
73    double *in;
74    fftw_plan plan;
75    int i;
76    double a ;
77
78    in = fftw_malloc(sizeof(double)*n);
79
80    for (i = 0; i != n; i++) {
81        a = hypot(re[i], im[i]);
82        in[i] = a*a;
83    }
84
85    plan = fftw_plan_r2r_1d(n, in, res, FFTW_REDFT10, FFTW_ESTIMATE);
86    fftw_execute(plan);
87
88    /* @@@ not sure at all about the scaling */
89    for (i = 0; i != n; i++)
90        res[i] = res[i]/sqrt(n);
91}
92
93
94static void do_fft(int skip, int dump, int low, int high, double threshold,
95    int split)
96{
97    float c[2];
98    float *re = NULL, *im = NULL;
99    double *out, *res;
100    int e = 0, n = 0;
101    int i, j, off;
102    double a;
103
104    while (1) {
105        size_t s;
106
107        s = fread(c, sizeof(c), 1, stdin);
108        if (!s) {
109            if (!ferror(stdin))
110                break;
111            if (s < 0) {
112                perror("read");
113                exit(1);
114            }
115        }
116        if (skip) {
117            skip--;
118            continue;
119        }
120        if (e <= n) {
121            e = e ? e*2 : 10000;
122            re = realloc(re, e*sizeof(float));
123            im = realloc(im, e*sizeof(float));
124            if (!re || !im) {
125                perror("realloc");
126                exit(1);
127            }
128        }
129        re[n] = c[0];
130        im[n] = c[1];
131        n++;
132    }
133
134    if (skip >= n) {
135        fprintf(stderr, "cannot skip %d of %d entries\n", skip, n);
136        exit(1);
137    }
138    re += skip;
139    im += skip;
140    n -= skip;
141
142    out = malloc(n/split*sizeof(double));
143    if (!out) {
144        perror("malloc");
145        exit(1);
146    }
147
148    res = malloc(n/split*sizeof(double));
149    if (!res) {
150        perror("malloc");
151        exit(1);
152    }
153
154    for (i = 0; i != n/split; i++)
155        res[i] = 0;
156
157    off = 0;
158    for (i = 0; i != split; i++) {
159        switch (alg) {
160        case 0:
161            fft_complex(n/split, re+off, im+off, out);
162            break;
163        case 1:
164            fft_real(n/split, re+off, im+off, out);
165            break;
166        default:
167            abort();
168        }
169        for (j = 0; j != n/split; j++)
170            res[j] += out[j];
171        off += n/split;
172    }
173    for (i = 0; i != n/split; i++)
174        res[i] /= split;
175
176    if (dump) {
177        for (i = 0; i != n/split; i++)
178            printf("%g\n",
179                10*log(res[(i+(n/split)/2) % (n/split)])/log(10));
180    } else {
181        /* @@@ need to think about supporting averaged FFT here later */
182        double s = 0;
183        double db;
184
185        if (high >= n+skip) {
186            fprintf(stderr, "end %d > number of samples %d\n",
187                high, n+skip);
188            exit(1);
189        }
190        low = low*(double) n/(n+skip);
191        high = high*(double) n/(n+skip);
192        if (high < n)
193            high++;
194        if (low == high)
195            low--;
196        for (i = low; i != high; i++) {
197            a = res[i];
198            db = 10*log(a)/log(10);
199            if (db >= threshold)
200                s += a;
201        }
202        printf("%f\n", 10*log(s)/log(10));
203    }
204}
205
206
207static void usage(const char *name)
208{
209    fprintf(stderr,
210"usage: %s [-s skip] [-w window] low high [threshold]\n"
211" %s [-s skip] [-w window] -d [split]\n\n"
212" threshold only use frequency bins with at least this power, in - dB.\n"
213" E.g., a threshold value of 60 would be -60 dB. (default: %d\n"
214" dB)\n"
215" -d [split] dump frequency-domain, optionally splitting the samples into\n"
216" several parts and averaging over them.\n"
217" -s skip skip this number of samples from the beginning (default: 0)\n"
218" -w window use the specified window function. Available: blackman, hann,\n"
219" hamming, rectangle. Default is rectangle.\n"
220    , name, name, -DEFAULT_THRESHOLD);
221    exit(1);
222}
223
224
225int main(int argc, char **argv)
226{
227    int dump = 0, skip = 0;
228    int low, high;
229    double threshold = DEFAULT_THRESHOLD;
230    int c;
231
232    while ((c = getopt(argc, argv, "a:ds:w:")) != EOF)
233        switch (c) {
234        case 'a':
235            alg = atoi(optarg);
236            break;
237        case 'd':
238            dump = 1;
239            break;
240        case 's':
241            skip = atoi(optarg);
242            break;
243        case 'w':
244            if (!strcmp(optarg, "blackman"))
245                window = window_blackman;
246            else if (!strcmp(optarg, "hann"))
247                window = window_hann;
248            else if (!strcmp(optarg, "hamming"))
249                window = window_hamming;
250            else if (!strcmp(optarg, "rectangle"))
251                window = window_rectangle;
252            else
253                usage(*argv);
254            break;
255        default:
256            usage(*argv);
257        }
258
259    switch (argc-optind) {
260    case 0:
261        if (!dump)
262            usage(*argv);
263        do_fft(skip, 1, 0, 0, 0, 1);
264        break;
265    case 1:
266        if (!dump)
267            usage(*argv);
268        do_fft(skip, 1, 0, 0, 0, atoi(argv[optind]));
269        break;
270    case 3:
271        threshold = -atof(argv[optind+2]);
272        /* fall through */
273    case 2:
274        if (dump)
275            usage(*argv);
276        low = atoi(argv[optind]);
277        high = atoi(argv[optind+1]);
278        if (low > high)
279            usage(*argv);
280        do_fft(skip, 0, low, high, threshold, 1);
281        break;
282    default:
283        usage(*argv);
284    }
285
286    return 0;
287}
288

Archive Download this file



interactive