New PLL: heuristics to find locked state

plugins/channelrx/chanalyzerng/chanalyzerng.cpp

@@ -197,8 +197,6 @@ bool ChannelAnalyzerNG::handleMessage(const Message& cmd)
 	}
 }
 
-
-
 void ChannelAnalyzerNG::apply(bool force)
 {
     if ((m_running.m_frequency != m_config.m_frequency) ||
@@ -253,6 +251,12 @@ void ChannelAnalyzerNG::apply(bool force)
         m_settingsMutex.unlock();
     }
 
+    if ((m_running.m_channelSampleRate != m_config.m_channelSampleRate) ||
+        (m_running.m_spanLog2 != m_config.m_spanLog2) || force)
+    {
+        m_pll.setSampleRate(m_running.m_channelSampleRate / (1<<m_running.m_spanLog2));
+    }
+
     if (m_running.m_pll != m_config.m_pll || force)
     {
         if (m_config.m_pll) {

plugins/channelrx/chanalyzerng/chanalyzerng.h

@@ -155,7 +155,6 @@ public:
     int getChannelSampleRate() const { return m_running.m_channelSampleRate; }
 	double getMagSq() const { return m_magsq; }
 	bool isPllLocked() const { return m_running.m_pll && m_pll.locked(); }
-	Real getPllFrequency() const { return m_pll.getFrequency(); }
 	Real getPllDeltaPhase() const { return m_pll.getDeltaPhi(); }
     Real getPllPhase() const { return m_pll.getPhiHat(); }
 
@@ -262,10 +261,10 @@ private:
                     m_pll.feed(re, im);
 
                     // Use -fPLL to mix (exchange PLL real and image in the complex multiplication)
-                    // Real mixI = m_sum.real() * m_pll.getImag() - m_sum.imag() * m_pll.getReal();
+                    Real mixI = m_sum.real() * m_pll.getImag() - m_sum.imag() * m_pll.getReal();
-                    // Real mixQ = m_sum.real() * m_pll.getReal() + m_sum.imag() * m_pll.getImag();
+                    Real mixQ = m_sum.real() * m_pll.getReal() + m_sum.imag() * m_pll.getImag();
-                    Real mixI = m_pll.getReal() * SDR_RX_SCALED;
+//                    Real mixI = m_pll.getReal() * SDR_RX_SCALED;
-                    Real mixQ = m_pll.getImag() * SDR_RX_SCALED;
+//                    Real mixQ = m_pll.getImag() * SDR_RX_SCALED;
 
                     if (m_running.m_ssb & !m_usb)
                     { // invert spectrum for LSB

plugins/channelrx/chanalyzerng/chanalyzernggui.cpp

@@ -237,15 +237,6 @@ void ChannelAnalyzerNGGUI::tick()
 	} else {
 	    ui->pll->setStyleSheet("QToolButton { background:rgb(79,79,79); }");
 	}
-
-	if (ui->pll->isChecked())
-	{
-	    int fHz = round(m_channelAnalyzer->getPllFrequency()*m_rate);
-	    ui->pll->setToolTip(tr("PLL lock (f:%1 Hz e:%2 rad p:%3 rad)")
-	            .arg(fHz)
-	            .arg(m_channelAnalyzer->getPllDeltaPhase())
-	            .arg(m_channelAnalyzer->getPllPhase()));
-	}
 }
 
 void ChannelAnalyzerNGGUI::on_channelSampleRate_changed(quint64 value)
@@ -262,10 +253,6 @@ void ChannelAnalyzerNGGUI::on_channelSampleRate_changed(quint64 value)
 
 void ChannelAnalyzerNGGUI::on_pll_toggled(bool checked)
 {
-    if (!checked && m_usePll) {
-        ui->pll->setToolTip("PLL lock");
-    }
-
     m_usePll = checked;
     applySettings();
 }

plugins/channelrx/demodam/amdemod.cpp

@@ -82,7 +82,7 @@ AMDemod::AMDemod(DeviceSourceAPI *deviceAPI) :
     m_deviceAPI->addThreadedSink(m_threadedChannelizer);
     m_deviceAPI->addChannelAPI(this);
 
-    m_pllFilt.create(101, m_audioSampleRate, 500.0);
+    m_pllFilt.create(101, m_audioSampleRate, 200.0);
     m_pll.computeCoefficients(0.05, 0.707, 1000);
     m_syncAMBuffIndex = 0;
 }
@@ -374,7 +374,7 @@ void AMDemod::applyAudioSampleRate(int sampleRate)
     m_audioFifo.setSize(sampleRate);
     m_squelchDelayLine.resize(sampleRate/5);
     DSBFilter->create_dsb_filter((2.0f * m_settings.m_rfBandwidth) / (float) sampleRate);
-    m_pllFilt.create(101, sampleRate, 500.0);
+    m_pllFilt.create(101, sampleRate, 200.0);
 
     if (m_settings.m_pll) {
         m_volumeAGC.resizeNew(sampleRate, 0.003);
@@ -383,6 +383,7 @@ void AMDemod::applyAudioSampleRate(int sampleRate)
     }
 
     m_syncAMAGC.resize(sampleRate/4, sampleRate/8, 0.1);
+    m_pll.setSampleRate(sampleRate);
 
     m_settingsMutex.unlock();
     m_audioSampleRate = sampleRate;

sdrbase/dsp/phaselockcomplex.cpp

@@ -44,9 +44,14 @@ PhaseLockComplex::PhaseLockComplex() :
     m_yRe(1.0),
     m_yIm(0.0),
     m_freq(0.0),
+    m_freqPrev(0.0),
     m_lock(0.0),
     m_lockCount(0),
-    m_pskOrder(1)
+    m_pskOrder(1),
+    m_lockTime1(480),
+    m_lockTime(2400),
+    m_lockTimef(2400.0f),
+    m_lockThreshold(4.8f)
 {
 }
 
@@ -83,6 +88,16 @@ void PhaseLockComplex::computeCoefficients(Real wn, Real zeta, Real K)
 void PhaseLockComplex::setPskOrder(unsigned int order)
 {
     m_pskOrder = order > 0 ? order : 1;
+    reset();
+}
+
+void PhaseLockComplex::setSampleRate(unsigned int sampleRate)
+{
+    m_lockTime1 = sampleRate / 100; // 10ms for order 1
+    m_lockTime = sampleRate / 20; // 50ms for order > 1
+    m_lockTimef = (float) m_lockTime;
+    m_lockThreshold = m_lockTime * 0.002f; // threshold of 0.002 taking division by lock time into account
+    reset();
 }
 
 void PhaseLockComplex::reset()
@@ -103,6 +118,7 @@ void PhaseLockComplex::reset()
     m_yRe = 1.0f;
     m_yIm = 0.0f;
     m_freq = 0.0f;
+    m_freqPrev = 0.0f;
     m_lock = 0.0f;
     m_lockCount = 0;
 }
