From 3f05240ff78325197010909518f3233a72fa2c84 Mon Sep 17 00:00:00 2001
From: prog <youssef@airspy.com>
Date: Mon, 30 Jul 2018 12:26:23 +0200
Subject: [PATCH 7/7] Updated the IQ correction algorithm with the latest
 tweaks. Tributes to Leif Asbrink SM5BSZ for making this achievement possible.

---
 libairspyhf/src/airspyhf.c   |  15 +-
 libairspyhf/src/airspyhf.h   |   4 +-
 libairspyhf/src/iqbalancer.c | 374 ++++++++++++++++++++++++-------------------
 libairspyhf/src/iqbalancer.h |  60 ++++---
 4 files changed, 243 insertions(+), 210 deletions(-)

diff --git a/libairspyhf/src/airspyhf.c b/libairspyhf/src/airspyhf.c
index 1351bf6..c75c7fe 100644
--- a/libairspyhf/src/airspyhf.c
+++ b/libairspyhf/src/airspyhf.c
@@ -83,7 +83,7 @@ typedef struct {
 
 #pragma pack(pop)
 
-struct airspyhf_device
+typedef struct airspyhf_device
 {
 	libusb_context* usb_context;
 	libusb_device_handle* usb_device;
@@ -102,7 +102,7 @@ struct airspyhf_device
 	volatile int32_t calibration_ppb;
 	uint8_t enable_dsp;
 	airspyhf_complex_float_t vec;
-	iq_balancer_t iq_balancer;
+	struct iq_balancer_t *iq_balancer;
 	uint32_t transfer_count;
 	uint32_t buffer_size;
 	uint32_t dropped_buffers;
@@ -115,7 +115,7 @@ struct airspyhf_device
 	volatile int received_buffer_count;
 	airspyhf_complex_float_t *output_buffer;
 	void* ctx;
-};
+} airspyhf_device_t;
 
 typedef struct calibration_record
 {
@@ -307,7 +307,7 @@ static void convert_samples(airspyhf_device_t* device, airspyhf_complex_int16_t
 
 	if (device->enable_dsp)
 	{
-		iq_balancer_process(&device->iq_balancer, dest, count);
+		iq_balancer_process(device->iq_balancer, dest, count);
 
 		for (i = 0; i < count; i++)
 		{
@@ -894,7 +894,7 @@ static int airspyhf_open_init(airspyhf_device_t** device, uint64_t serial_number
 		lib_device->calibration_ppb = 0;
 	}
 
-	iq_balancer_init(&lib_device->iq_balancer);
+	lib_device->iq_balancer = iq_balancer_create();
 
 	*device = lib_device;
 
@@ -940,6 +940,7 @@ int ADDCALL airspyhf_close(airspyhf_device_t* device)
 		airspyhf_open_exit(device);
 		free_transfers(device);
 		free(device->supported_samplerates);
+		iq_balancer_destroy(device->iq_balancer);
 		free(device);
 	}
 
@@ -1039,7 +1040,6 @@ int ADDCALL airspyhf_start(airspyhf_device_t* device, airspyhf_sample_block_cb_f
 
 	device->vec.re = 1.0f;
 	device->vec.im = 0.0f;
-	iq_balancer_init(&device->iq_balancer);
 
 	result = airspyhf_set_receiver_mode(device, RECEIVER_MODE_OFF);
 	if (result != AIRSPYHF_SUCCESS)
@@ -1069,6 +1069,7 @@ int ADDCALL airspyhf_stop(airspyhf_device_t* device)
 	int result1, result2;
 	result1 = kill_io_threads(device);
 	result2 = airspyhf_set_receiver_mode(device, RECEIVER_MODE_OFF);
+
 	if (result2 != AIRSPYHF_SUCCESS)
 	{
 		return result2;
@@ -1203,7 +1204,7 @@ int ADDCALL airspyhf_set_calibration(airspyhf_device_t* device, int32_t ppb)
 
 int ADDCALL airspyhf_set_optimal_iq_correction_point(airspyhf_device_t* device, float w)
 {
-	iq_balancer_set_optimal_point(&device->iq_balancer, w);
+	iq_balancer_set_optimal_point(device->iq_balancer, w);
 	return AIRSPYHF_SUCCESS;
 }
 
diff --git a/libairspyhf/src/airspyhf.h b/libairspyhf/src/airspyhf.h
index 29cfd76..1fa9559 100644
--- a/libairspyhf/src/airspyhf.h
+++ b/libairspyhf/src/airspyhf.h
@@ -28,9 +28,9 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
 #include <stdint.h>
 
-#define AIRSPYHF_VERSION "1.3.0"
+#define AIRSPYHF_VERSION "1.4.0"
 #define AIRSPYHF_VER_MAJOR 1
-#define AIRSPYHF_VER_MINOR 3
+#define AIRSPYHF_VER_MINOR 4
 #define AIRSPYHF_VER_REVISION 0
 
 #define AIRSPYHF_ENDPOINT_IN (1)
diff --git a/libairspyhf/src/iqbalancer.c b/libairspyhf/src/iqbalancer.c
index a97ccdd..3271fea 100644
--- a/libairspyhf/src/iqbalancer.c
+++ b/libairspyhf/src/iqbalancer.c
@@ -1,5 +1,6 @@
 /*
 Copyright (c) 2016-2018, Youssef Touil <youssef@airspy.com>
+Copyright (c) 2018, Leif Asbrink <leif@sm5bsz.com>
 
 All rights reserved.
 
@@ -27,16 +28,50 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
 #include "iqbalancer.h"
 
+#ifndef MATH_PI
 #define MATH_PI 3.14159265359
+#endif
 
-static uint8_t __window_initialized = 0;
-static float __window[FFTBins];
+#define EPSILON 0.01f
 
-static void __init_window()
+struct iq_balancer_t
+{
+	float phase;
+	float last_phase;
+	float phase_step;
+
+	float amplitude;
+	float last_amplitude;
+	float amplitude_step;
+
+	float iavg;
+	float qavg;
+
+	float raw_phases[MaxLookback];
+	float raw_amplitudes[MaxLookback];
+	int no_of_avg;
+	int no_of_raw;
+	int raw_ptr;
+	int initial_skip;
+
+	int optimal_bin;
+
+	complex_t *corr;
+	complex_t *corr_plus;
+	float *boost;
+
+	int estimation_count;
+};
+
+static uint8_t __lib_initialized = 0;
+static float __fft_window[FFTBins];
+static float __boost_window[FFTBins];
+
+static void __init_library()
 {
 	int i;
 
-	if (__window_initialized)
+	if (__lib_initialized)
 	{
 		return;
 	}
@@ -45,36 +80,34 @@ static void __init_window()
 
 	for (i = 0; i <= length; i++)
 	{
-		__window[i] = (float) (
-			+ 0.27105140069342
-			- 0.43329793923448 * cos(2.0 * MATH_PI * i / length)
-			+ 0.21812299954311 * cos(4.0 * MATH_PI * i / length)
-			- 0.06592544638803 * cos(6.0 * MATH_PI * i / length)
-			+ 0.01081174209837 * cos(8.0 * MATH_PI * i / length)
-			- 0.00077658482522 * cos(10.0 * MATH_PI * i / length)
-			+ 0.00001388721735 * cos(12.0 * MATH_PI * i / length)
+		__fft_window[i] = (float) (
+			+ 0.35875f
+			- 0.48829f * cos(2.0 * MATH_PI * i / length)
+			+ 0.14128f * cos(4.0 * MATH_PI * i / length)
+			- 0.01168f * cos(6.0 * MATH_PI * i / length)
 			);
+		__boost_window[i] = (float) (1.0 / BoostFactor + 1.0 / exp(pow(i * 2.0 / BinsToOptimize, 2.0)));
 	}
 
-	__window_initialized = 1;
+	__lib_initialized = 1;
 }
 
-static void window(airspyhf_complex_float_t *buffer, int length)
+static void window(complex_t *buffer, int length)
 {
 	int i;
 	for (i = 0; i < length; i++)
 	{
-		buffer[i].re *= __window[i];
-		buffer[i].im *= __window[i];
+		buffer[i].re *= __fft_window[i];
+		buffer[i].im *= __fft_window[i];
 	}
 }
 
-static void fft(airspyhf_complex_float_t *buffer, int length)
+static void fft(complex_t *buffer, int length)
 {
 	int nm1 = length - 1;
 	int nd2 = length / 2;
 	int i, j, jm1, k, l, m, le, le2, ip;
-	airspyhf_complex_float_t u, t, r;
+	complex_t u, t, r;
 
 	m = 0;
 	i = length;
@@ -142,9 +175,17 @@ static void fft(airspyhf_complex_float_t *buffer, int length)
 			u.im = t.im;
 		}
 	}
+
+	for (i = 0; i < nd2; i++)
+	{
+		j = nd2 + i;
+		t = buffer[i];
+		buffer[i] = buffer[j];
+		buffer[j] = t;
+	}
 }
 
-static void cancel_dc(iq_balancer_t *iq_balancer, airspyhf_complex_float_t* iq, int length)
+static void cancel_dc(struct iq_balancer_t *iq_balancer, complex_t* iq, int length)
 {
 	int i;
 	float iavg = iq_balancer->iavg;
@@ -163,7 +204,7 @@ static void cancel_dc(iq_balancer_t *iq_balancer, airspyhf_complex_float_t* iq,
 	iq_balancer->qavg = qavg;
 }
 
-static void adjust_benchmark(airspyhf_complex_float_t *iq, float phase, float amplitude)
+static void adjust_benchmark(complex_t *iq, float phase, float amplitude)
 {
 	int i;
 	for (i = 0; i < FFTBins; i++)
@@ -179,178 +220,167 @@ static void adjust_benchmark(airspyhf_complex_float_t *iq, float phase, float am
 	}
 }
 
-static airspyhf_complex_float_t multiply_complex_complex(airspyhf_complex_float_t *a, const airspyhf_complex_float_t *b)
+static complex_t multiply_complex_complex(complex_t *a, const complex_t *b)
 {
-	airspyhf_complex_float_t result;
+	complex_t result;
 	result.re = a->re * b->re - a->im * b->im;
 	result.im = a->im * b->re + a->re * b->im;
 	return result;
 }
 
-static float fsign(const float x)
-{
-	return x >= 0 ? 1.0f : -1.0f;
-}
-
-static float utility(iq_balancer_t *iq_balancer, airspyhf_complex_float_t* iq, int length, float phase, float amplitude)
+static void compute_corr(struct iq_balancer_t *iq_balancer, complex_t* iq, complex_t* ccorr, int length, int step)
 {
-	airspyhf_complex_float_t fftPtr[FFTBins];
-	float corr[FFTBins / 2];
-
-	memset(corr, 0, FFTBins / 2 * sizeof(float));
-
-	for (int n = 0, m = 0; n < length - FFTBins && m < FFTIntegration; n += FFTBins, m++)
+	complex_t cc, fftPtr[FFTBins];
+	int n, m;
+	int i, j;
+	float phase = iq_balancer->phase + step * iq_balancer->phase_step;
+	float amplitude = iq_balancer->amplitude + step * iq_balancer->amplitude_step;
+	memset(fftPtr, 0, FFTBins * sizeof(complex_t));
+	for (n = 0, m = 0; n <= length - FFTBins && m < FFTIntegration; n += FFTBins / 2, m++)
 	{
-		memcpy(fftPtr, iq + n, FFTBins * sizeof(airspyhf_complex_float_t));
-
+		memcpy(fftPtr, iq + n, FFTBins * sizeof(complex_t));
 		adjust_benchmark(fftPtr, phase, amplitude);
 		window(fftPtr, FFTBins);
 		fft(fftPtr, FFTBins);
+		for (i = 1, j = FFTBins - 1; i < FFTBins / 2; i++, j--)
+		{
+			cc = multiply_complex_complex(fftPtr + i, fftPtr + j);
+			ccorr[i].re += cc.re;
+			ccorr[i].im += cc.im;
 
-		for (int i = 1, j = FFTBins - 1; i < FFTBins / 2 - BinsToSkip; i++, j--)
+			ccorr[j].re = ccorr[i].re;
+			ccorr[j].im = ccorr[i].im;
+		}
+		if (step == 0)
 		{
-			airspyhf_complex_float_t prod = multiply_complex_complex(fftPtr + i, fftPtr + j);
-			corr[i] += prod.re * prod.re + prod.im * prod.im;
+			for (i = 0; i < FFTBins; i++)
+			{
+				iq_balancer->boost[i] += fftPtr[i].re * fftPtr[i].re + fftPtr[i].im * fftPtr[i].im;
+			}
 		}
 	}
+}
 
-	float acc1 = 0.0f;
-	float acc2 = 0.0f;
+static complex_t utility(struct iq_balancer_t *iq_balancer, complex_t* ccorr)
+{
+	int i;
+	int j;
 	float invskip = 1.0f / BinsToSkip;
-	int count1 = 0;
-	int count2 = 0;
-
-	for (int i = 1, j = FFTBins - 1; i < FFTBins / 2 - BinsToSkip; i++, j--)
+	complex_t acc = { 0, 0 };
+	for (i = 1, j = FFTBins - 1; i < FFTBins; i++, j--)
 	{
-		float weight = (i > BinsToSkip) ? 1.0f : (i * invskip);
-
-		if (i >= iq_balancer->optimal_bin - BinsToOptimize / 2 && i <= iq_balancer->optimal_bin + BinsToOptimize / 2)
-		{
-			acc1 += corr[i] * weight;
-			count1++;
-		}
-		else
+		int distance = abs(i - FFTBins / 2);
+		if (distance > 0)
 		{
-			acc2 += corr[i] * weight;
-			count2++;
+			float weight = (distance > BinsToSkip) ? 1.0f : (distance * invskip);
+			weight *= __boost_window[abs(iq_balancer->optimal_bin - i)];
+			weight *= iq_balancer->boost[j] / (iq_balancer->boost[i] + EPSILON);
+			acc.re += ccorr[i].re * weight;
+			acc.im += ccorr[i].im * weight;
 		}
 	}
-
-	if (count1 == 0)
-	{
-		return acc2;
-	}
-
-	acc1 /= count1;
-	acc2 /= count2;
-
-	return acc1 * BoostFactor + acc2;
+	return acc;
 }
 
-static int estimate_imbalance(iq_balancer_t *iq_balancer, airspyhf_complex_float_t* iq, int length)
+static void estimate_imbalance(struct iq_balancer_t *iq_balancer, complex_t* iq, int length)
 {
-	int result = 0;
-	float old_phase = iq_balancer->phase;
-	float old_amplitude = iq_balancer->amplitude;
-
-	float u = utility(iq_balancer, iq, length, old_phase, old_amplitude);
+	int i, j;
+	float amplitude, phase, mu;
+	complex_t a, b;
 
-	float phase = iq_balancer->phase + iq_balancer->phase_step;
-	if (phase > MaxPhaseCorrection)
-	{
-		phase = MaxPhaseCorrection;
-	}
-	else if (phase < -MaxPhaseCorrection)
+	if (iq_balancer->no_of_avg == 0)
 	{
-		phase = -MaxPhaseCorrection;
+		memset(iq_balancer->boost, 0, FFTBins * sizeof(float));
+		memset(iq_balancer->corr, 0, FFTBins * sizeof(complex_t));
+		memset(iq_balancer->corr_plus, 0, FFTBins * sizeof(complex_t));
 	}
-
-	float candidate_utility = utility(iq_balancer, iq, length, phase, old_amplitude);
-
-	if (candidate_utility < u)
+	compute_corr(iq_balancer, iq, iq_balancer->corr, length, 0);
+	compute_corr(iq_balancer, iq, iq_balancer->corr_plus, length, 1);
+	iq_balancer->no_of_avg++;
+	if (iq_balancer->no_of_avg <= CorrelationIntegration)
+		return;
+	iq_balancer->no_of_avg = 0;
+	iq_balancer->no_of_raw++;
+	if (iq_balancer->no_of_raw > MaxLookback - 1)
+		iq_balancer->no_of_raw = MaxLookback - 1;
+	iq_balancer->initial_skip++;
+	if (iq_balancer->initial_skip > MaxLookback - 1)
+		iq_balancer->initial_skip = MaxLookback - 1;
+	a = utility(iq_balancer, iq_balancer->corr);
+	b = utility(iq_balancer, iq_balancer->corr_plus);
+
+	mu = a.im - b.im;
+	mu = a.im / mu;
+	if (fabs(mu) < 1)
 	{
-		iq_balancer->phase += PhaseAlpha * (phase - iq_balancer->phase);
-
-		if (!iq_balancer->phase_failed)
+		iq_balancer->phase_step /= StepIncrement;
+		if (iq_balancer->phase_step < MinPhaseStep)
 		{
-			iq_balancer->phase_step *= StepIncrement;
-			if (fabsf(iq_balancer->phase_step) > MaximumPhaseStep)
-			{
-				iq_balancer->phase_step = MaximumPhaseStep * fsign(iq_balancer->phase_step);
-			}
+			iq_balancer->phase_step = MinPhaseStep;
 		}
-		iq_balancer->phase_failed = 0;
-		result = 1;
 	}
-	else
+	else if (fabs(mu) > 10)
 	{
-		if (iq_balancer->phase_failed)
-		{
-			iq_balancer->phase_step *= -StepDecrement;
-			iq_balancer->phase_failed = 0;
-		}
-		else
-		{
-			iq_balancer->phase_failed = 1;
-		}
-
-		iq_balancer->phase_step *= StepDecrement;
-		if (fabsf(iq_balancer->phase_step) < MinimumPhaseStep)
+		iq_balancer->phase_step *= StepIncrement;
+		if (iq_balancer->phase_step > MaxPhaseStep)
 		{
-			iq_balancer->phase_step = MinimumPhaseStep * fsign(iq_balancer->phase_step);
+			iq_balancer->phase_step = MaxPhaseStep;
 		}
 	}
-
-	float amplitude = iq_balancer->amplitude + iq_balancer->amplitude_step;
-	if (amplitude > MaxAmplitudeCorrection)
-	{
-		amplitude = MaxAmplitudeCorrection;
-	}
-	else if (amplitude < -MaxAmplitudeCorrection)
-	{
-		amplitude = -MaxAmplitudeCorrection;
-	}
-
-	candidate_utility = utility(iq_balancer, iq, length, old_phase, amplitude);
-
-	if (candidate_utility < u)
+	if (mu < -20.f)
+		mu = -20.f;
+	else if (mu > 20.f)
+		mu = 20.f;
+	phase = iq_balancer->phase + iq_balancer->phase_step * mu;
+
+	mu = a.re - b.re;
+	mu = a.re / mu;
+	if (fabs(mu) < 1)
 	{
-		iq_balancer->amplitude += AmplitudeAlpha * (amplitude - iq_balancer->amplitude);
-
-		if (!iq_balancer->amplitude_failed)
+		iq_balancer->amplitude_step /= StepIncrement;
+		if (iq_balancer->amplitude_step < MinAmplitudeStep)
 		{
-			iq_balancer->amplitude_step *= StepIncrement;
-			if (fabsf(iq_balancer->amplitude_step) > MaximumAmplitudeStep)
-			{
-				iq_balancer->amplitude_step = MaximumAmplitudeStep * fsign(iq_balancer->amplitude_step);
-			}
+			iq_balancer->amplitude_step = MinAmplitudeStep;
 		}
-		iq_balancer->amplitude_failed = 0;
-		result = 1;
 	}
-	else
+	else if (fabs(mu) > 10)
 	{
-		if (iq_balancer->amplitude_failed)
-		{
-			iq_balancer->amplitude_step *= -StepDecrement;
-			iq_balancer->amplitude_failed = 0;
-		}
-		else
+		iq_balancer->amplitude_step *= StepIncrement;
+		if (iq_balancer->amplitude_step > MaxAmplitudeStep)
 		{
-			iq_balancer->amplitude_failed = 1;
+			iq_balancer->amplitude_step = MaxAmplitudeStep;
 		}
-
-		iq_balancer->amplitude_step *= StepDecrement;
-		if (fabsf(iq_balancer->amplitude_step) < MinimumAmplitudeStep)
+	}
+	if (mu < -20.f)
+		mu = -20.f;
+	else if (mu > 20.f)
+		mu = 20.f;
+	amplitude = iq_balancer->amplitude + iq_balancer->amplitude_step * mu;
+	
+	if (iq_balancer->initial_skip == 5)
+		iq_balancer->no_of_raw = 1;
+
+	if (iq_balancer->initial_skip >= 5)
+	{
+		iq_balancer->raw_amplitudes[iq_balancer->raw_ptr] = amplitude;
+		iq_balancer->raw_phases[iq_balancer->raw_ptr] = phase;
+		i = iq_balancer->raw_ptr;
+		for (j = 1; j <= iq_balancer->no_of_raw; j++)
 		{
-			iq_balancer->amplitude_step = MinimumAmplitudeStep * fsign(iq_balancer->amplitude_step);
+			phase += iq_balancer->raw_phases[i];
+			amplitude += iq_balancer->raw_amplitudes[i];
+			i = (i + MaxLookback - 1) & (MaxLookback - 1);
 		}
+		phase /= (iq_balancer->no_of_raw + 1);
+		amplitude /= (iq_balancer->no_of_raw + 1);
+		iq_balancer->raw_ptr = (iq_balancer->raw_ptr + 1) & (MaxLookback - 1);
 	}
 
-	return result;
+	iq_balancer->phase = phase;
+	iq_balancer->amplitude = amplitude;
 }
 
-static void adjust_phase_amplitude(iq_balancer_t *iq_balancer, airspyhf_complex_float_t* iq, int length)
+static void adjust_phase_amplitude(struct iq_balancer_t *iq_balancer, complex_t* iq, int length)
 {
 	int i;
 	float scale = 1.0f / (length - 1);
@@ -374,46 +404,54 @@ static void adjust_phase_amplitude(iq_balancer_t *iq_balancer, airspyhf_complex_
 	iq_balancer->last_amplitude = iq_balancer->amplitude;
 }
 
-void iq_balancer_process(iq_balancer_t *iq_balancer, airspyhf_complex_float_t* iq, int length)
+void ADDCALL  iq_balancer_process(struct iq_balancer_t *iq_balancer, complex_t* iq, int length)
 {
 	cancel_dc(iq_balancer, iq, length);
-
-	int n = 0;
-	while (estimate_imbalance(iq_balancer, iq, length) && n < MaxTries)
+	if (++iq_balancer->estimation_count >  BuffersToSkip)
 	{
-		n++;
+		estimate_imbalance(iq_balancer, iq, length);
+		iq_balancer->estimation_count = 0;
 	}
-
 	adjust_phase_amplitude(iq_balancer, iq, length);
 }
 
-void iq_balancer_set_optimal_point(iq_balancer_t *iq_balancer, float w)
+void ADDCALL iq_balancer_set_optimal_point(struct iq_balancer_t *iq_balancer, float w)
 {
-	if (w < 0)
+	if (w < 0.0f)
 	{
 		w = -w;
 	}
+	
 	if (w > 0.5f)
 	{
-		w = 0.5;
+		w = 0.5f;
 	}
 
-	iq_balancer->optimal_bin = (int) (FFTBins * w);
+	iq_balancer->optimal_bin = (int) floor(FFTBins * (0.5 + w));
+	iq_balancer->initial_skip = 0;
+}
+
+struct iq_balancer_t * ADDCALL iq_balancer_create()
+{
+	struct iq_balancer_t *instance = (struct iq_balancer_t *) malloc(sizeof(struct iq_balancer_t));
+	memset(instance, 0, sizeof(struct iq_balancer_t));
+
+	instance->corr = (complex_t *) malloc(FFTBins * sizeof(complex_t));
+	instance->corr_plus = (complex_t *) malloc(FFTBins * sizeof(complex_t));
+	instance->boost = (float *) malloc(FFTBins * sizeof(float));
+
+	instance->phase_step = InitialPhaseStep;
+	instance->amplitude_step = InitialAmplitudeStep;
+
+	__init_library();
+
+	return instance;
 }
 
-void iq_balancer_init(iq_balancer_t *iq_balancer)
+void ADDCALL iq_balancer_destroy(struct iq_balancer_t *iq_balancer)
 {
-	iq_balancer->iavg = 0.0f;
-	iq_balancer->qavg = 0.0f;
-	iq_balancer->phase = 0.0f;
-	iq_balancer->last_phase = 0.0f;
-	iq_balancer->phase_step = MinimumPhaseStep;
-	iq_balancer->phase_failed = 0;
-	iq_balancer->amplitude = 0.0f;
-	iq_balancer->last_amplitude = 0.0f;
-	iq_balancer->amplitude_step = MinimumAmplitudeStep;
-	iq_balancer->amplitude_failed = 0;
-	iq_balancer->optimal_bin = 0;
-
-	__init_window();
+	free(iq_balancer->corr);
+	free(iq_balancer->corr_plus);
+	free(iq_balancer->boost);
+	free(iq_balancer);
 }
diff --git a/libairspyhf/src/iqbalancer.h b/libairspyhf/src/iqbalancer.h
index 4c040cf..cf3c775 100644
--- a/libairspyhf/src/iqbalancer.h
+++ b/libairspyhf/src/iqbalancer.h
@@ -1,5 +1,6 @@
 /*
 Copyright (c) 2016-2018, Youssef Touil <youssef@airspy.com>
+Copyright (c) 2018, Leif Asbrink <leif@sm5bsz.com>
 
 All rights reserved.
 
@@ -24,44 +25,37 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 
 #include "airspyhf.h"
 
-#define FFTBins (8 * 1024)
-#define FFTIntegration 2
-#define BinsToSkip 30
-#define BinsToOptimize 20
-#define BoostFactor 10000.0f
-#define MaxTries 5
-#define MaximumPhaseStep 1e-4f
-#define MinimumPhaseStep 1e-7f
-#define MaximumAmplitudeStep 1e-3f
-#define MinimumAmplitudeStep 1e-6f
+#define FFTBins (4 * 1024)
+#define BoostFactor 10000.0
+#define BinsToSkip (FFTBins/22)
+#define BinsToOptimize (FFTBins/50)
+#define MaxLookback 8
+#define MaxPhaseStep 1e-3f
+#define MinPhaseStep 1e-8f
+#define InitialPhaseStep MaxPhaseStep
+#define MaxAmplitudeStep 1e-2f
+#define MinAmplitudeStep 1e-7f
+#define InitialAmplitudeStep MaxAmplitudeStep
 #define StepIncrement 2.0f
-#define StepDecrement (1.0f / StepIncrement)
-#define MaxPhaseCorrection 0.2f
-#define MaxAmplitudeCorrection 0.3f
-#define PhaseAlpha 0.025f
-#define AmplitudeAlpha 0.025f
 #define DcTimeConst 5e-5f
 
-typedef struct _iq_balancer_t
-{
-	float phase;
-	float last_phase;
-	float phase_step;
-	uint8_t phase_failed;
-
-	float amplitude;
-	float last_amplitude;
-	float amplitude_step;
-	uint8_t amplitude_failed;
+#if defined(__arm__) && !defined(__force_hiq__)
+#define BuffersToSkip 4
+#define FFTIntegration 2
+#define CorrelationIntegration 4
+#else
+#define BuffersToSkip 1
+#define FFTIntegration 4
+#define CorrelationIntegration 8
+#endif
 
-	float iavg;
-	float qavg;
+struct iq_balancer_t;
 
-	int optimal_bin;
-} iq_balancer_t;
+typedef airspyhf_complex_float_t complex_t;
 
-void iq_balancer_init(iq_balancer_t *iq_balancer);
-void iq_balancer_set_optimal_point(iq_balancer_t *iq_balancer, float w);
-void iq_balancer_process(iq_balancer_t *iq_balancer, airspyhf_complex_float_t* iq, int length);
+ADDAPI struct iq_balancer_t * ADDCALL iq_balancer_create();
+ADDAPI void ADDCALL iq_balancer_set_optimal_point(struct iq_balancer_t *iq_balancer, float w);
+ADDAPI void ADDCALL iq_balancer_process(struct iq_balancer_t *iq_balancer, complex_t* iq, int length);
+ADDAPI void ADDCALL iq_balancer_destroy(struct iq_balancer_t *iq_balancer);
 
 #endif
-- 
2.11.0

