From b23620ede3850af72c295e3230b0624875d3ccbe Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Wed, 29 Apr 2020 20:42:20 +0200
Subject: [PATCH] Add de and pre emphasis

---
 FM.cpp             |  8 ++++++-
 FM.h               |  2 ++
 FMDirectForm1.h    |  2 +-
 Tools/emphasis.txt | 58 ++++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 68 insertions(+), 2 deletions(-)
 create mode 100644 Tools/emphasis.txt

diff --git a/FM.cpp b/FM.cpp
index fa5fd53..2258109 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -36,9 +36,11 @@ m_kerchunkTimer(),
 m_ackMinTimer(),
 m_ackDelayTimer(),
 m_hangTimer(),
-m_filterStage1(  724,   1448,   724, 32768, -37895, 21352),//3rd order Cheby Filter 300 to 2700Hz, 0.2dB passband ripple
+m_filterStage1(  724,   1448,   724, 32768, -37895, 21352),//3rd order Cheby Filter 300 to 2700Hz, 0.2dB passband ripple, sampling rate 24kHz
 m_filterStage2(32768,      0,-32768, 32768, -50339, 19052),
 m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460),
+m_preemphasis(32768,  13967, 0, 32768, -18801, 0),//75µS 24kHz sampling rate
+m_deemphasis (32768, -18801, 0, 32768,  13967, 0),//75µS 24kHz sampling rate
 m_blanking(),
 m_useCOS(true),
 m_rfAudioBoost(1U),
@@ -79,6 +81,8 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
       stateMachine(validCTCSS && cos, i + 1U);
     }
 
+    currentSample = m_deemphasis.filter(currentSample);
+
     // Only let audio through when relaying audio
     if (m_state == FS_RELAYING || m_state == FS_KERCHUNK) {    
       m_downsampler.addSample(currentSample);
@@ -104,6 +108,8 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 
     currentSample = m_filterStage3.filter(m_filterStage2.filter(m_filterStage1.filter(currentSample)));
 
+    currentSample = m_preemphasis.filter(currentSample);
+
     currentSample += m_ctcssTX.getAudio();
 
     samples[i] = currentSample;
diff --git a/FM.h b/FM.h
index fab8095..a852a80 100644
--- a/FM.h
+++ b/FM.h
@@ -76,6 +76,8 @@ private:
   CFMDirectFormI       m_filterStage1;
   CFMDirectFormI       m_filterStage2;
   CFMDirectFormI       m_filterStage3;
+  CFMDirectFormI       m_preemphasis;
+  CFMDirectFormI       m_deemphasis;
   CFMBlanking          m_blanking;
   bool                 m_useCOS;
   q15_t                m_rfAudioBoost;
diff --git a/FMDirectForm1.h b/FMDirectForm1.h
index 4b90245..450274d 100644
--- a/FMDirectForm1.h
+++ b/FMDirectForm1.h
@@ -79,7 +79,7 @@ public:
 	inline q15_t filter(const q15_t in)
 	{
 		// calculate the output
-		register q31_t out_upscaled = c_b0 * in    //F4FXL puting stauration here made everything quiet, not sure why
+		register q31_t out_upscaled = c_b0 * in
 									+ c_b1 * m_x1
 									+ c_b2 * m_x2
 									- c_a1 * m_y1
diff --git a/Tools/emphasis.txt b/Tools/emphasis.txt
new file mode 100644
index 0000000..39660d1
--- /dev/null
+++ b/Tools/emphasis.txt
@@ -0,0 +1,58 @@
+% GNU Octave script to generate pre and deemphasis filters
+% https://dsp.stackexchange.com/questions/34605/biquad-cookbook-formula-for-broadcast-fm-de-emphasis
+% PACKAGES
+
+pkg load control
+pkg load signal
+
+clear all;
+clc;
+
+fs = 24000;
+samplingtime = 1/fs;
+
+% analog prototype
+A2 = [1];
+B2 = [0.000075 1];
+
+% Pre
+Ds = tf(B2, A2);
+% De
+% Ds = tf(A2, B2);
+ 
+Ds = Ds/dcgain(Ds);
+
+% MZT
+T1 = 0.000075; % 75us
+z1 = -exp(-1.0/(fs*T1));
+p1 = 1+z1;
+
+a0 = 1.0;
+a1 = p1;
+a2 = 0;
+
+b0 = 1.0;
+b1 = z1;
+b2 = 0;
+
+% swap between a1, b1 to select pre- or de-emphasis
+
+# Pre
+Bmzt = [b0 a1 b2]
+Amzt = [a0 b1 a2]
+% De
+% Bmzt = [b0 b1 b2]
+% Amzt = [a0 a1 a2]
+
+DzMZT = tf(Amzt, Bmzt, samplingtime);
+DzMZT = DzMZT/dcgain(DzMZT);
+
+%% Plot
+wmin = 2 * pi * 20.0; % 20Hz
+wmax = 2 * pi * ((fs/2.0) - (fs/2 - 20000)); %20kHz
+
+figure(1);
+bode(Ds, 'b', DzMZT, 'c', {wmin, wmax});
+legend('Analog prototype', 'MZT 2nd order','location', 'northwest');
+grid on;
+pause();