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 23c37261b7e9772f7aedd2506dbb71c4abee8f54 created 13 years 3 months ago. By Werner Almesberger, atusb: use 0402 as the preferred component size and replace bad TVS | |
---|---|
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; |
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 | (void) a; |
50 | |
51 | for (i = 0; i != n; i++) |
52 | in[i] = hypot(re[i], im[i]); |
53 | |
54 | plan = fftw_plan_r2r_1d(n, in, res, FFTW_REDFT10, FFTW_ESTIMATE); |
55 | fftw_execute(plan); |
56 | |
57 | for (i = 0; i != n; i++) |
58 | res[i] /= n; |
59 | } |
60 | |
61 | |
62 | static void do_fft(int skip, int dump, int low, int high, double threshold) |
63 | { |
64 | float c[2]; |
65 | float *re = NULL, *im = NULL; |
66 | double *res; |
67 | int e = 0, n = 0; |
68 | int i; |
69 | double a; |
70 | |
71 | while (1) { |
72 | size_t s; |
73 | |
74 | s = fread(c, sizeof(c), 1, stdin); |
75 | if (!s) { |
76 | if (!ferror(stdin)) |
77 | break; |
78 | if (s < 0) { |
79 | perror("read"); |
80 | exit(1); |
81 | } |
82 | } |
83 | if (skip) { |
84 | skip--; |
85 | continue; |
86 | } |
87 | if (e <= n) { |
88 | e = e ? e*2 : 10000; |
89 | re = realloc(re, e*sizeof(float)); |
90 | im = realloc(im, e*sizeof(float)); |
91 | if (!re || !im) { |
92 | perror("realloc"); |
93 | exit(1); |
94 | } |
95 | } |
96 | re[n] = c[0]; |
97 | im[n] = c[1]; |
98 | n++; |
99 | } |
100 | |
101 | if (skip >= n) { |
102 | fprintf(stderr, "cannot skip %d of %d entries\n", skip, n); |
103 | exit(1); |
104 | } |
105 | re += skip; |
106 | im += skip; |
107 | n -= skip; |
108 | |
109 | res = malloc(n*sizeof(double)); |
110 | if (!res) { |
111 | perror("malloc"); |
112 | exit(1); |
113 | } |
114 | |
115 | switch (alg) { |
116 | case 0: |
117 | fft_complex(n, re, im, res); |
118 | break; |
119 | case 1: |
120 | fft_real(n, re, im, res); |
121 | break; |
122 | default: |
123 | abort(); |
124 | } |
125 | |
126 | if (dump) { |
127 | for (i = 0; i < n; i += 1) |
128 | printf("%d %g\n", i, 10*log(res[i])/log(10)); |
129 | } else { |
130 | double s = 0; |
131 | double db; |
132 | |
133 | if (high >= n+skip) { |
134 | fprintf(stderr, "end %d > number of samples %d\n", |
135 | high, n+skip); |
136 | exit(1); |
137 | } |
138 | low = low*(double) n/(n+skip); |
139 | high = high*(double) n/(n+skip); |
140 | if (high < n) |
141 | high++; |
142 | if (low == high) |
143 | low--; |
144 | for (i = low; i != high; i++) { |
145 | a = res[i]; |
146 | db = 10*log(a)/log(10); |
147 | if (db >= threshold) |
148 | s += a; |
149 | } |
150 | printf("%f\n", 10*log(s)/log(10)); |
151 | } |
152 | } |
153 | |
154 | |
155 | static void usage(const char *name) |
156 | { |
157 | fprintf(stderr, |
158 | "usage: %s [-s skip] low high [threshold]\n" |
159 | " %s [-s skip] -d\n\n" |
160 | " threshold only use frequency bins with at least this power, in - dB.\n" |
161 | " E.g., a threshold value of 60 would be -60 dB. (default: %d\n" |
162 | " dB)\n" |
163 | " -d dump frequency-domain \n" |
164 | " -s skip skip this number of samples from the beginning (default: 0)\n" |
165 | , name, name, -DEFAULT_THRESHOLD); |
166 | exit(1); |
167 | } |
168 | |
169 | |
170 | int main(int argc, char **argv) |
171 | { |
172 | int dump = 0, skip = 0; |
173 | int low, high; |
174 | double threshold = DEFAULT_THRESHOLD; |
175 | int c; |
176 | |
177 | while ((c = getopt(argc, argv, "a:ds:")) != EOF) |
178 | switch (c) { |
179 | case 'a': |
180 | alg = atoi(optarg); |
181 | break; |
182 | case 'd': |
183 | dump = 1; |
184 | break; |
185 | case 's': |
186 | skip = atoi(optarg); |
187 | break; |
188 | default: |
189 | usage(*argv); |
190 | } |
191 | |
192 | switch (argc-optind) { |
193 | case 0: |
194 | if (!dump) |
195 | usage(*argv); |
196 | do_fft(skip, 1, 0, 0, 0); |
197 | break; |
198 | case 3: |
199 | threshold = -atof(argv[optind+2]); |
200 | /* fall through */ |
201 | case 2: |
202 | if (dump) |
203 | usage(*argv); |
204 | low = atoi(argv[optind]); |
205 | high = atoi(argv[optind+1]); |
206 | if (low > high) |
207 | usage(*argv); |
208 | do_fft(skip, 0, low, high, threshold); |
209 | break; |
210 | default: |
211 | usage(*argv); |
212 | } |
213 | |
214 | return 0; |
215 | } |
216 |