From 5ca90041671f3670111408db27af37d7bb96e193 Mon Sep 17 00:00:00 2001
From: f4exb <f4exb06@gmail.com>
Date: Wed, 16 Dec 2015 08:49:09 +0100
Subject: [PATCH] NFM demod: cleanup and adjustment of phase discriminators

---
 plugins/channel/nfm/nfmdemod.cpp | 42 ++++++--------------------------
 plugins/channel/nfm/nfmdemod.h   | 33 ++++++++++++++++++++++++-
 2 files changed, 39 insertions(+), 36 deletions(-)

diff --git a/plugins/channel/nfm/nfmdemod.cpp b/plugins/channel/nfm/nfmdemod.cpp
index 077b7cc45..9db1fab78 100644
--- a/plugins/channel/nfm/nfmdemod.cpp
+++ b/plugins/channel/nfm/nfmdemod.cpp
@@ -38,6 +38,8 @@ NFMDemod::NFMDemod() :
 	m_audioMute(false),
 	m_afSquelch(2, afSqTones),
 	m_audioFifo(4, 48000),
+	m_fmExcursion(4800),
+	m_fmScaling(384000/4800),
 	m_settingsMutex(QMutex::Recursive)
 {
 	setObjectName("NFMDemod");
@@ -59,7 +61,6 @@ NFMDemod::NFMDemod() :
 
 	m_agcLevel = 1.0;
 	m_AGC.resize(m_agcAttack, m_agcLevel);
-	m_magsq = 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
@@ -138,40 +139,9 @@ void NFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
 
 				qint16 sample;
 
-				//m_AGC.feed(abs(ci));
-				//ci *= (m_agcLevel / m_AGC.getValue());
-
 				m_AGC.feed(ci);
-				//m_magsq = m_AGC.getMagSq() / (1<<30);
 
-				// demod
-				/*
-				Real argument = arg(ci);
-				Real demod = argument - m_lastArgument;
-				m_lastArgument = argument;
-				*/
-
-				/*
-				// Original NFM
-				Complex d = conj(m_m1Sample) * ci;
-				Real demod = atan2(d.imag(), d.real());
-				demod /= M_PI;
-				*/
-
-				/*
-				Real argument1 = arg(ci);//atan2(ci.imag(), ci.real());
-				Real argument2 = m_lastSample.real();
-				Real demod = angleDist(argument2, argument1);
-				m_lastSample = Complex(argument1, 0);
-				*/
-
-				// Alternative without atan - needs AGC
-				// http://www.embedded.com/design/configurable-systems/4212086/DSP-Tricks--Frequency-demodulation-algorithms-
-				Real ip = ci.real() - m_m2Sample.real();
-				Real qp = ci.imag() - m_m2Sample.imag();
-				Real h1 = m_m1Sample.real() * qp;
-				Real h2 = m_m1Sample.imag() * ip;
-				Real demod = (h1 - h2) * 1; // 10000 (multiply by 2^16 after demod)
+				Real demod = phaseDiscriminator2(ci, 0.5);
 
 				m_m2Sample = m_m1Sample;
 				m_m1Sample = ci;
@@ -240,8 +210,7 @@ void NFMDemod::feed(const SampleVector::const_iterator& begin, const SampleVecto
 					else
 					{
 						demod = m_bandpass.filter(demod);
-						demod *= m_running.m_volume;
-						sample = demod * 10;
+						sample = demod * m_running.m_volume;
 					}
 				}
 				else
@@ -362,6 +331,7 @@ void NFMDemod::apply()
 		m_interpolator.create(16, m_config.m_inputSampleRate, m_config.m_rfBandwidth / 2.2);
 		m_interpolatorDistanceRemain = 0;
 		m_interpolatorDistance =  (Real) m_config.m_inputSampleRate / (Real) m_config.m_audioSampleRate;
+		m_fmScaling = m_config.m_inputSampleRate / m_fmExcursion;
 		m_settingsMutex.unlock();
 	}
 
@@ -371,6 +341,8 @@ void NFMDemod::apply()
 		m_settingsMutex.lock();
 		m_lowpass.create(301, m_config.m_audioSampleRate, 250.0);
 		m_bandpass.create(301, m_config.m_audioSampleRate, 300.0, m_config.m_afBandwidth);
+		m_fmExcursion = m_config.m_afBandwidth;
+		m_fmScaling = m_config.m_inputSampleRate / m_fmExcursion;
 		m_settingsMutex.unlock();
 	}
 
diff --git a/plugins/channel/nfm/nfmdemod.h b/plugins/channel/nfm/nfmdemod.h
index 460b16aca..9ca596c18 100644
--- a/plugins/channel/nfm/nfmdemod.h
+++ b/plugins/channel/nfm/nfmdemod.h
@@ -176,7 +176,9 @@ private:
 	AFSquelch m_afSquelch;
 	Real m_agcLevel; // AGC will aim to  this level
 	Real m_agcFloor; // AGC will not go below this level
-	Real m_magsq;
+
+	Real m_fmExcursion;
+	Real m_fmScaling;
 
 	AudioVector m_audioBuffer;
 	uint m_audioBufferFill;
@@ -186,6 +188,35 @@ private:
 	NFMDemodGUI *m_nfmDemodGUI;
 	QMutex m_settingsMutex;
 
+	/**
+	 * Standard discriminator using atan2. On modern processors this is as efficient as the non atan2 one.
+	 * This is better for high fidelity.
+	 */
+	Real phaseDiscriminator(const Complex& sample)
+	{
+		Complex d(std::conj(m_m1Sample) * sample);
+		m_m1Sample = sample;
+		return (std::atan2(d.imag(), d.real()) / M_PI_2) * m_fmScaling;
+	}
+
+	/**
+	 * Alternative without atan at the expense of a slight distorsion on very wideband signals
+	 * http://www.embedded.com/design/configurable-systems/4212086/DSP-Tricks--Frequency-demodulation-algorithms-
+	 * in addition it needs scaling by instantaneous magnitude squared and volume (0..10) adjustment factor
+	 */
+	Real phaseDiscriminator2(const Complex& sample, Real msq)
+	{
+		Real ip = sample.real() - m_m2Sample.real();
+		Real qp = sample.imag() - m_m2Sample.imag();
+		Real h1 = m_m1Sample.real() * qp;
+		Real h2 = m_m1Sample.imag() * ip;
+
+		m_m2Sample = m_m1Sample;
+		m_m1Sample = sample;
+
+		return ((h1 - h2) / (msq * M_PI)) * m_fmScaling;
+	}
+
 	void apply();
 };