From be049374adc6c4e07e9f3891168c333c1db93915 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Thu, 1 Feb 2018 02:45:55 +0100
Subject: [PATCH] Improved DC offset correction

---
 sdrbase/dsp/dspdevicesourceengine.cpp | 49 +++++++++++++++++++++------
 sdrbase/dsp/dspdevicesourceengine.h   |  6 ++++
 sdrbase/util/movingaverage.h          |  1 +
 3 files changed, 45 insertions(+), 11 deletions(-)

diff --git a/sdrbase/dsp/dspdevicesourceengine.cpp b/sdrbase/dsp/dspdevicesourceengine.cpp
index 4899dce82..cc18698df 100644
--- a/sdrbase/dsp/dspdevicesourceengine.cpp
+++ b/sdrbase/dsp/dspdevicesourceengine.cpp
@@ -173,25 +173,52 @@ QString DSPDeviceSourceEngine::sourceDeviceDescription()
 	return cmd.getDeviceDescription();
 }
 
+void DSPDeviceSourceEngine::iqCorrections(SampleVector::iterator begin, SampleVector::iterator end, bool imbalanceCorrection)
+{
+    for(SampleVector::iterator it = begin; it < end; it++)
+    {
+        m_iBeta(it->real());
+        m_qBeta(it->imag());
+
+        if (imbalanceCorrection)
+        {
+            int32_t xi = it->m_real - (int32_t) m_iBeta;
+            int32_t xq = it->m_imag - (int32_t) m_qBeta;
+            m_avgII(xi*xi);
+            m_avgIQ(xi*xq);
+            // TODO
+        }
+        else
+        {
+            it->m_real -= (int32_t) m_iBeta;
+            it->m_imag -= (int32_t) m_qBeta;
+        }
+    }
+}
+
 void DSPDeviceSourceEngine::dcOffset(SampleVector::iterator begin, SampleVector::iterator end)
 {
-	double count;
-	int io = 0;
-	int qo = 0;
-	Sample corr((FixReal)m_iOffset, (FixReal)m_qOffset);
+//	double count;
+//	int io = 0;
+//	int qo = 0;
+//	Sample corr((FixReal)m_iOffset, (FixReal)m_qOffset);
 
 	// sum and correct in one pass
 	for(SampleVector::iterator it = begin; it < end; it++)
 	{
-		io += it->real();
-		qo += it->imag();
-		*it -= corr;
+	    m_iBeta(it->real());
+	    m_qBeta(it->imag());
+	    it->m_real -= (int32_t) m_iBeta;
+	    it->m_imag -= (int32_t) m_qBeta;
+//		io += it->real();
+//		qo += it->imag();
+//		*it -= corr;
 	}
 
-	// moving average
-	count = end - begin;
-	m_iOffset = (15.0 * m_iOffset + (double)io / count) / 16.0;
-	m_qOffset = (15.0 * m_qOffset + (double)qo / count) / 16.0;
+//	// moving average
+//	count = end - begin;
+//	m_iOffset = (15.0 * m_iOffset + (double)io / count) / 16.0;
+//	m_qOffset = (15.0 * m_qOffset + (double)qo / count) / 16.0;
 }
 
 void DSPDeviceSourceEngine::imbalance(SampleVector::iterator begin, SampleVector::iterator end)
diff --git a/sdrbase/dsp/dspdevicesourceengine.h b/sdrbase/dsp/dspdevicesourceengine.h
index 23561dd92..e6df47353 100644
--- a/sdrbase/dsp/dspdevicesourceengine.h
+++ b/sdrbase/dsp/dspdevicesourceengine.h
@@ -27,6 +27,7 @@
 #include "util/messagequeue.h"
 #include "util/syncmessenger.h"
 #include "util/export.h"
+#include "util/movingaverage.h"
 
 class DeviceSampleSource;
 class BasebandSampleSink;
@@ -101,12 +102,17 @@ private:
 	bool m_dcOffsetCorrection;
 	bool m_iqImbalanceCorrection;
 	double m_iOffset, m_qOffset;
+	MovingAverageUtil<int32_t, int64_t, 1024> m_iBeta;
+    MovingAverageUtil<int32_t, int64_t, 1024> m_qBeta;
+    MovingAverageUtil<int64_t, int64_t, 1024> m_avgII;
+    MovingAverageUtil<int64_t, int64_t, 1024> m_avgIQ;
 	qint32 m_iRange;
 	qint32 m_qRange;
 	qint32 m_imbalance;
 
 	void run();
 
+	void iqCorrections(SampleVector::iterator begin, SampleVector::iterator end, bool imbalanceCorrection);
 	void dcOffset(SampleVector::iterator begin, SampleVector::iterator end);
 	void imbalance(SampleVector::iterator begin, SampleVector::iterator end);
 	void work(); //!< transfer samples from source to sinks if in running state
diff --git a/sdrbase/util/movingaverage.h b/sdrbase/util/movingaverage.h
index 919a29b38..664bb57bc 100644
--- a/sdrbase/util/movingaverage.h
+++ b/sdrbase/util/movingaverage.h
@@ -49,6 +49,7 @@ class MovingAverageUtil
 
     operator double() const { return m_num_samples > 0 ? m_total / std::min(m_num_samples, N) : 0.0d; }
     operator float() const { return m_num_samples > 0 ? m_total / std::min(m_num_samples, N) : 0.0f; }
+    operator T() const { return  m_total / N; }
 
   private:
     T m_samples[N];