From bb2d53012243b768b1681f6cab631568691fc0e7 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Tue, 15 May 2018 19:40:53 +0200
Subject: [PATCH] New PLL: phase lock status draft

---
 plugins/channelrx/chanalyzerng/chanalyzerng.h |  8 +--
 sdrbase/dsp/phaselockcomplex.cpp              | 53 ++++++++++++++++---
 sdrbase/dsp/phaselockcomplex.h                | 10 +++-
 3 files changed, 57 insertions(+), 14 deletions(-)

diff --git a/plugins/channelrx/chanalyzerng/chanalyzerng.h b/plugins/channelrx/chanalyzerng/chanalyzerng.h
index 222b30628..ee7f7b573 100644
--- a/plugins/channelrx/chanalyzerng/chanalyzerng.h
+++ b/plugins/channelrx/chanalyzerng/chanalyzerng.h
@@ -262,10 +262,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 mixQ = m_sum.real() * m_pll.getReal() + m_sum.imag() * m_pll.getImag();
-//                    Real mixI = m_pll.getReal() * SDR_RX_SCALED;
-//                    Real mixQ = m_pll.getImag() * SDR_RX_SCALED;
+                    // 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 mixI = m_pll.getReal() * SDR_RX_SCALED;
+                    Real mixQ = m_pll.getImag() * SDR_RX_SCALED;
 
                     if (m_running.m_ssb & !m_usb)
                     { // invert spectrum for LSB
diff --git a/sdrbase/dsp/phaselockcomplex.cpp b/sdrbase/dsp/phaselockcomplex.cpp
index b6572ef58..c3440c0f2 100644
--- a/sdrbase/dsp/phaselockcomplex.cpp
+++ b/sdrbase/dsp/phaselockcomplex.cpp
@@ -34,12 +34,18 @@ PhaseLockComplex::PhaseLockComplex() :
     m_v1(0.0),
     m_v2(0.0),
     m_deltaPhi(0.0),
-    m_phiHatLast(0.0),
     m_phiHat(0.0),
+    m_phiHatPrev(0.0),
+    m_phiHat1(0.0),
+    m_phiHat2(0.0),
+    m_dPhiHatAccum(0.0),
+    m_phiHatCount(0),
     m_y(1.0, 0.0),
     m_yRe(1.0),
     m_yIm(0.0),
     m_freq(0.0),
+    m_lock(0.0),
+    m_lockCount(0),
     m_pskOrder(1)
 {
 }
@@ -86,13 +92,19 @@ void PhaseLockComplex::reset()
     m_v1 = 0.0f;
     m_v2 = 0.0f;
     m_deltaPhi = 0.0f;
-    m_phiHatLast = 0.0f;
     m_phiHat = 0.0f;
+    m_phiHatPrev = 0.0f;
+    m_phiHat1 = 0.0f;
+    m_phiHat2 = 0.0f;
+    m_dPhiHatAccum = 0.0f;
+    m_phiHatCount = 0;
     m_y.real(1.0);
     m_y.real(0.0);
     m_yRe = 1.0f;
     m_yIm = 0.0f;
     m_freq = 0.0f;
+    m_lock = 0.0f;
+    m_lockCount = 0;
 }
 
 void PhaseLockComplex::feed(float re, float im)
@@ -136,15 +148,40 @@ void PhaseLockComplex::feed(float re, float im)
         m_phiHat += 2.0*M_PI;
     }
 
-    m_freq = (m_phiHat - m_phiHatLast) / (2.0*M_PI);
+    float dPhi = normalizeAngle(m_phiHat - m_phiHatPrev);
+    m_phiHatPrev = m_phiHat;
 
-    if (m_freq < -1.0f) {
-        m_freq += 2.0f;
-    } else if (m_freq > 1.0f) {
-        m_freq -= 2.0f;
+    if (m_phiHatCount < 9)
+    {
+        m_dPhiHatAccum += dPhi;
+    }
+    else
+    {
+        float dPhi1 = (m_phiHat1 - m_dPhiHatAccum) / 10.0f;
+        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_lockCount < 1000) {
+                m_lockCount++;
+            }
+        }
+        else
+        {
+            if (m_lockCount > 0) {
+                m_lockCount--;
+            }
+        }
+
+        m_freq = dPhi1 / 2.0*M_PI; // first derivative of phase
+        m_phiHat2 = m_phiHat1; 
+        m_phiHat1 = m_dPhiHatAccum;
+        m_dPhiHatAccum = 0.0f;
+        m_phiHatCount = 0;
     }
 
-    m_phiHatLast = m_phiHat;
+    m_dPhiHatAccum += dPhi;
 }
 
 float PhaseLockComplex::normalizeAngle(float angle)
diff --git a/sdrbase/dsp/phaselockcomplex.h b/sdrbase/dsp/phaselockcomplex.h
index ed5d7633e..675c12bb8 100644
--- a/sdrbase/dsp/phaselockcomplex.h
+++ b/sdrbase/dsp/phaselockcomplex.h
@@ -46,7 +46,7 @@ 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_deltaPhi > -0.1) && (m_deltaPhi < 0.1); }
+    bool locked() const { return m_lockCount > 500; }
     float getFrequency() const { return m_freq; }
     float getDeltaPhi() const { return m_deltaPhi; }
     float getPhiHat() const { return m_phiHat; }
@@ -65,12 +65,18 @@ private:
     float m_v1;
     float m_v2;
     float m_deltaPhi;
-    float m_phiHatLast;
     float m_phiHat;
+    float m_phiHatPrev;
+    float m_phiHat1;
+    float m_phiHat2;
+    float m_dPhiHatAccum;
+    int m_phiHatCount;
     std::complex<float> m_y;
     float m_yRe;
     float m_yIm;
     float m_freq;
+    float m_lock;
+    int m_lockCount;
     unsigned int m_pskOrder;
 };