Root/usrp/fft.c

Source at commit 95a9e12e2e0f539c204b8512655a1e6e93f1ef52 created 8 years 6 months ago.
By Werner Almesberger, usrp/fft.c: made window function user-selectable, added hann, blackman, rect
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{
96    float c[2];
97    float *re = NULL, *im = NULL;
98    double *res;
99    int e = 0, n = 0;
100    int i;
101    double a;
102
103    while (1) {
104        size_t s;
105
106        s = fread(c, sizeof(c), 1, stdin);
107        if (!s) {
108            if (!ferror(stdin))
109                break;
110            if (s < 0) {
111                perror("read");
112                exit(1);
113            }
114        }
115        if (skip) {
116            skip--;
117            continue;
118        }
119        if (e <= n) {
120            e = e ? e*2 : 10000;
121            re = realloc(re, e*sizeof(float));
122            im = realloc(im, e*sizeof(float));
123            if (!re || !im) {
124                perror("realloc");
125                exit(1);
126            }
127        }
128        re[n] = c[0];
129        im[n] = c[1];
130        n++;
131    }
132
133    if (skip >= n) {
134        fprintf(stderr, "cannot skip %d of %d entries\n", skip, n);
135        exit(1);
136    }
137    re += skip;
138    im += skip;
139    n -= skip;
140
141    res = malloc(n*sizeof(double));
142    if (!res) {
143        perror("malloc");
144        exit(1);
145    }
146
147    switch (alg) {
148    case 0:
149        fft_complex(n, re, im, res);
150        break;
151    case 1:
152        fft_real(n, re, im, res);
153        break;
154    default:
155        abort();
156    }
157
158    if (dump) {
159        for (i = 0; i < n; i += 1)
160            printf("%d %g\n", i,
161                10*log(res[(i+n/2) % n])/log(10));
162    } else {
163        double s = 0;
164        double db;
165
166        if (high >= n+skip) {
167            fprintf(stderr, "end %d > number of samples %d\n",
168                high, n+skip);
169            exit(1);
170        }
171        low = low*(double) n/(n+skip);
172        high = high*(double) n/(n+skip);
173        if (high < n)
174            high++;
175        if (low == high)
176            low--;
177        for (i = low; i != high; i++) {
178            a = res[i];
179            db = 10*log(a)/log(10);
180            if (db >= threshold)
181                s += a;
182        }
183        printf("%f\n", 10*log(s)/log(10));
184    }
185}
186
187
188static void usage(const char *name)
189{
190    fprintf(stderr,
191"usage: %s [-s skip] [-w window] low high [threshold]\n"
192" %s [-s skip] [-w window] -d\n\n"
193" threshold only use frequency bins with at least this power, in - dB.\n"
194" E.g., a threshold value of 60 would be -60 dB. (default: %d\n"
195" dB)\n"
196" -d dump frequency-domain \n"
197" -s skip skip this number of samples from the beginning (default: 0)\n"
198" -w window use the specified window function. Available: blackman, hann,\n"
199" hamming, rectangle. Default is rectangle.\n"
200    , name, name, -DEFAULT_THRESHOLD);
201    exit(1);
202}
203
204
205int main(int argc, char **argv)
206{
207    int dump = 0, skip = 0;
208    int low, high;
209    double threshold = DEFAULT_THRESHOLD;
210    int c;
211
212    while ((c = getopt(argc, argv, "a:ds:w:")) != EOF)
213        switch (c) {
214        case 'a':
215            alg = atoi(optarg);
216            break;
217        case 'd':
218            dump = 1;
219            break;
220        case 's':
221            skip = atoi(optarg);
222            break;
223        case 'w':
224            if (!strcmp(optarg, "blackman"))
225                window = window_blackman;
226            else if (!strcmp(optarg, "hann"))
227                window = window_hann;
228            else if (!strcmp(optarg, "hamming"))
229                window = window_hamming;
230            else if (!strcmp(optarg, "rectangle"))
231                window = window_rectangle;
232            else
233                usage(*argv);
234            break;
235        default:
236            usage(*argv);
237        }
238
239    switch (argc-optind) {
240    case 0:
241        if (!dump)
242            usage(*argv);
243        do_fft(skip, 1, 0, 0, 0);
244        break;
245    case 3:
246        threshold = -atof(argv[optind+2]);
247        /* fall through */
248    case 2:
249        if (dump)
250            usage(*argv);
251        low = atoi(argv[optind]);
252        high = atoi(argv[optind+1]);
253        if (low > high)
254            usage(*argv);
255        do_fft(skip, 0, low, high, threshold);
256        break;
257    default:
258        usage(*argv);
259    }
260
261    return 0;
262}
263

Archive Download this file



interactive