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 49e7c83796bc04941e9dbcec69bc0751563ff4d4 created 7 years 6 days ago. By Werner Almesberger, atusb/: use ""VDD" symbol from kicad-libs | |
---|---|
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 | |
13 | static int alg = 0; |
14 | |
15 | |
16 | static double window_rectangle(int i, int n) |
17 | { |
18 | return 1; |
19 | } |
20 | |
21 | |
22 | static double window_hann(int i, int n) |
23 | { |
24 | return 0.5-0.5*cos(M_PI*2*i/(n-1)); |
25 | } |
26 | |
27 | |
28 | static double window_hamming(int i, int n) |
29 | { |
30 | return 0.54-0.46*cos(M_PI*2*i/(n-1)); |
31 | } |
32 | |
33 | |
34 | static 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 | |
40 | static double (*window)(int i, int n) = window_rectangle; |
41 | |
42 | |
43 | static 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 | |
71 | static 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 | |
94 | static 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 | |
207 | static 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 | |
225 | int 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 |