Add Costas Loop PLL in Channel Analyzer

Add loop bandwidth and other PLL controls to Channel Analyzer GUI. Fix bug where PLL lock frequency would be incorrect by the decimation factor.
2026-06-08 08:54:49 -04:00 · 2021-03-05 13:37:49 +00:00
parent 2389f0d55c
commit 5d5b221e83
18 changed files with 933 additions and 73 deletions
@@ -0,0 +1,110 @@
+///////////////////////////////////////////////////////////////////////////////////
+// Copyright 2006-2021 Free Software Foundation, Inc.                            //
+// Copyright (C) 2018 Edouard Griffiths, F4EXB                                   //
+// Copyright (C) 2021 Jon Beniston, M7RCE                                        //
+//                                                                               //
+// Based on the Costas Loop from GNU Radio                                       //
+//                                                                               //
+// 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          //
+// the Free Software Foundation as version 3 of the License, or                  //
+// (at your option) any later version.                                           //
+//                                                                               //
+// This program is distributed in the hope that it will be useful,               //
+// but WITHOUT ANY WARRANTY; without even the implied warranty of                //
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the                  //
+// GNU General Public License V3 for more details.                               //
+//                                                                               //
+// You should have received a copy of the GNU General Public License             //
+// along with this program. If not, see <http://www.gnu.org/licenses/>.          //
+///////////////////////////////////////////////////////////////////////////////////
+
+#include "costasloop.h"
+#include <cmath>
+
+// Loop bandwidth supposedly ~ 2pi/100 rads/sample
+// pskOrder 2, 4 or 8
+CostasLoop::CostasLoop(float loopBW, unsigned int pskOrder) :
+    m_maxFreq(1.0f),
+    m_minFreq(-1.0f),
+    m_pskOrder(pskOrder)
+{
+    computeCoefficients(loopBW);
+    reset();
+}
+
+CostasLoop::~CostasLoop()
+{
+}
+
+void CostasLoop::reset()
+{
+    m_y.real(1.0f);
+    m_y.imag(0.0f);
+    m_freq = 0.0f;
+    m_phase = 0.0f;
+    m_freq = 0.0f;
+    m_error = 0.0f;
+}
+
+// 2nd order loop with critical damping
+void CostasLoop::computeCoefficients(float loopBW)
+{
+    float damping = sqrtf(2.0f) / 2.0f;
+    float denom = (1.0 + 2.0 * damping * loopBW + loopBW * loopBW);
+    m_alpha = (4 * damping * loopBW) / denom;
+    m_beta = (4 * loopBW * loopBW) / denom;
+}
+
+void CostasLoop::setSampleRate(unsigned int sampleRate)
+{
+    reset();
+}
+
+static float branchlessClip(float x, float clip)
+{
+    return 0.5f * (std::abs(x + clip) - std::abs(x - clip));
+}
+
+// Don't use built-in complex.h multiply to avoid NaN/INF checking
+static void fastComplexMultiply(std::complex<float> &out, const std::complex<float> cc1, const std::complex<float> cc2)
+{
+    float o_r, o_i;
+
+    o_r = (cc1.real() * cc2.real()) - (cc1.imag() * cc2.imag());
+    o_i = (cc1.real() * cc2.imag()) + (cc1.imag() * cc2.real());
+
+    out.real(o_r);
+    out.imag(o_i);
+}
+
+void CostasLoop::feed(float re, float im)
+{
+    std::complex<float> nco(std::cosf(-m_phase), std::sinf(-m_phase));
+
+    std::complex<float> in, out;
+    in.real(re);
+    in.imag(im);
+    fastComplexMultiply(out, in, nco);
+
+    switch (m_pskOrder)
+    {
+    case 2:
+        m_error = phaseDetector2(out);
+        break;
+    case 4:
+        m_error = phaseDetector4(out);
+        break;
+    case 8:
+        m_error = phaseDetector8(out);
+        break;
+    }
+    m_error = branchlessClip(m_error, 1.0f);
+
+    advanceLoop(m_error);
+    phaseWrap();
+    frequencyLimit();
+
+    m_y.real(-nco.real());
+    m_y.imag(nco.imag());
+}