IEEE 802.15.4 subsystem
Sign in or create your account | Project List | Help
IEEE 802.15.4 subsystem Git Source Tree
Root/
Source at commit ec21e4ba4756379934fc24635438040f66d2ab7c created 13 years 1 month ago. By Werner Almesberger, atusb/fw2: support device -> host side of the ATUSB EP0 protocol | |
---|---|
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 | |
12 | static int alg = 0; |
13 | |
14 | |
15 | static 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 | |
41 | static 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 | |
64 | static 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 | |
157 | static 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 | |
172 | int 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 |