Add de and pre emphasis

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;

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;

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

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();