From 906de3ad119cdc1effd4acafad04d94735a0cbc4 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Thu, 23 Apr 2020 21:16:20 +0200
Subject: [PATCH 01/10] Remove bit roation

---
 FMCTCSSRX.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp
index 3305ba8..623fc5b 100644
--- a/FMCTCSSRX.cpp
+++ b/FMCTCSSRX.cpp
@@ -111,7 +111,7 @@ CTCSSState CFMCTCSSRX::process(q15_t sample)
 {
   m_result = m_result & (~CTS_READY);
 
-  q31_t samp = q31_t(sample) << 16;
+  q31_t samp = q31_t(sample);
 
   q31_t q2 = m_q1;
   m_q1 = m_q0;

From ce0aebce488b9c595c3f7601285c448d53b63014 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Thu, 23 Apr 2020 21:16:52 +0200
Subject: [PATCH 02/10] Using 2 stage IIR filter

---
 FM.cpp | 45 +++++++++------------------------------------
 FM.h   |  7 +++++--
 2 files changed, 14 insertions(+), 38 deletions(-)

diff --git a/FM.cpp b/FM.cpp
index d94c5a4..1c70b40 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -20,18 +20,11 @@
 #include "Globals.h"
 #include "FM.h"
 
-q15_t FILTER_COEFFS[] = {
-   -630,   -842,   -846,   -634,   -312,    -53,    -14,   -251,   -683,  -1113,  -1322,  -1179,   -718,   -147,    234,    172,
-   -399,  -1298,  -2124,  -2402,  -1783,   -201,   2051,   4399,   6169,   6827,   6169,   4399,   2051,   -201,  -1783,  -2402,
-  -2124,  -1298,   -399,    172,    234,   -147,   -718,  -1179,  -1322,  -1113,   -683,   -251,    -14,    -53,   -312,   -634,
-   -846,   -842,   -630};
-
-const uint16_t FILTER_COEFFS_LEN = 51U;
-
+// 2 stage IIR Butterworth filter generated using https://github.com/F4FXL/iir_fixed_point/blob/4f1e580a7dad9f8742d24a06edd14b62110ba6e4/gen_coeff.py
+q15_t FILTER_COEFFS[] = {1105,  2210, 1105, 16384,-19003,7512, 14,//1st stage
+                         16384,-32768,16384,16384,-31020,14751,14};//2nd stage
 
 CFM::CFM() :
-m_filterBuffer(NULL),
-m_filterPosition(0U),
 m_callsign(),
 m_rfAck(),
 m_ctcssRX(),
@@ -46,13 +39,15 @@ m_holdoffTimer(),
 m_kerchunkTimer(),
 m_ackMinTimer(),
 m_ackDelayTimer(),
-m_hangTimer()
+m_hangTimer(),
+m_filter()
 {
-  m_filterBuffer = new q15_t[FILTER_COEFFS_LEN];
+  arm_biquad_cascade_df1_init_q15(&m_filter, 2, FILTER_COEFFS, m_filterState, 0);
 }
 
 void CFM::samples(q15_t* samples, uint8_t length)
 {
+  arm_biquad_casd_df1_inst_q15* filterPtr = &m_filter;
   uint8_t i = 0;
   for (; i < length; i++) {
     q15_t currentSample = samples[i];//save to a local variable to avoid indirection on every access
@@ -98,7 +93,8 @@ void CFM::samples(q15_t* samples, uint8_t length)
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
       currentSample += m_timeoutTone.getAudio();
 
-    currentSample = filter(currentSample);
+    //currentSample = filter(currentSample);
+    arm_biquad_cascade_df1_fast_q15(filterPtr, samples +i, &currentSample, 1);
 
     currentSample += m_ctcssTX.getAudio();
 
@@ -394,26 +390,3 @@ void CFM::beginRelaying()
   m_timeoutTimer.start();
   m_ackMinTimer.start();
 }
-
-q15_t CFM::filter(q15_t sample)
-{
-  q15_t output = 0;
-
-  m_filterBuffer[m_filterPosition] = sample;
-
-  uint8_t iTaps = 0U;
-
-  for (int8_t i = m_filterPosition; i >= 0; i--) {
-    q31_t temp = FILTER_COEFFS[iTaps++] * m_filterBuffer[i];
-    output += q15_t(__SSAT((temp >> 15), 16));
-  }
-
-  for (int8_t i = FILTER_COEFFS_LEN - 1; i >= m_filterPosition; i--) {
-    q31_t temp = FILTER_COEFFS[iTaps++] * m_filterBuffer[i];
-    output += q15_t(__SSAT((temp >> 15), 16));
-  }
-
-  m_filterPosition = (m_filterPosition + 1U) % FILTER_COEFFS_LEN;
-
-  return output;
-}
diff --git a/FM.h b/FM.h
index d2f9419..a125a87 100644
--- a/FM.h
+++ b/FM.h
@@ -37,6 +37,9 @@ enum FM_STATE {
   FS_HANG
 };
 
+
+
+
 class CFM {
 public:
   CFM();
@@ -52,8 +55,6 @@ public:
   uint8_t setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFrequency, uint8_t ctcssThreshold, uint8_t ctcssLevel, uint8_t kerchunkTime, uint8_t hangTime);
 
 private:
-  q15_t*               m_filterBuffer;
-  uint8_t              m_filterPosition;
   CFMKeyer             m_callsign;
   CFMKeyer             m_rfAck;
   CFMCTCSSRX           m_ctcssRX;
@@ -69,6 +70,8 @@ private:
   CFMTimer             m_ackMinTimer;
   CFMTimer             m_ackDelayTimer;
   CFMTimer             m_hangTimer;
+  arm_biquad_casd_df1_inst_q15 m_filter;
+  q15_t                m_filterState[8];//must be filterOrder * 4 long
 
   void stateMachine(bool validSignal, uint8_t length);
   void listeningState(bool validSignal);

From 507a4bf64c496e0e61f43b3308e7e1700f13cbe4 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Thu, 23 Apr 2020 22:37:29 +0200
Subject: [PATCH 03/10] 3rd order IIR

---
 FM.cpp | 13 ++++++++-----
 FM.h   |  2 +-
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/FM.cpp b/FM.cpp
index 1c70b40..1ec448b 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -20,9 +20,13 @@
 #include "Globals.h"
 #include "FM.h"
 
-// 2 stage IIR Butterworth filter generated using https://github.com/F4FXL/iir_fixed_point/blob/4f1e580a7dad9f8742d24a06edd14b62110ba6e4/gen_coeff.py
-q15_t FILTER_COEFFS[] = {1105,  2210, 1105, 16384,-19003,7512, 14,//1st stage
-                         16384,-32768,16384,16384,-31020,14751,14};//2nd stage
+// 3 stage IIR Butterworth filter generated (if you change the order change the size of m_filterState). Also change the ordre in init call below
+// 0.2db band pass ripple
+// 300 - 2700Hz
+q15_t FILTER_COEFFS[] = {  362,   724,    362,16384,-18947,10676, 14,//1st stage
+                         16384,     0, -16384,16384,-25170, 9526, 14,//2nd stage
+                         16384,-32768,  16384,16384,-32037,15730,14};//3rd stage
+
 
 CFM::CFM() :
 m_callsign(),
@@ -42,7 +46,7 @@ m_ackDelayTimer(),
 m_hangTimer(),
 m_filter()
 {
-  arm_biquad_cascade_df1_init_q15(&m_filter, 2, FILTER_COEFFS, m_filterState, 0);
+  arm_biquad_cascade_df1_init_q15(&m_filter, 3, FILTER_COEFFS, m_filterState, 0);
 }
 
 void CFM::samples(q15_t* samples, uint8_t length)
@@ -93,7 +97,6 @@ void CFM::samples(q15_t* samples, uint8_t length)
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
       currentSample += m_timeoutTone.getAudio();
 
-    //currentSample = filter(currentSample);
     arm_biquad_cascade_df1_fast_q15(filterPtr, samples +i, &currentSample, 1);
 
     currentSample += m_ctcssTX.getAudio();
diff --git a/FM.h b/FM.h
index a125a87..474f36d 100644
--- a/FM.h
+++ b/FM.h
@@ -71,7 +71,7 @@ private:
   CFMTimer             m_ackDelayTimer;
   CFMTimer             m_hangTimer;
   arm_biquad_casd_df1_inst_q15 m_filter;
-  q15_t                m_filterState[8];//must be filterOrder * 4 long
+  q15_t                m_filterState[12];//must be filterOrder * 4 long
 
   void stateMachine(bool validSignal, uint8_t length);
   void listeningState(bool validSignal);

From f8d082e2ddfff0358f9255504cd7f56b8f1d15a5 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Fri, 24 Apr 2020 09:10:50 +0200
Subject: [PATCH 04/10] Add FM filtering

---
 FM.cpp                          |  14 ++--
 FM.h                            |   6 +-
 FMDirectForm1.h                 | 114 ++++++++++++++++++++++++++++++++
 FMGenerateFilterCoefficients.py |  45 +++++++++++++
 4 files changed, 170 insertions(+), 9 deletions(-)
 create mode 100644 FMDirectForm1.h
 create mode 100644 FMGenerateFilterCoefficients.py

diff --git a/FM.cpp b/FM.cpp
index 1ec448b..03aa6ec 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -23,9 +23,9 @@
 // 3 stage IIR Butterworth filter generated (if you change the order change the size of m_filterState). Also change the ordre in init call below
 // 0.2db band pass ripple
 // 300 - 2700Hz
-q15_t FILTER_COEFFS[] = {  362,   724,    362,16384,-18947,10676, 14,//1st stage
-                         16384,     0, -16384,16384,-25170, 9526, 14,//2nd stage
-                         16384,-32768,  16384,16384,-32037,15730,14};//3rd stage
+q15_t FILTER_COEFFS[] = {362,724,362,16384,-18947,10676,
+                          16384,0,-16384,16384,-25170,9526,
+                          16384,-32768,16384,16384,-32037,15730};
 
 
 CFM::CFM() :
@@ -44,14 +44,14 @@ m_kerchunkTimer(),
 m_ackMinTimer(),
 m_ackDelayTimer(),
 m_hangTimer(),
-m_filter()
+m_filterStage1(  724,   1448,   724, 32768, -37895, 21352),
+m_filterStage2(32768,      0,-32768, 32768, -50339, 19052),
+m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460)
 {
-  arm_biquad_cascade_df1_init_q15(&m_filter, 3, FILTER_COEFFS, m_filterState, 0);
 }
 
 void CFM::samples(q15_t* samples, uint8_t length)
 {
-  arm_biquad_casd_df1_inst_q15* filterPtr = &m_filter;
   uint8_t i = 0;
   for (; i < length; i++) {
     q15_t currentSample = samples[i];//save to a local variable to avoid indirection on every access
@@ -97,7 +97,7 @@ void CFM::samples(q15_t* samples, uint8_t length)
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
       currentSample += m_timeoutTone.getAudio();
 
-    arm_biquad_cascade_df1_fast_q15(filterPtr, samples +i, &currentSample, 1);
+    currentSample = q15_t(m_filterStage3.filter(m_filterStage2.filter(m_filterStage1.filter(currentSample))));
 
     currentSample += m_ctcssTX.getAudio();
 
diff --git a/FM.h b/FM.h
index 474f36d..77a3d81 100644
--- a/FM.h
+++ b/FM.h
@@ -26,6 +26,7 @@
 #include "FMTimeout.h"
 #include "FMKeyer.h"
 #include "FMTimer.h"
+#include "FMDirectForm1.h"
 
 enum FM_STATE {
   FS_LISTENING,
@@ -70,8 +71,9 @@ private:
   CFMTimer             m_ackMinTimer;
   CFMTimer             m_ackDelayTimer;
   CFMTimer             m_hangTimer;
-  arm_biquad_casd_df1_inst_q15 m_filter;
-  q15_t                m_filterState[12];//must be filterOrder * 4 long
+  CFMDirectFormI          m_filterStage1;
+  CFMDirectFormI          m_filterStage2;
+  CFMDirectFormI          m_filterStage3;
 
   void stateMachine(bool validSignal, uint8_t length);
   void listeningState(bool validSignal);
diff --git a/FMDirectForm1.h b/FMDirectForm1.h
new file mode 100644
index 0000000..857e0a6
--- /dev/null
+++ b/FMDirectForm1.h
@@ -0,0 +1,114 @@
+
+/*******************************************************************************
+This header file has been taken from:
+"A Collection of Useful C++ Classes for Digital Signal Processing"
+By Vinnie Falco 
+Bernd Porr adapted it for Linux and turned it into a filter using
+fixed point arithmetic.
+--------------------------------------------------------------------------------
+License: MIT License (http://www.opensource.org/licenses/mit-license.php)
+Copyright (c) 2009 by Vinnie Falco
+Copyright (C) 2013-2017, Bernd Porr, mail@berndporr.me.uk
+Copyright (C) 2020 , Mario Molitor , mario_molitor@web.de
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*******************************************************************************/
+
+// based on https://raw.githubusercontent.com/berndporr/iir_fixed_point/master/DirectFormI.h
+
+#include "Globals.h"
+
+#ifndef DIRECTFORMI_HPP_
+#define DIRECTFORMI_HPP_
+class CFMDirectFormI
+{
+public:
+
+	// convenience function which takes the a0 argument but ignores it!
+	CFMDirectFormI(const q31_t b0, const q31_t b1, const q31_t b2,
+			const q31_t, const q31_t a1, const q31_t a2)
+	{
+		// FIR coefficients
+		c_b0 = b0;
+		c_b1 = b1;
+		c_b2 = b2;
+		// IIR coefficients
+		c_a1 = a1;
+		c_a2 = a2;
+		reset();
+	}
+
+	CFMDirectFormI(const CFMDirectFormI &my)
+	{
+	// delay line
+		m_x2 = my.m_x2; // x[n-2]
+		m_y2 = my.m_y2; // y[n-2]
+		m_x1 = my.m_x1; // x[n-1]
+		m_y1 = my.m_y1; // y[n-1]
+
+	// coefficients
+		c_b0 = my.c_b0;
+		c_b1 = my.c_b1;
+		c_b2 = my.c_b2; // FIR
+		c_a1 = my.c_a1;
+		c_a2 = my.c_a2; // IIR
+
+	// scaling factor
+		q_scaling = my.q_scaling; // 2^q_scaling
+    }
+
+	void reset ()
+	{
+		m_x1 = 0;
+		m_x2 = 0;
+		m_y1 = 0;
+		m_y2 = 0;
+	}
+
+	// filtering operation: one sample in and one out
+	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
+									+ c_b1 * m_x1
+									+ c_b2 * m_x2
+									- c_a1 * m_y1
+									- c_a2 * m_y2;
+
+		q15_t out = __SSAT(out_upscaled >> 15, 15);
+
+		// update the delay lines
+		m_x2 = m_x1;
+		m_y2 = m_y1;
+		m_x1 = in;
+		m_y1 = out;
+
+		return out;
+	}
+
+private:
+	// delay line
+	q31_t m_x2; // x[n-2]
+	q31_t m_y2; // y[n-2]
+	q31_t m_x1; // x[n-1]
+	q31_t m_y1; // y[n-1]
+
+	// coefficients
+	q31_t c_b0,c_b1,c_b2; // FIR
+	q31_t c_a1,c_a2; // IIR
+};
+
+#endif /* DIRECTFORMI_HPP_ */
\ No newline at end of file
diff --git a/FMGenerateFilterCoefficients.py b/FMGenerateFilterCoefficients.py
new file mode 100644
index 0000000..84ea62b
--- /dev/null
+++ b/FMGenerateFilterCoefficients.py
@@ -0,0 +1,45 @@
+# based on https://github.com/berndporr/iir_fixed_point/blob/master/gen_coeff.py
+
+import numpy as np
+import scipy.signal as signal
+import pylab as pl
+
+# Calculate the coefficients for a pure fixed point
+# integer filter
+
+# sampling rate
+fs = 24000
+
+# cutoffs
+f1 = 300
+f2 = 2700
+# ripple
+rp = 0.2
+
+# scaling factor in bits, do not change !
+q = 15
+# scaling factor as facor...
+scaling_factor = 2**q
+
+# let's generate a sequence of 2nd order IIR filters
+#sos = signal.butter(2,[f1/fs*2,f2/fs*2],'pass',output='sos')
+sos = signal.cheby1(3,rp,[f1/fs*2,f2/fs*2],'bandpass', output='sos')
+
+sos = np.round((sos) * scaling_factor)
+
+# print coefficients
+for biquad in sos:
+    for coeff in biquad:
+        print(int(coeff),",",sep="",end="")
+        #print((coeff),",",sep="",end="")
+    print("")
+
+# plot the frequency response
+b,a = signal.sos2tf(sos)
+w,h = signal.freqz(b,a)
+pl.plot(w/np.pi/2*fs,20*np.log(np.abs(h)))
+pl.xlabel('frequency/Hz');
+pl.ylabel('gain/dB');
+pl.ylim(top=1,bottom=-20);
+pl.xlim(left=250, right=12000);
+pl.show()
\ No newline at end of file

From a720ceeb54ba6f39f4dff700791b5afed8ace2db Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Fri, 24 Apr 2020 09:20:26 +0200
Subject: [PATCH 05/10] clean up

---
 FM.cpp          | 8 --------
 FMDirectForm1.h | 9 +++------
 2 files changed, 3 insertions(+), 14 deletions(-)

diff --git a/FM.cpp b/FM.cpp
index 03aa6ec..019e27e 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -20,14 +20,6 @@
 #include "Globals.h"
 #include "FM.h"
 
-// 3 stage IIR Butterworth filter generated (if you change the order change the size of m_filterState). Also change the ordre in init call below
-// 0.2db band pass ripple
-// 300 - 2700Hz
-q15_t FILTER_COEFFS[] = {362,724,362,16384,-18947,10676,
-                          16384,0,-16384,16384,-25170,9526,
-                          16384,-32768,16384,16384,-32037,15730};
-
-
 CFM::CFM() :
 m_callsign(),
 m_rfAck(),
diff --git a/FMDirectForm1.h b/FMDirectForm1.h
index 857e0a6..4b90245 100644
--- a/FMDirectForm1.h
+++ b/FMDirectForm1.h
@@ -31,8 +31,8 @@ THE SOFTWARE.
 
 #include "Globals.h"
 
-#ifndef DIRECTFORMI_HPP_
-#define DIRECTFORMI_HPP_
+#ifndef DIRECTFORMI_H_
+#define DIRECTFORMI_H_
 class CFMDirectFormI
 {
 public:
@@ -65,9 +65,6 @@ public:
 		c_b2 = my.c_b2; // FIR
 		c_a1 = my.c_a1;
 		c_a2 = my.c_a2; // IIR
-
-	// scaling factor
-		q_scaling = my.q_scaling; // 2^q_scaling
     }
 
 	void reset ()
@@ -111,4 +108,4 @@ private:
 	q31_t c_a1,c_a2; // IIR
 };
 
-#endif /* DIRECTFORMI_HPP_ */
\ No newline at end of file
+#endif /* DIRECTFORMI_H */
\ No newline at end of file

From 22c40e1d968c4ddff03cd25fc588df4aa0a8bc08 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Thu, 23 Apr 2020 21:16:20 +0200
Subject: [PATCH 06/10] Remove bit roation

---
 FMCTCSSRX.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp
index 5c36f66..6240f34 100644
--- a/FMCTCSSRX.cpp
+++ b/FMCTCSSRX.cpp
@@ -111,6 +111,8 @@ CTCSSState CFMCTCSSRX::process(q15_t sample)
 {
   m_result = m_result & (~CTS_READY);
 
+  q31_t samp = q31_t(sample);
+
   q31_t q2 = m_q1;
   m_q1 = m_q0;
 

From 8ecd8b67f40997e5bbbb31599653a03f9376c5ae Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Thu, 23 Apr 2020 21:16:52 +0200
Subject: [PATCH 07/10] Using 2 stage IIR filter

---
 FM.cpp | 45 +++++++++------------------------------------
 FM.h   |  7 +++++--
 2 files changed, 14 insertions(+), 38 deletions(-)

diff --git a/FM.cpp b/FM.cpp
index d94c5a4..1c70b40 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -20,18 +20,11 @@
 #include "Globals.h"
 #include "FM.h"
 
-q15_t FILTER_COEFFS[] = {
-   -630,   -842,   -846,   -634,   -312,    -53,    -14,   -251,   -683,  -1113,  -1322,  -1179,   -718,   -147,    234,    172,
-   -399,  -1298,  -2124,  -2402,  -1783,   -201,   2051,   4399,   6169,   6827,   6169,   4399,   2051,   -201,  -1783,  -2402,
-  -2124,  -1298,   -399,    172,    234,   -147,   -718,  -1179,  -1322,  -1113,   -683,   -251,    -14,    -53,   -312,   -634,
-   -846,   -842,   -630};
-
-const uint16_t FILTER_COEFFS_LEN = 51U;
-
+// 2 stage IIR Butterworth filter generated using https://github.com/F4FXL/iir_fixed_point/blob/4f1e580a7dad9f8742d24a06edd14b62110ba6e4/gen_coeff.py
+q15_t FILTER_COEFFS[] = {1105,  2210, 1105, 16384,-19003,7512, 14,//1st stage
+                         16384,-32768,16384,16384,-31020,14751,14};//2nd stage
 
 CFM::CFM() :
-m_filterBuffer(NULL),
-m_filterPosition(0U),
 m_callsign(),
 m_rfAck(),
 m_ctcssRX(),
@@ -46,13 +39,15 @@ m_holdoffTimer(),
 m_kerchunkTimer(),
 m_ackMinTimer(),
 m_ackDelayTimer(),
-m_hangTimer()
+m_hangTimer(),
+m_filter()
 {
-  m_filterBuffer = new q15_t[FILTER_COEFFS_LEN];
+  arm_biquad_cascade_df1_init_q15(&m_filter, 2, FILTER_COEFFS, m_filterState, 0);
 }
 
 void CFM::samples(q15_t* samples, uint8_t length)
 {
+  arm_biquad_casd_df1_inst_q15* filterPtr = &m_filter;
   uint8_t i = 0;
   for (; i < length; i++) {
     q15_t currentSample = samples[i];//save to a local variable to avoid indirection on every access
@@ -98,7 +93,8 @@ void CFM::samples(q15_t* samples, uint8_t length)
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
       currentSample += m_timeoutTone.getAudio();
 
-    currentSample = filter(currentSample);
+    //currentSample = filter(currentSample);
+    arm_biquad_cascade_df1_fast_q15(filterPtr, samples +i, &currentSample, 1);
 
     currentSample += m_ctcssTX.getAudio();
 
@@ -394,26 +390,3 @@ void CFM::beginRelaying()
   m_timeoutTimer.start();
   m_ackMinTimer.start();
 }
-
-q15_t CFM::filter(q15_t sample)
-{
-  q15_t output = 0;
-
-  m_filterBuffer[m_filterPosition] = sample;
-
-  uint8_t iTaps = 0U;
-
-  for (int8_t i = m_filterPosition; i >= 0; i--) {
-    q31_t temp = FILTER_COEFFS[iTaps++] * m_filterBuffer[i];
-    output += q15_t(__SSAT((temp >> 15), 16));
-  }
-
-  for (int8_t i = FILTER_COEFFS_LEN - 1; i >= m_filterPosition; i--) {
-    q31_t temp = FILTER_COEFFS[iTaps++] * m_filterBuffer[i];
-    output += q15_t(__SSAT((temp >> 15), 16));
-  }
-
-  m_filterPosition = (m_filterPosition + 1U) % FILTER_COEFFS_LEN;
-
-  return output;
-}
diff --git a/FM.h b/FM.h
index d2f9419..a125a87 100644
--- a/FM.h
+++ b/FM.h
@@ -37,6 +37,9 @@ enum FM_STATE {
   FS_HANG
 };
 
+
+
+
 class CFM {
 public:
   CFM();
@@ -52,8 +55,6 @@ public:
   uint8_t setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFrequency, uint8_t ctcssThreshold, uint8_t ctcssLevel, uint8_t kerchunkTime, uint8_t hangTime);
 
 private:
-  q15_t*               m_filterBuffer;
-  uint8_t              m_filterPosition;
   CFMKeyer             m_callsign;
   CFMKeyer             m_rfAck;
   CFMCTCSSRX           m_ctcssRX;
@@ -69,6 +70,8 @@ private:
   CFMTimer             m_ackMinTimer;
   CFMTimer             m_ackDelayTimer;
   CFMTimer             m_hangTimer;
+  arm_biquad_casd_df1_inst_q15 m_filter;
+  q15_t                m_filterState[8];//must be filterOrder * 4 long
 
   void stateMachine(bool validSignal, uint8_t length);
   void listeningState(bool validSignal);

From 4f9d5a8c70d9a8c84563471cd0fe5adae0fa226e Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Thu, 23 Apr 2020 22:37:29 +0200
Subject: [PATCH 08/10] 3rd order IIR

---
 FM.cpp | 13 ++++++++-----
 FM.h   |  2 +-
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/FM.cpp b/FM.cpp
index 1c70b40..1ec448b 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -20,9 +20,13 @@
 #include "Globals.h"
 #include "FM.h"
 
-// 2 stage IIR Butterworth filter generated using https://github.com/F4FXL/iir_fixed_point/blob/4f1e580a7dad9f8742d24a06edd14b62110ba6e4/gen_coeff.py
-q15_t FILTER_COEFFS[] = {1105,  2210, 1105, 16384,-19003,7512, 14,//1st stage
-                         16384,-32768,16384,16384,-31020,14751,14};//2nd stage
+// 3 stage IIR Butterworth filter generated (if you change the order change the size of m_filterState). Also change the ordre in init call below
+// 0.2db band pass ripple
+// 300 - 2700Hz
+q15_t FILTER_COEFFS[] = {  362,   724,    362,16384,-18947,10676, 14,//1st stage
+                         16384,     0, -16384,16384,-25170, 9526, 14,//2nd stage
+                         16384,-32768,  16384,16384,-32037,15730,14};//3rd stage
+
 
 CFM::CFM() :
 m_callsign(),
@@ -42,7 +46,7 @@ m_ackDelayTimer(),
 m_hangTimer(),
 m_filter()
 {
-  arm_biquad_cascade_df1_init_q15(&m_filter, 2, FILTER_COEFFS, m_filterState, 0);
+  arm_biquad_cascade_df1_init_q15(&m_filter, 3, FILTER_COEFFS, m_filterState, 0);
 }
 
 void CFM::samples(q15_t* samples, uint8_t length)
@@ -93,7 +97,6 @@ void CFM::samples(q15_t* samples, uint8_t length)
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
       currentSample += m_timeoutTone.getAudio();
 
-    //currentSample = filter(currentSample);
     arm_biquad_cascade_df1_fast_q15(filterPtr, samples +i, &currentSample, 1);
 
     currentSample += m_ctcssTX.getAudio();
diff --git a/FM.h b/FM.h
index a125a87..474f36d 100644
--- a/FM.h
+++ b/FM.h
@@ -71,7 +71,7 @@ private:
   CFMTimer             m_ackDelayTimer;
   CFMTimer             m_hangTimer;
   arm_biquad_casd_df1_inst_q15 m_filter;
-  q15_t                m_filterState[8];//must be filterOrder * 4 long
+  q15_t                m_filterState[12];//must be filterOrder * 4 long
 
   void stateMachine(bool validSignal, uint8_t length);
   void listeningState(bool validSignal);

From 5c2659deaa21cdb22797ca553cab53ea14910d0b Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Fri, 24 Apr 2020 09:10:50 +0200
Subject: [PATCH 09/10] Add FM filtering

---
 FM.cpp                          |  14 ++--
 FM.h                            |   6 +-
 FMDirectForm1.h                 | 114 ++++++++++++++++++++++++++++++++
 FMGenerateFilterCoefficients.py |  45 +++++++++++++
 4 files changed, 170 insertions(+), 9 deletions(-)
 create mode 100644 FMDirectForm1.h
 create mode 100644 FMGenerateFilterCoefficients.py

diff --git a/FM.cpp b/FM.cpp
index 1ec448b..03aa6ec 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -23,9 +23,9 @@
 // 3 stage IIR Butterworth filter generated (if you change the order change the size of m_filterState). Also change the ordre in init call below
 // 0.2db band pass ripple
 // 300 - 2700Hz
-q15_t FILTER_COEFFS[] = {  362,   724,    362,16384,-18947,10676, 14,//1st stage
-                         16384,     0, -16384,16384,-25170, 9526, 14,//2nd stage
-                         16384,-32768,  16384,16384,-32037,15730,14};//3rd stage
+q15_t FILTER_COEFFS[] = {362,724,362,16384,-18947,10676,
+                          16384,0,-16384,16384,-25170,9526,
+                          16384,-32768,16384,16384,-32037,15730};
 
 
 CFM::CFM() :
@@ -44,14 +44,14 @@ m_kerchunkTimer(),
 m_ackMinTimer(),
 m_ackDelayTimer(),
 m_hangTimer(),
-m_filter()
+m_filterStage1(  724,   1448,   724, 32768, -37895, 21352),
+m_filterStage2(32768,      0,-32768, 32768, -50339, 19052),
+m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460)
 {
-  arm_biquad_cascade_df1_init_q15(&m_filter, 3, FILTER_COEFFS, m_filterState, 0);
 }
 
 void CFM::samples(q15_t* samples, uint8_t length)
 {
-  arm_biquad_casd_df1_inst_q15* filterPtr = &m_filter;
   uint8_t i = 0;
   for (; i < length; i++) {
     q15_t currentSample = samples[i];//save to a local variable to avoid indirection on every access
@@ -97,7 +97,7 @@ void CFM::samples(q15_t* samples, uint8_t length)
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
       currentSample += m_timeoutTone.getAudio();
 
-    arm_biquad_cascade_df1_fast_q15(filterPtr, samples +i, &currentSample, 1);
+    currentSample = q15_t(m_filterStage3.filter(m_filterStage2.filter(m_filterStage1.filter(currentSample))));
 
     currentSample += m_ctcssTX.getAudio();
 
diff --git a/FM.h b/FM.h
index 474f36d..77a3d81 100644
--- a/FM.h
+++ b/FM.h
@@ -26,6 +26,7 @@
 #include "FMTimeout.h"
 #include "FMKeyer.h"
 #include "FMTimer.h"
+#include "FMDirectForm1.h"
 
 enum FM_STATE {
   FS_LISTENING,
@@ -70,8 +71,9 @@ private:
   CFMTimer             m_ackMinTimer;
   CFMTimer             m_ackDelayTimer;
   CFMTimer             m_hangTimer;
-  arm_biquad_casd_df1_inst_q15 m_filter;
-  q15_t                m_filterState[12];//must be filterOrder * 4 long
+  CFMDirectFormI          m_filterStage1;
+  CFMDirectFormI          m_filterStage2;
+  CFMDirectFormI          m_filterStage3;
 
   void stateMachine(bool validSignal, uint8_t length);
   void listeningState(bool validSignal);
diff --git a/FMDirectForm1.h b/FMDirectForm1.h
new file mode 100644
index 0000000..857e0a6
--- /dev/null
+++ b/FMDirectForm1.h
@@ -0,0 +1,114 @@
+
+/*******************************************************************************
+This header file has been taken from:
+"A Collection of Useful C++ Classes for Digital Signal Processing"
+By Vinnie Falco 
+Bernd Porr adapted it for Linux and turned it into a filter using
+fixed point arithmetic.
+--------------------------------------------------------------------------------
+License: MIT License (http://www.opensource.org/licenses/mit-license.php)
+Copyright (c) 2009 by Vinnie Falco
+Copyright (C) 2013-2017, Bernd Porr, mail@berndporr.me.uk
+Copyright (C) 2020 , Mario Molitor , mario_molitor@web.de
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*******************************************************************************/
+
+// based on https://raw.githubusercontent.com/berndporr/iir_fixed_point/master/DirectFormI.h
+
+#include "Globals.h"
+
+#ifndef DIRECTFORMI_HPP_
+#define DIRECTFORMI_HPP_
+class CFMDirectFormI
+{
+public:
+
+	// convenience function which takes the a0 argument but ignores it!
+	CFMDirectFormI(const q31_t b0, const q31_t b1, const q31_t b2,
+			const q31_t, const q31_t a1, const q31_t a2)
+	{
+		// FIR coefficients
+		c_b0 = b0;
+		c_b1 = b1;
+		c_b2 = b2;
+		// IIR coefficients
+		c_a1 = a1;
+		c_a2 = a2;
+		reset();
+	}
+
+	CFMDirectFormI(const CFMDirectFormI &my)
+	{
+	// delay line
+		m_x2 = my.m_x2; // x[n-2]
+		m_y2 = my.m_y2; // y[n-2]
+		m_x1 = my.m_x1; // x[n-1]
+		m_y1 = my.m_y1; // y[n-1]
+
+	// coefficients
+		c_b0 = my.c_b0;
+		c_b1 = my.c_b1;
+		c_b2 = my.c_b2; // FIR
+		c_a1 = my.c_a1;
+		c_a2 = my.c_a2; // IIR
+
+	// scaling factor
+		q_scaling = my.q_scaling; // 2^q_scaling
+    }
+
+	void reset ()
+	{
+		m_x1 = 0;
+		m_x2 = 0;
+		m_y1 = 0;
+		m_y2 = 0;
+	}
+
+	// filtering operation: one sample in and one out
+	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
+									+ c_b1 * m_x1
+									+ c_b2 * m_x2
+									- c_a1 * m_y1
+									- c_a2 * m_y2;
+
+		q15_t out = __SSAT(out_upscaled >> 15, 15);
+
+		// update the delay lines
+		m_x2 = m_x1;
+		m_y2 = m_y1;
+		m_x1 = in;
+		m_y1 = out;
+
+		return out;
+	}
+
+private:
+	// delay line
+	q31_t m_x2; // x[n-2]
+	q31_t m_y2; // y[n-2]
+	q31_t m_x1; // x[n-1]
+	q31_t m_y1; // y[n-1]
+
+	// coefficients
+	q31_t c_b0,c_b1,c_b2; // FIR
+	q31_t c_a1,c_a2; // IIR
+};
+
+#endif /* DIRECTFORMI_HPP_ */
\ No newline at end of file
diff --git a/FMGenerateFilterCoefficients.py b/FMGenerateFilterCoefficients.py
new file mode 100644
index 0000000..84ea62b
--- /dev/null
+++ b/FMGenerateFilterCoefficients.py
@@ -0,0 +1,45 @@
+# based on https://github.com/berndporr/iir_fixed_point/blob/master/gen_coeff.py
+
+import numpy as np
+import scipy.signal as signal
+import pylab as pl
+
+# Calculate the coefficients for a pure fixed point
+# integer filter
+
+# sampling rate
+fs = 24000
+
+# cutoffs
+f1 = 300
+f2 = 2700
+# ripple
+rp = 0.2
+
+# scaling factor in bits, do not change !
+q = 15
+# scaling factor as facor...
+scaling_factor = 2**q
+
+# let's generate a sequence of 2nd order IIR filters
+#sos = signal.butter(2,[f1/fs*2,f2/fs*2],'pass',output='sos')
+sos = signal.cheby1(3,rp,[f1/fs*2,f2/fs*2],'bandpass', output='sos')
+
+sos = np.round((sos) * scaling_factor)
+
+# print coefficients
+for biquad in sos:
+    for coeff in biquad:
+        print(int(coeff),",",sep="",end="")
+        #print((coeff),",",sep="",end="")
+    print("")
+
+# plot the frequency response
+b,a = signal.sos2tf(sos)
+w,h = signal.freqz(b,a)
+pl.plot(w/np.pi/2*fs,20*np.log(np.abs(h)))
+pl.xlabel('frequency/Hz');
+pl.ylabel('gain/dB');
+pl.ylim(top=1,bottom=-20);
+pl.xlim(left=250, right=12000);
+pl.show()
\ No newline at end of file

From 2057ef5206d43d80d3317b793cb0196390c45678 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Fri, 24 Apr 2020 09:20:26 +0200
Subject: [PATCH 10/10] clean up

---
 FM.cpp          | 8 --------
 FMDirectForm1.h | 9 +++------
 2 files changed, 3 insertions(+), 14 deletions(-)

diff --git a/FM.cpp b/FM.cpp
index 03aa6ec..019e27e 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -20,14 +20,6 @@
 #include "Globals.h"
 #include "FM.h"
 
-// 3 stage IIR Butterworth filter generated (if you change the order change the size of m_filterState). Also change the ordre in init call below
-// 0.2db band pass ripple
-// 300 - 2700Hz
-q15_t FILTER_COEFFS[] = {362,724,362,16384,-18947,10676,
-                          16384,0,-16384,16384,-25170,9526,
-                          16384,-32768,16384,16384,-32037,15730};
-
-
 CFM::CFM() :
 m_callsign(),
 m_rfAck(),
diff --git a/FMDirectForm1.h b/FMDirectForm1.h
index 857e0a6..4b90245 100644
--- a/FMDirectForm1.h
+++ b/FMDirectForm1.h
@@ -31,8 +31,8 @@ THE SOFTWARE.
 
 #include "Globals.h"
 
-#ifndef DIRECTFORMI_HPP_
-#define DIRECTFORMI_HPP_
+#ifndef DIRECTFORMI_H_
+#define DIRECTFORMI_H_
 class CFMDirectFormI
 {
 public:
@@ -65,9 +65,6 @@ public:
 		c_b2 = my.c_b2; // FIR
 		c_a1 = my.c_a1;
 		c_a2 = my.c_a2; // IIR
-
-	// scaling factor
-		q_scaling = my.q_scaling; // 2^q_scaling
     }
 
 	void reset ()
@@ -111,4 +108,4 @@ private:
 	q31_t c_a1,c_a2; // IIR
 };
 
-#endif /* DIRECTFORMI_HPP_ */
\ No newline at end of file
+#endif /* DIRECTFORMI_H */
\ No newline at end of file