Line | Branch | Decision | Exec | Source |
---|---|---|---|---|
1 | #pragma once | |||
2 | ||||
3 | namespace fft { | |||
4 | ||||
5 | #ifndef M_PI | |||
6 | #define M_PI 3.1415926535897932 | |||
7 | #endif | |||
8 | ||||
9 | ||||
10 | 1 | inline bool isPow(const size_t num) | ||
11 | { | |||
12 |
2/4✓ Branch 0 taken 1 time.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 time.
✗ Branch 3 not taken.
|
1 | return num && (!(num & (num - 1))); | |
13 | } | |||
14 | ||||
15 | 1 | void rerrange(complex_type* data, const size_t num_elements) | ||
16 | { | |||
17 | 1 | size_t target_index = 0; | ||
18 | size_t bit_mask; | |||
19 | ||||
20 | 1 | complex_type buffer; | ||
21 |
2/2✓ Branch 0 taken 1024 times.
✓ Branch 1 taken 1 time.
|
2/2✓ Decision 'true' taken 1024 times.
✓ Decision 'false' taken 1 time.
|
1025 | for (size_t i = 0; i < num_elements; ++i) |
22 | { | |||
23 |
2/2✓ Branch 0 taken 496 times.
✓ Branch 1 taken 528 times.
|
2/2✓ Decision 'true' taken 496 times.
✓ Decision 'false' taken 528 times.
|
1024 | if (target_index > i) |
24 | { | |||
25 | 496 | buffer = data[target_index]; | ||
26 | 496 | data[target_index] = data[i]; | ||
27 | 496 | data[i]= buffer; | ||
28 | } | |||
29 | ||||
30 | 1024 | bit_mask = num_elements; | ||
31 | ||||
32 |
2/2✓ Branch 0 taken 1023 times.
✓ Branch 1 taken 1024 times.
|
2/2✓ Decision 'true' taken 1023 times.
✓ Decision 'false' taken 1024 times.
|
2047 | while (target_index & (bit_mask >>= 1)) |
33 | { | |||
34 | 1023 | target_index &= ~bit_mask; | ||
35 | } | |||
36 | ||||
37 | 1024 | target_index |= bit_mask; | ||
38 | } | |||
39 | 1 | } | ||
40 | ||||
41 | 1 | bool transform(complex_type* data, const size_t count) | ||
42 | { | |||
43 | 1 | double local_pi = -M_PI; | ||
44 | ||||
45 | size_t next, match; | |||
46 | real_type sine; | |||
47 | real_type delta; | |||
48 | 1 | complex_type mult, factor, product; | ||
49 | ||||
50 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 time.
|
2/2✓ Decision 'true' taken 10 times.
✓ Decision 'false' taken 1 time.
|
11 | for (size_t i = 1; i < count; i <<= 1) |
51 | { | |||
52 | 10 | next = i << 1; | ||
53 | 10 | delta = local_pi / i; | ||
54 | 10 | sine = sin(0.5 * delta); | ||
55 | ||||
56 | 10 | mult = complex_type(-2.0 * sine * sine, sin(delta)); | ||
57 | 10 | factor = 1.0; | ||
58 | ||||
59 |
2/2✓ Branch 0 taken 1023 times.
✓ Branch 1 taken 10 times.
|
2/2✓ Decision 'true' taken 1023 times.
✓ Decision 'false' taken 10 times.
|
1033 | for (size_t j = 0; j < i; ++j) |
60 | { | |||
61 |
2/2✓ Branch 0 taken 5120 times.
✓ Branch 1 taken 1023 times.
|
2/2✓ Decision 'true' taken 5120 times.
✓ Decision 'false' taken 1023 times.
|
6143 | for (size_t k = j; k < count; k += next) |
62 | { | |||
63 | 5120 | match = k + i; | ||
64 | ||||
65 |
1/1✓ Branch 1 taken 5120 times.
|
5120 | product = data[match] * factor; | |
66 |
1/1✓ Branch 1 taken 5120 times.
|
5120 | data[match] = data[k] - product; | |
67 | 5120 | data[k] += product; | ||
68 | } | |||
69 | ||||
70 |
1/1✓ Branch 2 taken 1023 times.
|
1023 | factor = mult * factor + factor; | |
71 | } | |||
72 | } | |||
73 | ||||
74 | 2 | return true; | ||
75 | } | |||
76 | ||||
77 | 1 | static bool ffti(complex_type* data, const size_t size) | ||
78 | { | |||
79 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 time.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 1 time.
|
1 | if(!isPow(size)) { |
80 | ✗ | return false; | ||
81 | } | |||
82 | ||||
83 | 1 | rerrange(data, size); | ||
84 | ||||
85 | 1 | return transform(data, size); | ||
86 | } | |||
87 | ||||
88 | 1 | bool fft_adc_sample(float * w, float ratio, float sensitivity, const adcsample_t* data_in, complex_type* data_out, const size_t size) | ||
89 | { | |||
90 |
2/2✓ Branch 0 taken 1024 times.
✓ Branch 1 taken 1 time.
|
2/2✓ Decision 'true' taken 1024 times.
✓ Decision 'false' taken 1 time.
|
1025 | for(size_t i = 0; i < size; ++i) { |
91 | 1024 | float voltage = ratio * data_in[i]; | ||
92 | 1024 | data_out[i] = complex_type(sensitivity * voltage * w[i], 0.0); | ||
93 | } | |||
94 | ||||
95 | 1 | return ffti(data_out, size); | ||
96 | } | |||
97 | ||||
98 | ✗ | bool fft_adc_sample_filtered(Biquad& knockFilter, float * w, float ratio, float sensitivity, const adcsample_t* data_in, complex_type* data_out, const size_t size) | ||
99 | { | |||
100 | ✗ | for(size_t i = 0; i < size; ++i) { | ||
101 | ✗ | float voltage = ratio * data_in[i]; | ||
102 | ✗ | float filtered = knockFilter.filter(voltage); | ||
103 | ✗ | data_out[i] = complex_type(filtered * w[i] * sensitivity, 0.0); | ||
104 | } | |||
105 | ||||
106 | ✗ | return ffti(data_out, size); | ||
107 | } | |||
108 | ||||
109 | ✗ | bool fft(const real_type* data_in, complex_type* data_out, const size_t size) | ||
110 | { | |||
111 | ✗ | for(size_t i = 0; i < size; ++i) { | ||
112 | ✗ | data_out[i] = complex_type(data_in[i], 0.0); | ||
113 | } | |||
114 | ||||
115 | ✗ | return ffti(data_out, size); | ||
116 | } | |||
117 | ||||
118 | // Fast inverse square root aka "Quake 3 fast inverse square root" | |||
119 | ✗ | float fast_sqrt(float x) { | ||
120 | union | |||
121 | { | |||
122 | float x; | |||
123 | int32_t i; | |||
124 | } u; | |||
125 | ✗ | u.x = x; | ||
126 | ✗ | u.i = 0x5f375a86 - (u.i >> 1); | ||
127 | ✗ | float xu = x * u.x; | ||
128 | ✗ | float xu2 = xu * u.x; | ||
129 | ✗ | u.x = (0.125 * 3.0) * xu * (5.0 - xu2 * ((10.0 / 3.0) - xu2)); | ||
130 | ✗ | return u.x; | ||
131 | } | |||
132 | ||||
133 | ✗ | float amplitude(const complex_type& fft) { | ||
134 | ✗ | return fast_sqrt(fft.real()*fft.real() + fft.imag()*fft.imag()); | ||
135 | } | |||
136 | ||||
137 | ✗ | void cosine_window(float * w, unsigned n, const float * coeff, unsigned ncoeff, bool sflag) | ||
138 | { | |||
139 | ✗ | if (n == 1) | ||
140 | { | |||
141 | ✗ | w[0] = 1.0; | ||
142 | } | |||
143 | else | |||
144 | { | |||
145 | ✗ | const unsigned wlength = sflag ? (n - 1) : n; | ||
146 | ||||
147 | ✗ | for (unsigned i = 0; i < n; ++i) | ||
148 | { | |||
149 | ✗ | float wi = 0.0; | ||
150 | ||||
151 | ✗ | for (unsigned j = 0; j < ncoeff; ++j) | ||
152 | { | |||
153 | ✗ | wi += coeff[j] * cos(i * j * 2.0 * M_PI / wlength); | ||
154 | } | |||
155 | ||||
156 | ✗ | w[i] = wi; | ||
157 | } | |||
158 | } | |||
159 | ✗ | } | ||
160 | ||||
161 | ✗ | void rectwin(float * w, unsigned n) | ||
162 | { | |||
163 | ✗ | for (unsigned i = 0; i < n; ++i) | ||
164 | { | |||
165 | ✗ | w[i] = 1.0; | ||
166 | } | |||
167 | ✗ | } | ||
168 | ||||
169 | ✗ | void hann(float * w, unsigned n, bool sflag) | ||
170 | { | |||
171 | ✗ | const float coeff[2] = { 0.5, -0.5 }; | ||
172 | ||||
173 | ✗ | cosine_window(w, n, coeff, sizeof(coeff) / sizeof(float), sflag); | ||
174 | ✗ | } | ||
175 | ||||
176 | ✗ | void hamming(float * w, unsigned n, bool sflag) | ||
177 | { | |||
178 | ✗ | const float coeff[2] = { 0.54, -0.46 }; | ||
179 | ✗ | cosine_window(w, n, coeff, sizeof(coeff) / sizeof(float), sflag); | ||
180 | ✗ | } | ||
181 | ||||
182 | ✗ | void blackman(float * w, unsigned n, bool sflag) | ||
183 | { | |||
184 | ✗ | const float coeff[3] = { 0.42, -0.5, 0.08 }; | ||
185 | ✗ | cosine_window(w, n, coeff, sizeof(coeff) / sizeof(float), sflag); | ||
186 | ✗ | } | ||
187 | ||||
188 | ✗ | void blackmanharris(float * w, unsigned n, bool sflag) | ||
189 | { | |||
190 | ✗ | const float coeff[4] = { 0.35875, -0.48829, 0.14128, -0.01168 }; | ||
191 | ✗ | cosine_window(w, n, coeff, sizeof(coeff) / sizeof(float), sflag); | ||
192 | ✗ | } | ||
193 | ||||
194 | } | |||
195 |