diff --git a/sdrbase/dsp/dspdevicesourceengine.cpp b/sdrbase/dsp/dspdevicesourceengine.cpp index cc18698df..fe12e69e3 100644 --- a/sdrbase/dsp/dspdevicesourceengine.cpp +++ b/sdrbase/dsp/dspdevicesourceengine.cpp @@ -23,6 +23,7 @@ #include #include #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; - int32_t xq = it->m_imag - (int32_t) m_qBeta; - m_avgII(xi*xi); - m_avgIQ(xi*xq); - // TODO + // DC correction and conversion + float xi = (it->m_real - (int32_t) m_iBeta) / SDR_RX_SCALEF; + float xq = (it->m_imag - (int32_t) m_qBeta) / SDR_RX_SCALEF; + + // phase imbalance + m_avgII(xi*xi); // + m_avgIQ(xi*xq); // + + + 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); // + m_avgQQ2(yq*yq); // + + 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) @@ -289,15 +304,20 @@ void DSPDeviceSourceEngine::work() if (part1begin != part1end) { // correct stuff - if (m_dcOffsetCorrection) - { - dcOffset(part1begin, part1end); - } + if (m_dcOffsetCorrection) + { + iqCorrections(part1begin, part1end, m_iqImbalanceCorrection); + } - if (m_iqImbalanceCorrection) - { - imbalance(part1begin, part1end); - } +// if (m_dcOffsetCorrection) +// { +// 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) @@ -316,15 +336,20 @@ void DSPDeviceSourceEngine::work() if(part2begin != part2end) { // correct stuff - if (m_dcOffsetCorrection) - { - dcOffset(part2begin, part2end); - } + if (m_dcOffsetCorrection) + { + iqCorrections(part2begin, part2end, m_iqImbalanceCorrection); + } - if (m_iqImbalanceCorrection) - { - imbalance(part2begin, part2end); - } +// if (m_dcOffsetCorrection) +// { +// 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++) diff --git a/sdrbase/dsp/dspdevicesourceengine.h b/sdrbase/dsp/dspdevicesourceengine.h index e6df47353..aca574b91 100644 --- a/sdrbase/dsp/dspdevicesourceengine.h +++ b/sdrbase/dsp/dspdevicesourceengine.h @@ -104,8 +104,14 @@ private: double m_iOffset, m_qOffset; MovingAverageUtil m_iBeta; MovingAverageUtil m_qBeta; - MovingAverageUtil m_avgII; - MovingAverageUtil m_avgIQ; + MovingAverageUtil m_avgII; + MovingAverageUtil m_avgIQ; + MovingAverageUtil m_avgII2; + MovingAverageUtil m_avgQQ2; + MovingAverageUtil m_avgPhi; + MovingAverageUtil m_avgAmp; +// MovingAverageUtil m_avgII; +// MovingAverageUtil m_avgIQ; qint32 m_iRange; qint32 m_qRange; qint32 m_imbalance; diff --git a/sdrbase/util/fixed.h b/sdrbase/util/fixed.h index 96193212b..0e9d8b395 100644 --- a/sdrbase/util/fixed.h +++ b/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& Fixed::operator/=(Fixed Fixed Fixed::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::fixed_resolution_shift) / 2; + unsigned int b_shift = (max_shift + FixedTraits::fixed_resolution_shift) / 2; uint64_t a = 1LL << b_shift; uint64_t x = m_nVal; diff --git a/sdrbase/util/fixedtraits.cpp b/sdrbase/util/fixedtraits.cpp index 095e58882..df8db322d 100644 --- a/sdrbase/util/fixedtraits.cpp +++ b/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] = { diff --git a/sdrbase/util/fixedtraits.h b/sdrbase/util/fixedtraits.h index db53bb2cf..7b7167031 100644 --- a/sdrbase/util/fixedtraits.h +++ b/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];