IQ correction with phase imbalance: floating point implementation

sdrbase/dsp/dspdevicesourceengine.cpp

@@ -23,6 +23,7 @@
 #include <stdio.h>
 #include <QDebug>
 #include "dsp/dspcommands.h"
+#include "util/fixed.h"
 #include "samplesinkfifo.h"
 #include "threadedbasebandsamplesink.h"
 
@@ -182,14 +183,41 @@ void DSPDeviceSourceEngine::iqCorrections(SampleVector::iterator begin, SampleVe
 
         if (imbalanceCorrection)
         {
-            int32_t xi = it->m_real - (int32_t) m_iBeta;
+            // DC correction and conversion
-            int32_t xq = it->m_imag - (int32_t) m_qBeta;
+            float xi = (it->m_real - (int32_t) m_iBeta) / SDR_RX_SCALEF;
-            m_avgII(xi*xi);
+            float xq = (it->m_imag - (int32_t) m_qBeta) / SDR_RX_SCALEF;
-            m_avgIQ(xi*xq);
+
-            // TODO
+            // phase imbalance
+            m_avgII(xi*xi); // <I", I">
+            m_avgIQ(xi*xq); // <I", Q">
+
+
+            if (m_avgII.asDouble() != 0) {
+                m_avgPhi(m_avgIQ.asDouble()/m_avgII.asDouble());
+            }
+
+            float yi = xi - m_avgPhi.asDouble()*xq;
+            float yq = xq;
+
+            // amplitude I/Q imbalance
+            m_avgII2(yi*yi); // <I, I>
+            m_avgQQ2(yq*yq); // <Q, Q>
+
+            if (m_avgQQ2.asDouble() != 0) {
+                m_avgAmp(sqrt(m_avgII2.asDouble() / m_avgQQ2.asDouble()));
+            }
+
+            // final correction
+            float zi = yi;
+            float zq = m_avgAmp.asDouble() * yq;
+
+            // convert and store
+            it->m_real = zi * SDR_RX_SCALEF;
+            it->m_imag = zq * SDR_RX_SCALEF;
         }
         else
         {
+            // DC correction only
             it->m_real -= (int32_t) m_iBeta;
             it->m_imag -= (int32_t) m_qBeta;
         }
@@ -198,11 +226,6 @@ void DSPDeviceSourceEngine::iqCorrections(SampleVector::iterator begin, SampleVe
 
 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);
-
 	// sum and correct in one pass
 	for(SampleVector::iterator it = begin; it < end; it++)
 	{
@@ -210,15 +233,7 @@ void DSPDeviceSourceEngine::dcOffset(SampleVector::iterator begin, SampleVector:
 	    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;
 }
 
 void DSPDeviceSourceEngine::imbalance(SampleVector::iterator begin, SampleVector::iterator end)
@@ -291,13 +306,18 @@ void DSPDeviceSourceEngine::work()
 			// correct stuff
             if (m_dcOffsetCorrection)
             {
-				dcOffset(part1begin, part1end);
+                iqCorrections(part1begin, part1end, m_iqImbalanceCorrection);
             }
 
-			if (m_iqImbalanceCorrection)
+//			if (m_dcOffsetCorrection)
-			{
+//			{
-				imbalance(part1begin, part1end);
+//				dcOffset(part1begin, part1end);
-			}
+//			}
+//
+//			if (m_iqImbalanceCorrection)
+//			{
+//				imbalance(part1begin, part1end);
+//			}
 
 			// feed data to direct sinks
 			for (BasebandSampleSinks::const_iterator it = m_basebandSampleSinks.begin(); it != m_basebandSampleSinks.end(); ++it)
@@ -318,13 +338,18 @@ void DSPDeviceSourceEngine::work()
 			// correct stuff
             if (m_dcOffsetCorrection)
             {
-				dcOffset(part2begin, part2end);
+                iqCorrections(part2begin, part2end, m_iqImbalanceCorrection);
             }
 
-			if (m_iqImbalanceCorrection)
+//            if (m_dcOffsetCorrection)
-			{
+//			{
-				imbalance(part2begin, part2end);
+//				dcOffset(part2begin, part2end);
-			}
+//			}
+//
+//			if (m_iqImbalanceCorrection)
+//			{
+//				imbalance(part2begin, part2end);
+//			}
 
 			// feed data to direct sinks
 			for (BasebandSampleSinks::const_iterator it = m_basebandSampleSinks.begin(); it != m_basebandSampleSinks.end(); it++)

sdrbase/dsp/dspdevicesourceengine.h

@@ -104,8 +104,14 @@ private:
 	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<float, double, 128> m_avgII;
-    MovingAverageUtil<int64_t, int64_t, 1024> m_avgIQ;
+    MovingAverageUtil<float, double, 128> m_avgIQ;
+    MovingAverageUtil<float, double, 128> m_avgII2;
+    MovingAverageUtil<float, double, 128> m_avgQQ2;
+    MovingAverageUtil<double, double, 128> m_avgPhi;
+    MovingAverageUtil<double, double, 128> m_avgAmp;
+//    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;

sdrbase/util/fixed.h

@@ -12,6 +12,8 @@
 // Modified as fully templatized class with variable size and type internal      //
 // representation                                                                //
 //                                                                               //
+// sqrt requires even IntBits                                                    //
+//                                                                               //
 // This program is free software; you can redistribute it and/or modify          //
 // it under the terms of the GNU General Public License as published by          //
 // the Free Software Foundation as version 3 of the License, or                  //
@@ -479,9 +481,9 @@ Fixed<IntType, IntBits>& Fixed<IntType, IntBits>::operator/=(Fixed<IntType, IntB
 template<typename IntType, uint32_t IntBits>
 Fixed<IntType, IntBits> Fixed<IntType, IntBits>::sqrt() const
 {
-    unsigned const max_shift = 62;
+    unsigned int const max_shift = 62;
     uint64_t a_squared = 1LL << max_shift;
-    unsigned b_shift = (max_shift + FixedTraits<IntBits>::fixed_resolution_shift) / 2;
+    unsigned int b_shift = (max_shift + FixedTraits<IntBits>::fixed_resolution_shift) / 2;
     uint64_t a = 1LL << b_shift;
 
     uint64_t x = m_nVal;

sdrbase/util/fixedtraits.cpp

@@ -282,6 +282,138 @@ const int64_t  FixedTraits<16>::arctantab[32] = {
     0LL
 };
 
+// 1.0 = 2^23 internal representation
+
+const int64_t  FixedTraits<23>::log_two_power_n_reversed[40] = {
+    232581599LL,
+    226767059LL,
+    220952519LL,
+    215137979LL,
+    209323439LL,
+    203508899LL,
+    197694359LL,
+    191879819LL,
+    186065279LL,
+    180250740LL,
+    174436200LL,
+    168621660LL,
+    162807120LL,
+    156992580LL,
+    151178040LL,
+    145363500LL,
+    139548960LL,
+    133734420LL,
+    127919880LL,
+    122105340LL,
+    116290800LL,
+    110476260LL,
+    104661720LL,
+    98847180LL,
+    93032640LL,
+    87218100LL,
+    81403560LL,
+    75589020LL,
+    69774480LL,
+    63959940LL,
+    58145400LL,
+    52330860LL,
+    46516320LL,
+    40701780LL,
+    34887240LL,
+    29072700LL,
+    23258160LL,
+    17443620LL,
+    11629080LL,
+    5814540LL
+};
+
+const int64_t  FixedTraits<23>::log_one_plus_two_power_minus_n[23] = {
+    3401288LL,
+    1871864LL,
+    988036LL,
+    508556LL,
+    258131LL,
+    130059LL,
+    65281LL,
+    32704LL,
+    16368LL,
+    8188LL,
+    4095LL,
+    2048LL,
+    1024LL,
+    512LL,
+    256LL,
+    128LL,
+    64LL,
+    32LL,
+    16LL,
+    8LL,
+    4LL,
+    2LL,
+    1LL
+};
+
+const int64_t  FixedTraits<23>::log_one_over_one_minus_two_power_minus_n[23] = {
+    5814540LL,
+    2413252LL,
+    1120143LL,
+    541388LL,
+    266327LL,
+    132107LL,
+    65793LL,
+    32832LL,
+    16400LL,
+    8196LL,
+    4097LL,
+    2048LL,
+    1024LL,
+    512LL,
+    256LL,
+    128LL,
+    64LL,
+    32LL,
+    16LL,
+    8LL,
+    4LL,
+    2LL,
+    1LL
+};
+
+const int64_t  FixedTraits<23>::arctantab[32] = {
+    9287436LL,
+    6588397LL,
+    3889358LL,
+    2055029LL,
+    1043165LL,
+    523606LL,
+    262058LL,
+    131061LL,
+    65534LL,
+    32767LL,
+    16383LL,
+    8192LL,
+    4096LL,
+    2048LL,
+    1024LL,
+    512LL,
+    256LL,
+    128LL,
+    64LL,
+    32LL,
+    16LL,
+    8LL,
+    4LL,
+    2LL,
+    1LL,
+    0LL,
+    0LL,
+    0LL,
+    0LL,
+    0LL,
+    0LL,
+    0LL
+};
+
 // 1.0 = 2^24 internal representation
 
 const int64_t  FixedTraits<24>::log_two_power_n_reversed[39] = {

sdrbase/util/fixedtraits.h

@@ -57,6 +57,22 @@ struct FixedTraits<16>
     static const int64_t  arctantab[32];
 };
 
+template<>
+struct FixedTraits<23>
+{
+    static const uint32_t fixed_resolution_shift = 23;
+    static const int64_t  fixed_resolution       = 1LL << fixed_resolution_shift;
+    static const int32_t  max_power              = 63 - fixed_resolution_shift;
+    static const int64_t  internal_pi            = 26353589;
+    static const int64_t  internal_two_pi        = 52707179;
+    static const int64_t  internal_half_pi       = 13176795;
+    static const int64_t  internal_quarter_pi    = 6588397;
+    static const int64_t  log_two_power_n_reversed[40]; // 40 = 63 - 23
+    static const int64_t  log_one_plus_two_power_minus_n[23];
+    static const int64_t  log_one_over_one_minus_two_power_minus_n[23];
+    static const int64_t  arctantab[32];
+};
+
 template<>
 struct FixedTraits<24>
 {
@@ -67,7 +83,7 @@ struct FixedTraits<24>
     static const int64_t  internal_two_pi        = 105414357;
     static const int64_t  internal_half_pi       = 26353589;
     static const int64_t  internal_quarter_pi    = 13176795;
-    static const int64_t  log_two_power_n_reversed[39]; // 39 = 63 - 16
+    static const int64_t  log_two_power_n_reversed[39]; // 39 = 63 - 24
     static const int64_t  log_one_plus_two_power_minus_n[24];
     static const int64_t  log_one_over_one_minus_two_power_minus_n[24];
     static const int64_t  arctantab[32];