From 68c50769fe2992de24b44bf469301000a8271c10 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Mon, 14 May 2018 19:14:30 +0200
Subject: [PATCH] New PLL: implemented trick on the phase comparator for M-ary
 PSK operation

---
 sdrbase/dsp/phaselockcomplex.cpp | 28 +++++++++++++++++++++++++---
 sdrbase/dsp/phaselockcomplex.h   | 13 +++++++++++--
 2 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/sdrbase/dsp/phaselockcomplex.cpp b/sdrbase/dsp/phaselockcomplex.cpp
index 1fedb512d..b6572ef58 100644
--- a/sdrbase/dsp/phaselockcomplex.cpp
+++ b/sdrbase/dsp/phaselockcomplex.cpp
@@ -3,7 +3,9 @@
 // written by Edouard Griffiths                                                  //
 //                                                                               //
 // See: http://liquidsdr.org/blog/pll-howto/                                     //
-// Fixes filter registers saturation                                             //
+// Fixed filter registers saturation                                             //
+// Added order for PSK locking. This brilliant idea actually comes from this     //
+// post: https://www.dsprelated.com/showthread/comp.dsp/36356-1.php              //
 //                                                                               //
 // 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          //
@@ -37,7 +39,8 @@ PhaseLockComplex::PhaseLockComplex() :
     m_y(1.0, 0.0),
     m_yRe(1.0),
     m_yIm(0.0),
-    m_freq(0.0)
+    m_freq(0.0),
+    m_pskOrder(1)
 {
 }
 
@@ -71,6 +74,11 @@ void PhaseLockComplex::computeCoefficients(Real wn, Real zeta, Real K)
     reset();
 }
 
+void PhaseLockComplex::setPskOrder(unsigned int order)
+{
+    m_pskOrder = order > 0 ? order : 1;
+}
+
 void PhaseLockComplex::reset()
 {
     // reset filter accumulators and phase
@@ -96,6 +104,11 @@ void PhaseLockComplex::feed(float re, float im)
     std::complex<float> x(re, im);
     m_deltaPhi = std::arg(x * std::conj(m_y));
 
+    // bring phase 0 on any of the PSK symbols
+    if (m_pskOrder > 1) {
+        m_deltaPhi = normalizeAngle(m_pskOrder*m_deltaPhi);
+    }
+
     // advance buffer
     m_v2 = m_v1;  // shift center register to upper register
     m_v1 = m_v0;  // shift lower register to center register
@@ -134,4 +147,13 @@ void PhaseLockComplex::feed(float re, float im)
     m_phiHatLast = m_phiHat;
 }
 
-
+float PhaseLockComplex::normalizeAngle(float angle)
+{
+    while (angle <= -M_PI) {
+        angle += 2.0*M_PI;
+    }
+    while (angle > M_PI) {
+        angle -= 2.0*M_PI;
+    }
+    return angle;
+}
diff --git a/sdrbase/dsp/phaselockcomplex.h b/sdrbase/dsp/phaselockcomplex.h
index de5479c51..ed5d7633e 100644
--- a/sdrbase/dsp/phaselockcomplex.h
+++ b/sdrbase/dsp/phaselockcomplex.h
@@ -3,7 +3,9 @@
 // written by Edouard Griffiths                                                  //
 //                                                                               //
 // See: http://liquidsdr.org/blog/pll-howto/                                     //
-// Fixes filter registers saturation                                             //
+// Fixed filter registers saturation                                             //
+// Added order for PSK locking. This brilliant idea actually comes from this     //
+// post: https://www.dsprelated.com/showthread/comp.dsp/36356-1.php              //
 //                                                                               //
 // 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          //
@@ -35,6 +37,10 @@ public:
      * \param K    PLL loop gain
      * */
     void computeCoefficients(Real wn, Real zeta, Real K);
+    /** Set the PSK order for the phase comparator
+     * \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);
     void reset();
     void feed(float re, float im);
     const std::complex<float>& getComplex() const { return m_y; }
@@ -46,6 +52,9 @@ public:
     float getPhiHat() const { return m_phiHat; }
 
 private:
+    /** Normalize angle in radians into the [-pi,+pi] region */
+    static float normalizeAngle(float angle);
+
     // a0 = 1 is implied
     float m_a1;
     float m_a2;
@@ -62,7 +71,7 @@ private:
     float m_yRe;
     float m_yIm;
     float m_freq;
-
+    unsigned int m_pskOrder;
 };