From 48cac5385bd532fa6bb44331f99147a5163af4d2 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Sun, 20 May 2018 03:50:22 +0200
Subject: [PATCH] PLL lock indication fixes

---
 plugins/channelrx/demodam/amdemod.h      |  1 +
 plugins/channelrx/demodam/amdemodgui.cpp |  7 ++++++-
 sdrbase/dsp/phaselockcomplex.cpp         | 18 ++++++-----------
 sdrbase/dsp/phaselockcomplex.h           | 25 +++++++++++++++++++++++-
 4 files changed, 37 insertions(+), 14 deletions(-)

diff --git a/plugins/channelrx/demodam/amdemod.h b/plugins/channelrx/demodam/amdemod.h
index bd971b9ad..d8b292417 100644
--- a/plugins/channelrx/demodam/amdemod.h
+++ b/plugins/channelrx/demodam/amdemod.h
@@ -123,6 +123,7 @@ public:
 	double getMagSq() const { return m_magsq; }
 	bool getSquelchOpen() const { return m_squelchOpen; }
 	bool getPllLocked() const { return m_settings.m_pll && m_pll.locked(); }
+	Real getPllFrequency() const { return m_pll.getFreq(); }
 
 	void getMagSqLevels(double& avg, double& peak, int& nbSamples)
 	{
diff --git a/plugins/channelrx/demodam/amdemodgui.cpp b/plugins/channelrx/demodam/amdemodgui.cpp
index 2c0eff5a4..c1a8184d2 100644
--- a/plugins/channelrx/demodam/amdemodgui.cpp
+++ b/plugins/channelrx/demodam/amdemodgui.cpp
@@ -142,8 +142,10 @@ void AMDemodGUI::on_deltaFrequency_changed(qint64 value)
 
 void AMDemodGUI::on_pll_toggled(bool checked)
 {
-    if (!checked) {
+    if (!checked)
+    {
         ui->pll->setStyleSheet("QToolButton { background:rgb(79,79,79); }");
+        ui->pll->setToolTip(tr("PLL for synchronous AM"));
     }
 
     m_settings.m_pll = checked;
@@ -427,6 +429,9 @@ void AMDemodGUI::tick()
 	    } else {
 	        ui->pll->setStyleSheet("QToolButton { background:rgb(79,79,79); }");
 	    }
+
+        int freq = (m_amDemod->getPllFrequency() * m_amDemod->getAudioSampleRate()) / (2.0*M_PI);
+        ui->pll->setToolTip(tr("PLL for synchronous AM. Freq = %1 Hz").arg(freq));
 	}
 
 	m_tickCount++;
diff --git a/sdrbase/dsp/phaselockcomplex.cpp b/sdrbase/dsp/phaselockcomplex.cpp
index 991e3b157..0c8d604f3 100644
--- a/sdrbase/dsp/phaselockcomplex.cpp
+++ b/sdrbase/dsp/phaselockcomplex.cpp
@@ -90,7 +90,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
+    m_lockFreq = (2.0*M_PI*(m_pskOrder > 1 ? 6.0 : 1.0)) / sampleRate; // +/- 6 Hz frequency swing
     reset();
 }
 
@@ -161,7 +161,7 @@ void PhaseLockComplex::feed(float re, float im)
     if (m_pskOrder > 1)
     {
         float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
-        m_freq = 0.001*dPhi + 0.999*m_freqPrev;
+        m_freq = m_expAvg.feed(dPhi);
 
         if (m_lockTimeCount < m_lockTime-1)
         {
@@ -188,20 +188,14 @@ void PhaseLockComplex::feed(float re, float im)
             m_lockTimeCount = 0;
         }
 
-        m_freqPrev = m_freq;
         m_phiHatPrev = m_phiHat;
     }
     else
     {
-        m_freq = (m_phiHat - m_phiHatPrev) / (2.0*M_PI);
+        m_freqTest = normalizeAngle(m_phiHat - m_phiHatPrev);
+        m_freq = m_expAvg.feed(m_freqTest);
 
-        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;
+        float dFreq = m_freqTest - m_freqPrev;
 
         if ((dFreq > -0.01) && (dFreq < 0.01))
         {
@@ -215,7 +209,7 @@ void PhaseLockComplex::feed(float re, float im)
         }
 
         m_phiHatPrev = m_phiHat;
-        m_freqPrev = m_freq;
+        m_freqPrev = m_freqTest;
     }
 }
 
diff --git a/sdrbase/dsp/phaselockcomplex.h b/sdrbase/dsp/phaselockcomplex.h
index cd52e72d0..247181e74 100644
--- a/sdrbase/dsp/phaselockcomplex.h
+++ b/sdrbase/dsp/phaselockcomplex.h
@@ -49,12 +49,34 @@ 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 ? m_lockCount > 16 : m_lockCount > m_lockTime-2; }
+    bool locked() const { return m_pskOrder > 1 ? m_lockCount > 10 : m_lockCount > m_lockTime-2; }
     float getFreq() const { return m_freq; }
     float getDeltaPhi() const { return m_deltaPhi; }
     float getPhiHat() const { return m_phiHat; }
 
 private:
+    class ExpAvg
+    {
+    public:
+        ExpAvg() : m_a0(0.999), m_a1(0.001), m_y1(0.0f)
+        {}
+        void setAlpha(const float& alpha)
+        {
+            m_a0 = alpha;
+            m_a1 = 1.0 - alpha;
+        }
+        float feed(const float& x)
+        {
+            float y = m_a1*x + m_a0*m_y1;
+            m_y1 = y;
+            return y;
+        }
+    private:
+        float m_a0; //!< alpha
+        float m_a1; //!< 1 - alpha
+        float m_y1;
+    };
+
     /** Normalize angle in radians into the [-pi,+pi] region */
     static float normalizeAngle(float angle);
 
@@ -82,6 +104,7 @@ private:
     unsigned int m_pskOrder;
     int m_lockTime;
     int m_lockTimeCount;
+    ExpAvg m_expAvg;
 };
 
 #endif /* SDRBASE_DSP_PHASELOCKCOMPLEX_H_ */