From 317836ac923318d35bf6794faea707d6edbe7c66 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Mon, 5 Dec 2016 03:46:42 +0100
Subject: [PATCH] NFM demod: channel power calculation and squelch corrections

---
 plugins/channelrx/demodnfm/nfmdemod.cpp | 32 +++++++++++++------------
 plugins/channelrx/demodnfm/nfmdemod.h   |  7 +++---
 sdrbase/dsp/agc.cpp                     |  4 ++--
 3 files changed, 23 insertions(+), 20 deletions(-)

diff --git a/plugins/channelrx/demodnfm/nfmdemod.cpp b/plugins/channelrx/demodnfm/nfmdemod.cpp
index c89f0fb78..d6aa26041 100644
--- a/plugins/channelrx/demodnfm/nfmdemod.cpp
+++ b/plugins/channelrx/demodnfm/nfmdemod.cpp
@@ -35,7 +35,7 @@ NFMDemod::NFMDemod() :
 	m_ctcssIndex(0),
 	m_sampleCount(0),
 	m_squelchCount(0),
-	m_agcAttack(2400),
+	m_squelchGate(2400),
 	m_audioMute(false),
 	m_squelchOpen(false),
     m_magsqSum(0.0f),
@@ -66,7 +66,8 @@ NFMDemod::NFMDemod() :
 	m_audioBufferFill = 0;
 
 	m_agcLevel = 1.0;
-	m_AGC.resize(m_agcAttack, m_agcLevel);
+	m_AGC.resize(m_squelchGate, m_agcLevel);
+	m_movingAverage.resize(16, 0);
 
 	m_ctcssDetector.setCoefficients(3000, 6000.0); // 0.5s / 2 Hz resolution
 	m_afSquelch.setCoefficients(24, 600, 48000.0, 200, 0); // 4000 Hz span, 250us, 100ms attack
@@ -149,16 +150,18 @@ void NFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
 				qint16 sample;
 
 				m_AGC.feed(ci);
-				Real magsq = m_AGC.getMagSq();
-		        magsq /= (1<<30);
-		        m_magsqSum += magsq;
 
-		        if (magsq > m_magsqPeak)
-		        {
-		            m_magsqPeak = magsq;
-		        }
+                double magsqRaw = m_AGC.getMagSq();
+                Real magsq = magsqRaw / (1<<30);
+                m_movingAverage.feed(magsq);
+                m_magsqSum += magsq;
 
-		        m_magsqCount++;
+                if (magsq > m_magsqPeak)
+                {
+                    m_magsqPeak = magsq;
+                }
+
+                m_magsqCount++;
 
 				Real demod = m_phaseDiscri.phaseDiscriminator2(ci);
 
@@ -168,9 +171,9 @@ void NFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
 
 				// AF processing
 
-				if (m_AGC.getAverage()/(1<<30) > m_squelchLevel)
+				if (m_movingAverage.average() > m_squelchLevel)
 				{
-					if (m_squelchCount < m_agcAttack)
+					if (m_squelchCount < m_squelchGate)
 					{
 						m_squelchCount++;
 					}
@@ -181,7 +184,7 @@ void NFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
 				}
 
 				//squelchOpen = (getMag() > m_squelchLevel);
-				m_squelchOpen = m_squelchCount == m_agcAttack; // wait for AGC to stabilize
+				m_squelchOpen = m_squelchCount == m_squelchGate; // wait for AGC to stabilize
 
 				/*
 				if (m_afSquelch.analyze(demod))
@@ -374,8 +377,7 @@ void NFMDemod::apply()
 
 	if (m_config.m_squelchGate != m_running.m_squelchGate)
 	{
-		m_agcAttack = 480 * m_config.m_squelchGate; // gate is given in 10s of ms at 48000 Hz audio sample rate
-		m_AGC.resize(m_agcAttack, m_agcLevel);
+		m_squelchGate = 480 * m_config.m_squelchGate; // gate is given in 10s of ms at 48000 Hz audio sample rate
 		m_squelchCount = 0; // reset squelch open counter
 	}
 
diff --git a/plugins/channelrx/demodnfm/nfmdemod.h b/plugins/channelrx/demodnfm/nfmdemod.h
index fa03ccabc..eb2c1e593 100644
--- a/plugins/channelrx/demodnfm/nfmdemod.h
+++ b/plugins/channelrx/demodnfm/nfmdemod.h
@@ -68,7 +68,7 @@ public:
 		m_ctcssIndexSelected = selectedCtcssIndex;
 	}
 
-	Real getMag() { return m_AGC.getAverage() / (1<<15); }
+	Real getMag() { return m_movingAverage.average(); }
 	bool getSquelchOpen() const { return m_squelchOpen; }
 
     void getMagSqLevels(Real& avg, Real& peak, int& nbSamples)
@@ -192,10 +192,10 @@ private:
 	int m_ctcssIndexSelected;
 	int m_sampleCount;
 	int m_squelchCount;
-	int m_agcAttack;
+	int m_squelchGate;
 	bool m_audioMute;
 
-	double m_squelchLevel;
+	Real m_squelchLevel;
 	bool m_squelchOpen;
     Real m_magsqSum;
     Real m_magsqPeak;
@@ -205,6 +205,7 @@ private:
 	//Complex m_m1Sample;
 	//Complex m_m2Sample;
 	MagAGC m_AGC;
+	MovingAverage<Real> m_movingAverage;
 	AFSquelch m_afSquelch;
 	Real m_agcLevel; // AGC will aim to  this level
 	Real m_agcFloor; // AGC will not go below this level
diff --git a/sdrbase/dsp/agc.cpp b/sdrbase/dsp/agc.cpp
index fa1f5da02..f62b36b8a 100644
--- a/sdrbase/dsp/agc.cpp
+++ b/sdrbase/dsp/agc.cpp
@@ -82,9 +82,9 @@ MagAGC::~MagAGC()
 
 void MagAGC::feed(Complex& ci)
 {
-	m_magsq = sqrt(ci.real()*ci.real() + ci.imag()*ci.imag());
+	m_magsq = ci.real()*ci.real() + ci.imag()*ci.imag();
 	m_moving_average.feed(m_magsq);
-	m_u0 = m_R / m_moving_average.average();
+	m_u0 = m_R / sqrt(m_moving_average.average());
 	ci *= m_u0;
 }