@@ -148,40 +164,69 @@ void PhaseLockComplex::feed(float re, float im)
         m_phiHat += 2.0*M_PI;
     }
 
-    float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
+    // lock estimation
-    m_phiHatPrev = m_phiHat;
+    if (m_pskOrder > 1)
-
-    if (m_phiHatCount < 9)
     {
-        m_dPhiHatAccum += dPhi;
+        float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
+
+        if (m_phiHatCount < (m_lockTime-1))
+        {
+            m_dPhiHatAccum += dPhi; // re-accumulate phase for differential calculation
+            m_phiHatCount++;
+        }
+        else
+        {
+            float dPhi11 = (m_dPhiHatAccum - m_phiHat1); // optimized out division by lock time
+            float dPhi12 = (m_phiHat1 - m_phiHat2);
+            m_lock = dPhi11 - dPhi12; // second derivative of phase to get lock status
+
+            if ((m_lock > -m_lockThreshold) && (m_lock < m_lockThreshold)) // includes re-multiplication by lock time
+            {
+                if (m_lockCount < 20) { // [0..20]
+                    m_lockCount++;
+                }
+            }
+            else
+            {
+                if (m_lockCount > 0) {
+                    m_lockCount -= 2;
+                }
+            }
+
+            m_phiHat2 = m_phiHat1;
+            m_phiHat1 = m_dPhiHatAccum;
+            m_dPhiHatAccum = 0.0f;
+            m_phiHatCount = 0;
+        }
+
+        m_phiHatPrev = m_phiHat;
     }
     else
     {
-        float dPhi1 = (m_phiHat1 - m_dPhiHatAccum) / 10.0f;
+        m_freq = (m_phiHat - m_phiHatPrev) / (2.0*M_PI);
-        float dPhi1Prev = (m_phiHat2 - m_phiHat1) / 10.0f;
-        m_lock = dPhi1 - dPhi1Prev; // second derivative of phase
 
-        if ((m_lock > -0.01) && (m_lock < 0.01))
+        if (m_freq < -1.0f) {
+            m_freq += 2.0f;
+        } else if (m_freq > 1.0f) {
+            m_freq -= 2.0f;
+        }
+
+        float dFreq = m_freq - m_freqPrev;
+
+        if ((dFreq > -0.01) && (dFreq < 0.01))
         {
-            if (m_lockCount < 1000) {
+            if (m_lockCount < (m_lockTime1-1)) { // [0..479]
                 m_lockCount++;
             }
         }
         else
         {
-            if (m_lockCount > 0) {
+            m_lockCount = 0;
-                m_lockCount--;
-            }
         }
 
-        m_freq = dPhi1 / 2.0*M_PI; // first derivative of phase
+        m_phiHatPrev = m_phiHat;
-        m_phiHat2 = m_phiHat1; 
+        m_freqPrev = m_freq;
-        m_phiHat1 = m_dPhiHatAccum;
-        m_dPhiHatAccum = 0.0f;
-        m_phiHatCount = 0;
     }
-
-    m_dPhiHatAccum += dPhi;
 }
 
 float PhaseLockComplex::normalizeAngle(float angle)

sdrbase/dsp/phaselockcomplex.h

@@ -41,13 +41,14 @@ public:
      * \param order 0,1: no PSK (CW), 2: BPSK, 4: QPSK, 8: 8-PSK, ... use powers of two for real cases
      */
     void setPskOrder(unsigned int order);
+    /** Set sample rate information only for frequency and lock condition calculation */
+    void setSampleRate(unsigned int sampleRate);
     void reset();
     void feed(float re, float im);
     const std::complex<float>& getComplex() const { return m_y; }
     float getReal() const { return m_yRe; }
     float getImag() const { return m_yIm; }
-    bool locked() const { return m_lockCount > 500; }
+    bool locked() const { return m_lockCount > (m_pskOrder > 1 ? 15 : (m_lockTime1-2)); } // 6
-    float getFrequency() const { return m_freq; }
     float getDeltaPhi() const { return m_deltaPhi; }
     float getPhiHat() const { return m_phiHat; }
 
@@ -75,9 +76,14 @@ private:
     float m_yRe;
     float m_yIm;
     float m_freq;
+    float m_freqPrev;
     float m_lock;
     int m_lockCount;
     unsigned int m_pskOrder;
+    int m_lockTime1;
+    int m_lockTime;
+    float m_lockTimef;
+    float m_lockThreshold;
 };