Debian Codegraph @

libairspyhf/src/iqbalancer.c

raw ◀ ▶
Source reconstructed at patch state 0 of 2 — not the installed (fully patched) source.
2 Debian patches touch this file
1 /*
2 Copyright (c) 2016-2018, Youssef Touil <[email protected]>
3 Copyright (c) 2018, Leif Asbrink <[email protected]>
4
5 All rights reserved.
6
7 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
8
9 Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
10 Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the
11 documentation and/or other materials provided with the distribution.
12 Neither the name of Airspy HF+ nor the names of its contributors may be used to endorse or promote products derived from this software
13 without specific prior written permission.
14
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
16 THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
17 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
18 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
19 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
20 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
21 */
22
23 #include <stdlib.h>
24 #include <stdint.h>
25 #include <stdio.h>
26 #include <string.h>
27 #include <math.h>
28
29 #include "iqbalancer.h"
30
31 #ifndef MATH_PI
32 #define MATH_PI 3.14159265359
33 #endif
34
35 #define EPSILON 0.01f
36 #define WorkingBufferLength (FFTBins * (1 + FFTIntegration / FFTOverlap))
37
38 struct iq_balancer_t
39 {
40 float phase;
41 float last_phase;
42
43 float amplitude;
44 float last_amplitude;
45
46 float iavg;
47 float qavg;
48 float integrated_total_power;
49 float integrated_image_power;
50 float maximum_image_power;
51
52 float raw_phases[MaxLookback];
53 float raw_amplitudes[MaxLookback];
54
55 int skipped_buffers;
56 int buffers_to_skip;
57 int working_buffer_pos;
58 int fft_integration;
59 int fft_overlap;
60 int correlation_integration;
61
62 int no_of_avg;
63 int no_of_raw;
64 int raw_ptr;
65 int optimal_bin;
66 int reset_flag;
67 int *power_flag;
68
69 complex_t *corr;
70 complex_t *corr_plus;
71 complex_t *working_buffer;
72 float *boost;
73 };
74
75 static uint8_t __lib_initialized = 0;
76 static float __fft_window[FFTBins];
77 static float __boost_window[FFTBins];
78
79 static void __init_library()
80 {
81 int i;
82
83 if (__lib_initialized)
84 {
85 return;
86 }
87
88 const int length = FFTBins - 1;
89
90 for (i = 0; i <= length; i++)
91 {
92 __fft_window[i] = (float)(
93 +0.35875f
94 - 0.48829f * cos(2.0 * MATH_PI * i / length)
95 + 0.14128f * cos(4.0 * MATH_PI * i / length)
96 - 0.01168f * cos(6.0 * MATH_PI * i / length)
97 );
98 __boost_window[i] = (float)(1.0 / BoostFactor + 1.0 / exp(pow(i * 2.0 / BinsToOptimize, 2.0)));
99 }
100
101 __lib_initialized = 1;
102 }
103
104 static void window(complex_t *buffer, int length)
105 {
106 int i;
107 for (i = 0; i < length; i++)
108 {
109 buffer[i].re *= __fft_window[i];
110 buffer[i].im *= __fft_window[i];
111 }
112 }
113
114 static void fft(complex_t *buffer, int length)
115 {
116 int nm1 = length - 1;
117 int nd2 = length / 2;
118 int i, j, jm1, k, l, m, le, le2, ip;
119 complex_t u, t, r;
120
121 m = 0;
122 i = length;
123 while (i > 1)
124 {
125 ++m;
126 i = (i >> 1);
127 }
128
129 j = nd2;
130
131 for (i = 1; i < nm1; ++i)
132 {
133 if (i < j)
134 {
135 t = buffer[j];
136 buffer[j] = buffer[i];
137 buffer[i] = t;
138 }
139
140 k = nd2;
141
142 while (k <= j)
143 {
144 j = j - k;
145 k = k / 2;
146 }
147
148 j += k;
149 }
150
151 for (l = 1; l <= m; ++l)
152 {
153 le = 1 << l;
154 le2 = le / 2;
155
156 u.re = 1.0f;
157 u.im = 0.0f;
158
159 r.re = (float)cos(MATH_PI / le2);
160 r.im = (float)-sin(MATH_PI / le2);
161
162 for (j = 1; j <= le2; ++j)
163 {
164 jm1 = j - 1;
165
166 for (i = jm1; i <= nm1; i += le)
167 {
168 ip = i + le2;
169
170 t.re = u.re * buffer[ip].re - u.im * buffer[ip].im;
171 t.im = u.im * buffer[ip].re + u.re * buffer[ip].im;
172
173 buffer[ip].re = buffer[i].re - t.re;
174 buffer[ip].im = buffer[i].im - t.im;
175
176 buffer[i].re += t.re;
177 buffer[i].im += t.im;
178 }
179
180 t.re = u.re * r.re - u.im * r.im;
181 t.im = u.im * r.re + u.re * r.im;
182
183 u.re = t.re;
184 u.im = t.im;
185 }
186 }
187
188 for (i = 0; i < nd2; i++)
189 {
190 j = nd2 + i;
191 t = buffer[i];
192 buffer[i] = buffer[j];
193 buffer[j] = t;
194 }
195 }
196
197 static void cancel_dc(struct iq_balancer_t *iq_balancer, complex_t* iq, int length)
198 {
199 int i;
200 float iavg = iq_balancer->iavg;
201 float qavg = iq_balancer->qavg;
202
203 for (i = 0; i < length; i++)
204 {
205 iavg += DcTimeConst * (iq[i].re - iavg);
206 qavg += DcTimeConst * (iq[i].im - qavg);
207
208 iq[i].re -= iavg;
209 iq[i].im -= qavg;
210 }
211
212 iq_balancer->iavg = iavg;
213 iq_balancer->qavg = qavg;
214 }
215
216 static float adjust_benchmark(struct iq_balancer_t *iq_balancer, complex_t *iq, float phase, float amplitude, int skip_power_calculation)
217 {
218 int i;
219 float sum = 0;
220 for (i = 0; i < FFTBins; i++)
221 {
222 float re = iq[i].re;
223 float im = iq[i].im;
224
225 iq[i].re += phase * im;
226 iq[i].im += phase * re;
227
228 iq[i].re *= 1 + amplitude;
229 iq[i].im *= 1 - amplitude;
230 if (!skip_power_calculation)
231 sum += re * re + im * im;
232 }
233 return sum;
234 }
235
236 static complex_t multiply_complex_complex(complex_t *a, const complex_t *b)
237 {
238 complex_t result;
239 result.re = a->re * b->re - a->im * b->im;
240 result.im = a->im * b->re + a->re * b->im;
241 return result;
242 }
243
244 static int compute_corr(struct iq_balancer_t *iq_balancer, complex_t* iq, complex_t* ccorr, int length, int step)
245 {
246 complex_t cc, fftPtr[FFTBins];
247 int n, m;
248 int i, j;
249 int count = 0;
250 float power;
251 float phase = iq_balancer->phase + step * PhaseStep;
252 float amplitude = iq_balancer->amplitude + step * AmplitudeStep;
253
254 for (n = 0, m = 0; n <= length - FFTBins && m < iq_balancer->fft_integration; n += FFTBins / iq_balancer->fft_overlap, m++)
255 {
256 memcpy(fftPtr, iq + n, FFTBins * sizeof(complex_t));
257 power = adjust_benchmark(iq_balancer, fftPtr, phase, amplitude, step);
258 if (step == 0)
259 {
260 if (power > MinimumPower)
261 {
262 iq_balancer->power_flag[m] = 1;
263 iq_balancer->integrated_total_power += power;
264 }
265 else
266 {
267 iq_balancer->power_flag[m] = 0;
268 }
269 }
270 if (iq_balancer->power_flag[m] == 1)
271 {
272 count++;
273 window(fftPtr, FFTBins);
274 fft(fftPtr, FFTBins);
275 for (i = EdgeBinsToSkip, j = FFTBins - EdgeBinsToSkip; i <= FFTBins - EdgeBinsToSkip; i++, j--)
276 {
277 cc = multiply_complex_complex(fftPtr + i, fftPtr + j);
278 ccorr[i].re += cc.re;
279 ccorr[i].im += cc.im;
280
281 ccorr[j].re = ccorr[i].re;
282 ccorr[j].im = ccorr[i].im;
283 }
284 if (step == 0)
285 {
286 for (i = EdgeBinsToSkip; i <= FFTBins - EdgeBinsToSkip; i++)
287 {
288 power = fftPtr[i].re * fftPtr[i].re + fftPtr[i].im * fftPtr[i].im;
289 iq_balancer->boost[i] += power;
290 if (iq_balancer->optimal_bin == FFTBins / 2)
291 {
292 iq_balancer->integrated_image_power += power;
293 }
294 else
295 {
296 iq_balancer->integrated_image_power += power * __boost_window[abs(FFTBins - i - iq_balancer->optimal_bin)];
297 }
298 }
299 }
300 }
301 }
302
303 return count;
304 }
305
306 static complex_t utility(struct iq_balancer_t *iq_balancer, complex_t* ccorr)
307 {
308 int i;
309 int j;
310 float invskip = 1.0f / EdgeBinsToSkip;
311 complex_t acc = { 0, 0 };
312 for (i = EdgeBinsToSkip, j = FFTBins - EdgeBinsToSkip; i <= FFTBins - EdgeBinsToSkip; i++, j--)
313 {
314 int distance = abs(i - FFTBins / 2);
315 if (distance > CenterBinsToSkip)
316 {
317 float weight = (distance > EdgeBinsToSkip) ? 1.0f : (distance * invskip);
318 if (iq_balancer->optimal_bin != FFTBins / 2)
319 {
320 weight *= __boost_window[abs(iq_balancer->optimal_bin - i)];
321 }
322 weight *= iq_balancer->boost[j] / (iq_balancer->boost[i] + EPSILON);
323 acc.re += ccorr[i].re * weight;
324 acc.im += ccorr[i].im * weight;
325 }
326 }
327 return acc;
328 }
329
330 static void estimate_imbalance(struct iq_balancer_t *iq_balancer, complex_t* iq, int length)
331 {
332 int i, j;
333 float amplitude, phase, mu;
334 complex_t a, b;
335
336 if (iq_balancer->reset_flag)
337 {
338 iq_balancer->reset_flag = 0;
339 iq_balancer->no_of_avg = -BuffersToSkipOnReset;
340 iq_balancer->maximum_image_power = 0;
341 }
342
343 if (iq_balancer->no_of_avg < 0)
344 {
345 iq_balancer->no_of_avg++;
346 return;
347 }
348 else if (iq_balancer->no_of_avg == 0)
349 {
350 iq_balancer->integrated_image_power = 0;
351 iq_balancer->integrated_total_power = 0;
352 memset(iq_balancer->boost, 0, FFTBins * sizeof(float));
353 memset(iq_balancer->corr, 0, FFTBins * sizeof(complex_t));
354 memset(iq_balancer->corr_plus, 0, FFTBins * sizeof(complex_t));
355 }
356
357 iq_balancer->maximum_image_power *= MaxPowerDecay;
358
359 i = compute_corr(iq_balancer, iq, iq_balancer->corr, length, 0);
360 if (i == 0)
361 return;
362
363 iq_balancer->no_of_avg += i;
364 compute_corr(iq_balancer, iq, iq_balancer->corr_plus, length, 1);
365
366 if (iq_balancer->no_of_avg <= iq_balancer->correlation_integration * iq_balancer->fft_integration)
367 return;
368
369 iq_balancer->no_of_avg = 0;
370
371 if (iq_balancer->optimal_bin == FFTBins / 2)
372 {
373 if (iq_balancer->integrated_total_power < iq_balancer->maximum_image_power)
374 return;
375 iq_balancer->maximum_image_power = iq_balancer->integrated_total_power;
376 }
377 else
378 {
379 if (iq_balancer->integrated_image_power - iq_balancer->integrated_total_power * BoostWindowNorm < iq_balancer->maximum_image_power * PowerThreshold)
380 return;
381 iq_balancer->maximum_image_power = iq_balancer->integrated_image_power - iq_balancer->integrated_total_power * BoostWindowNorm;
382 }
383
384 a = utility(iq_balancer, iq_balancer->corr);
385 b = utility(iq_balancer, iq_balancer->corr_plus);
386
387 mu = a.im - b.im;
388 if (fabs(mu) > MinDeltaMu)
389 {
390 mu = a.im / mu;
391 if (mu < -MaxMu)
392 mu = -MaxMu;
393 else if (mu > MaxMu)
394 mu = MaxMu;
395 }
396 else
397 {
398 mu = 0;
399 }
400
401 phase = iq_balancer->phase + PhaseStep * mu;
402
403 mu = a.re - b.re;
404 if (fabs(mu) > MinDeltaMu)
405 {
406 mu = a.re / mu;
407 if (mu < -MaxMu)
408 mu = -MaxMu;
409 else if (mu > MaxMu)
410 mu = MaxMu;
411 }
412 else
413 {
414 mu = 0;
415 }
416
417 amplitude = iq_balancer->amplitude + AmplitudeStep * mu;
418
419 if (iq_balancer->no_of_raw < MaxLookback)
420 iq_balancer->no_of_raw++;
421 iq_balancer->raw_amplitudes[iq_balancer->raw_ptr] = amplitude;
422 iq_balancer->raw_phases[iq_balancer->raw_ptr] = phase;
423 i = iq_balancer->raw_ptr;
424 for (j = 0; j < iq_balancer->no_of_raw - 1; j++)
425 {
426 i = (i + MaxLookback - 1) & (MaxLookback - 1);
427 phase += iq_balancer->raw_phases[i];
428 amplitude += iq_balancer->raw_amplitudes[i];
429 }
430 phase /= iq_balancer->no_of_raw;
431 amplitude /= iq_balancer->no_of_raw;
432 iq_balancer->raw_ptr = (iq_balancer->raw_ptr + 1) & (MaxLookback - 1);
433
434 iq_balancer->phase = phase;
435 iq_balancer->amplitude = amplitude;
436 }
437
438 static void adjust_phase_amplitude(struct iq_balancer_t *iq_balancer, complex_t* iq, int length)
439 {
440 int i;
441 float scale = 1.0f / (length - 1);
442
443 for (i = 0; i < length; i++)
444 {
445 float phase = (i * iq_balancer->last_phase + (length - 1 - i) * iq_balancer->phase) * scale;
446 float amplitude = (i * iq_balancer->last_amplitude + (length - 1 - i) * iq_balancer->amplitude) * scale;
447
448 float re = iq[i].re;
449 float im = iq[i].im;
450
451 iq[i].re += phase * im;
452 iq[i].im += phase * re;
453
454 iq[i].re *= 1 + amplitude;
455 iq[i].im *= 1 - amplitude;
456 }
457
458 iq_balancer->last_phase = iq_balancer->phase;
459 iq_balancer->last_amplitude = iq_balancer->amplitude;
460 }
461
462 void ADDCALL iq_balancer_process(struct iq_balancer_t *iq_balancer, complex_t* iq, int length)
463 {
464 int count;
465
466 cancel_dc(iq_balancer, iq, length);
467
468 count = WorkingBufferLength - iq_balancer->working_buffer_pos;
469 if (count >= length)
470 {
471 count = length;
472 }
473 memcpy(iq_balancer->working_buffer + iq_balancer->working_buffer_pos, iq, count * sizeof(complex_t));
474 iq_balancer->working_buffer_pos += count;
475 if (iq_balancer->working_buffer_pos >= WorkingBufferLength)
476 {
477 iq_balancer->working_buffer_pos = 0;
478
479 if (++iq_balancer->skipped_buffers > iq_balancer->buffers_to_skip)
480 {
481 iq_balancer->skipped_buffers = 0;
482 estimate_imbalance(iq_balancer, iq_balancer->working_buffer, WorkingBufferLength);
483 }
484 }
485
486 adjust_phase_amplitude(iq_balancer, iq, length);
487 }
488
489 void ADDCALL iq_balancer_set_optimal_point(struct iq_balancer_t *iq_balancer, float w)
490 {
491 if (w < -0.5f)
492 {
493 w = -0.5f;
494 }
495 else if (w > 0.5f)
496 {
497 w = 0.5f;
498 }
499
500 iq_balancer->optimal_bin = (int)floor(FFTBins * (0.5 + w));
501 iq_balancer->reset_flag = 1;
502 }
503
504 void ADDCALL iq_balancer_configure(struct iq_balancer_t *iq_balancer, int buffers_to_skip, int fft_integration, int fft_overlap, int correlation_integration)
505 {
506 iq_balancer->buffers_to_skip = buffers_to_skip;
507 iq_balancer->fft_integration = fft_integration;
508 iq_balancer->fft_overlap = fft_overlap;
509 iq_balancer->correlation_integration = correlation_integration;
510
511 free(iq_balancer->power_flag);
512 iq_balancer->power_flag = (int *)malloc(iq_balancer->fft_integration * sizeof(int));
513 memset(iq_balancer->power_flag, 0, iq_balancer->fft_integration * sizeof(int));
514
515 iq_balancer->reset_flag = 1;
516 }
517
518 struct iq_balancer_t * ADDCALL iq_balancer_create(float initial_phase, float initial_amplitude)
519 {
520 struct iq_balancer_t *instance = (struct iq_balancer_t *) malloc(sizeof(struct iq_balancer_t));
521 memset(instance, 0, sizeof(struct iq_balancer_t));
522
523 instance->phase = initial_phase;
524 instance->amplitude = initial_amplitude;
525
526 instance->optimal_bin = FFTBins / 2;
527
528 instance->buffers_to_skip = BuffersToSkip;
529 instance->fft_integration = FFTIntegration;
530 instance->fft_overlap = FFTOverlap;
531 instance->correlation_integration = CorrelationIntegration;
532
533 instance->corr = (complex_t *)malloc(FFTBins * sizeof(complex_t));
534 instance->corr_plus = (complex_t *)malloc(FFTBins * sizeof(complex_t));
535 instance->working_buffer = (complex_t *)malloc(WorkingBufferLength * sizeof(complex_t));
536 instance->boost = (float *)malloc(FFTBins * sizeof(float));
537 instance->power_flag = (int *)malloc(instance->fft_integration * sizeof(int));
538
539 __init_library();
540 return instance;
541 }
542
543 void ADDCALL iq_balancer_destroy(struct iq_balancer_t *iq_balancer)
544 {
545 free(iq_balancer->corr);
546 free(iq_balancer->corr_plus);
547 free(iq_balancer->working_buffer);
548 free(iq_balancer->boost);
549 free(iq_balancer->power_flag);
550 free(iq_balancer);
551 }