Root/usrp/fft.c

Source at commit ebe667197a3c09a1f9b3005efcf7729c8b12da84 created 9 years 6 months ago.
By Werner Almesberger, dirtly made the atrf tools run with the new atusd board
1#include <stdlib.h>
2#include <stdio.h>
3#include <unistd.h>
4#include <math.h>
5#include <sys/types.h>
6
7#include <fftw3.h>
8
9
10#define DEFAULT_THRESHOLD 100
11
12static int alg = 0;
13
14
15static void fft_complex(int n, const float *re, const float *im, double *res)
16{
17    fftw_complex *in, *out;
18    fftw_plan plan;
19    int i;
20    double a;
21
22    in = fftw_malloc(sizeof(fftw_complex)*n);
23    out = fftw_malloc(sizeof(fftw_complex)*n);
24
25    for (i = 0; i != n; i++) {
26        in[i][0] = re[i];
27        in[i][1] = im[i];
28    }
29
30    plan = fftw_plan_dft_1d(n, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
31    fftw_execute(plan);
32
33    for (i = 0; i != n; i++) {
34        a = hypot(out[i][0], out[i][1]); // /n;
35        a = a*a/n;
36        res[i] = a;
37    }
38}
39
40
41static void fft_real(int n, const float *re, const float *im, double *res)
42{
43    double *in;
44    fftw_plan plan;
45    int i;
46    double a ;
47
48    in = fftw_malloc(sizeof(double)*n);
49
50    for (i = 0; i != n; i++) {
51        a = hypot(re[i], im[i]);
52        in[i] = a*a;
53    }
54
55    plan = fftw_plan_r2r_1d(n, in, res, FFTW_REDFT10, FFTW_ESTIMATE);
56    fftw_execute(plan);
57
58    /* @@@ not sure at all about the scaling */
59    for (i = 0; i != n; i++)
60        res[i] = res[i]/sqrt(n);
61}
62
63
64static void do_fft(int skip, int dump, int low, int high, double threshold)
65{
66    float c[2];
67    float *re = NULL, *im = NULL;
68    double *res;
69    int e = 0, n = 0;
70    int i;
71    double a;
72
73    while (1) {
74        size_t s;
75
76        s = fread(c, sizeof(c), 1, stdin);
77        if (!s) {
78            if (!ferror(stdin))
79                break;
80            if (s < 0) {
81                perror("read");
82                exit(1);
83            }
84        }
85        if (skip) {
86            skip--;
87            continue;
88        }
89        if (e <= n) {
90            e = e ? e*2 : 10000;
91            re = realloc(re, e*sizeof(float));
92            im = realloc(im, e*sizeof(float));
93            if (!re || !im) {
94                perror("realloc");
95                exit(1);
96            }
97        }
98        re[n] = c[0];
99        im[n] = c[1];
100        n++;
101    }
102
103    if (skip >= n) {
104        fprintf(stderr, "cannot skip %d of %d entries\n", skip, n);
105        exit(1);
106    }
107    re += skip;
108    im += skip;
109    n -= skip;
110
111    res = malloc(n*sizeof(double));
112    if (!res) {
113        perror("malloc");
114        exit(1);
115    }
116
117    switch (alg) {
118    case 0:
119        fft_complex(n, re, im, res);
120        break;
121    case 1:
122        fft_real(n, re, im, res);
123        break;
124    default:
125        abort();
126    }
127
128    if (dump) {
129        for (i = 0; i < n; i += 1)
130            printf("%d %g\n", i, 10*log(res[i])/log(10));
131    } else {
132        double s = 0;
133        double db;
134
135        if (high >= n+skip) {
136            fprintf(stderr, "end %d > number of samples %d\n",
137                high, n+skip);
138            exit(1);
139        }
140        low = low*(double) n/(n+skip);
141        high = high*(double) n/(n+skip);
142        if (high < n)
143            high++;
144        if (low == high)
145            low--;
146        for (i = low; i != high; i++) {
147            a = res[i];
148            db = 10*log(a)/log(10);
149            if (db >= threshold)
150                s += a;
151        }
152        printf("%f\n", 10*log(s)/log(10));
153    }
154}
155
156
157static void usage(const char *name)
158{
159    fprintf(stderr,
160"usage: %s [-s skip] low high [threshold]\n"
161" %s [-s skip] -d\n\n"
162" threshold only use frequency bins with at least this power, in - dB.\n"
163" E.g., a threshold value of 60 would be -60 dB. (default: %d\n"
164" dB)\n"
165" -d dump frequency-domain \n"
166" -s skip skip this number of samples from the beginning (default: 0)\n"
167    , name, name, -DEFAULT_THRESHOLD);
168    exit(1);
169}
170
171
172int main(int argc, char **argv)
173{
174    int dump = 0, skip = 0;
175    int low, high;
176    double threshold = DEFAULT_THRESHOLD;
177    int c;
178
179    while ((c = getopt(argc, argv, "a:ds:")) != EOF)
180        switch (c) {
181        case 'a':
182            alg = atoi(optarg);
183            break;
184        case 'd':
185            dump = 1;
186            break;
187        case 's':
188            skip = atoi(optarg);
189            break;
190        default:
191            usage(*argv);
192        }
193
194    switch (argc-optind) {
195    case 0:
196        if (!dump)
197            usage(*argv);
198        do_fft(skip, 1, 0, 0, 0);
199        break;
200    case 3:
201        threshold = -atof(argv[optind+2]);
202        /* fall through */
203    case 2:
204        if (dump)
205            usage(*argv);
206        low = atoi(argv[optind]);
207        high = atoi(argv[optind+1]);
208        if (low > high)
209            usage(*argv);
210        do_fft(skip, 0, low, high, threshold);
211        break;
212    default:
213        usage(*argv);
214    }
215
216    return 0;
217}
218

Archive Download this file



interactive