From 9f483786774b54d44de3abd6132936c6a2830bec Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Fri, 18 May 2018 19:03:54 +0200
Subject: [PATCH] Channel analyzer NG: return of the lock status indicator and
 PLL frequency shift for PSK modulated signals

---
 plugins/channelrx/chanalyzerng/chanalyzerng.h |  1 +
 .../chanalyzerng/chanalyzernggui.cpp          | 12 +++++-
 sdrbase/dsp/phaselockcomplex.cpp              | 43 +++++++++++++++++--
 sdrbase/dsp/phaselockcomplex.h                |  6 ++-
 sdrbase/util/movingaverage.h                  |  5 +++
 5 files changed, 62 insertions(+), 5 deletions(-)

diff --git a/plugins/channelrx/chanalyzerng/chanalyzerng.h b/plugins/channelrx/chanalyzerng/chanalyzerng.h
index 44670ee57..770fdbd96 100644
--- a/plugins/channelrx/chanalyzerng/chanalyzerng.h
+++ b/plugins/channelrx/chanalyzerng/chanalyzerng.h
@@ -163,6 +163,7 @@ 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.getFreq(); }
 	Real getPllDeltaPhase() const { return m_pll.getDeltaPhi(); }
     Real getPllPhase() const { return m_pll.getPhiHat(); }
 
diff --git a/plugins/channelrx/chanalyzerng/chanalyzernggui.cpp b/plugins/channelrx/chanalyzerng/chanalyzernggui.cpp
index 4d63a3b34..586f22612 100644
--- a/plugins/channelrx/chanalyzerng/chanalyzernggui.cpp
+++ b/plugins/channelrx/chanalyzerng/chanalyzernggui.cpp
@@ -237,6 +237,12 @@ void ChannelAnalyzerNGGUI::tick()
 	} else {
 	    ui->pll->setStyleSheet("QToolButton { background:rgb(79,79,79); }");
 	}
+
+	if (ui->pll->isChecked()) 
+	{
+		int freq = (m_channelAnalyzer->getPllFrequency() * m_channelAnalyzer->getChannelSampleRate()) / (2.0*M_PI);
+		ui->pll->setToolTip(tr("PLL lock. Freq = %1 Hz").arg(freq));
+	}
 }
 
 void ChannelAnalyzerNGGUI::on_channelSampleRate_changed(quint64 value)
@@ -251,8 +257,12 @@ void ChannelAnalyzerNGGUI::on_channelSampleRate_changed(quint64 value)
     }
 }
 
-void ChannelAnalyzerNGGUI::on_pll_toggled(bool checked __attribute__((unused)))
+void ChannelAnalyzerNGGUI::on_pll_toggled(bool checked)
 {
+	if (!checked) {
+		ui->pll->setToolTip(tr("PLL lock"));
+	}
+
     applySettings();
 }
 
diff --git a/sdrbase/dsp/phaselockcomplex.cpp b/sdrbase/dsp/phaselockcomplex.cpp
index 1eb0b1edc..87b8b6158 100644
--- a/sdrbase/dsp/phaselockcomplex.cpp
+++ b/sdrbase/dsp/phaselockcomplex.cpp
@@ -43,8 +43,10 @@ PhaseLockComplex::PhaseLockComplex() :
     m_freq(0.0),
     m_freqPrev(0.0),
     m_lockCount(0),
+    m_lockFreq(0.026f),
     m_pskOrder(1),
-    m_lockTime(480)
+    m_lockTime(480),
+    m_lockTimeCount(0)
 {
 }
 
@@ -87,6 +89,7 @@ void PhaseLockComplex::setPskOrder(unsigned int order)
 void PhaseLockComplex::setSampleRate(unsigned int sampleRate)
 {
     m_lockTime = sampleRate / 100; // 10ms for order 1
+    m_lockFreq = (2.0*M_PI*5.0) / sampleRate; // +/- 5 Hz frequency swing
     reset();
 }
 
@@ -108,6 +111,7 @@ void PhaseLockComplex::reset()
     m_freq = 0.0f;
     m_freqPrev = 0.0f;
     m_lockCount = 0;
+    m_lockTimeCount = 0;
 }
 
 void PhaseLockComplex::feed(float re, float im)
@@ -151,8 +155,41 @@ void PhaseLockComplex::feed(float re, float im)
         m_phiHat += 2.0*M_PI;
     }
 
-    // lock estimation
-    if (m_pskOrder <= 1)
+    // lock and frequency estimation
+    if (m_pskOrder > 1)
+    {
+        float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
+        m_freq = 0.001*dPhi + 0.999*m_freqPrev;
+
+        if (m_lockTimeCount < m_lockTime-1)
+        {
+            m_lockTimeCount++;
+        }
+        else
+        {
+            float dF = m_freq - m_freqTest;
+
+            if ((dF > -m_lockFreq) && (dF < m_lockFreq)) 
+            {
+                if (m_lockCount < 20) {
+                    m_lockCount++;
+                }
+            } 
+            else 
+            {
+                if (m_lockCount > 0) {
+                    m_lockCount--;
+                }
+            }
+
+            m_freqTest = m_freq;
+            m_lockTimeCount = 0;
+        }
+
+        m_freqPrev = m_freq;
+        m_phiHatPrev = m_phiHat;        
+    }
+    else
     {
         m_freq = (m_phiHat - m_phiHatPrev) / (2.0*M_PI);
 
diff --git a/sdrbase/dsp/phaselockcomplex.h b/sdrbase/dsp/phaselockcomplex.h
index baf4af9b7..cd52e72d0 100644
--- a/sdrbase/dsp/phaselockcomplex.h
+++ b/sdrbase/dsp/phaselockcomplex.h
@@ -49,7 +49,8 @@ public:
     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_pskOrder > 1 ? false : m_lockCount > m_lockTime-2; }
+    bool locked() const { return m_pskOrder > 1 ? m_lockCount > 16 : m_lockCount > m_lockTime-2; }
+    float getFreq() const { return m_freq; }
     float getDeltaPhi() const { return m_deltaPhi; }
     float getPhiHat() const { return m_phiHat; }
 
@@ -75,9 +76,12 @@ private:
     float m_yIm;
     float m_freq;
     float m_freqPrev;
+    float m_freqTest;
     int m_lockCount;
+    float m_lockFreq;
     unsigned int m_pskOrder;
     int m_lockTime;
+    int m_lockTimeCount;
 };
 
 #endif /* SDRBASE_DSP_PHASELOCKCOMPLEX_H_ */
diff --git a/sdrbase/util/movingaverage.h b/sdrbase/util/movingaverage.h
index 0461fbb09..74ac1e94e 100644
--- a/sdrbase/util/movingaverage.h
+++ b/sdrbase/util/movingaverage.h
@@ -90,6 +90,11 @@ class MovingAverageUtilVar
         m_samples.resize(size);
     }
 
+    unsigned int size() const
+    {
+        return m_samples.size();
+    }
+
     void operator()(T sample)
     {
         if (m_num_samples < m_samples.size()) // fill up