From 57730d7f81fd2316cf0f3a6f4e5848589da8a597 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Wed, 22 Apr 2020 14:37:23 +0100
Subject: [PATCH] Implement an efficient FIR filter.

---
 FM.cpp | 36 +++++++++++++++++++++++++++---------
 FM.h   |  6 ++++--
 2 files changed, 31 insertions(+), 11 deletions(-)

diff --git a/FM.cpp b/FM.cpp
index aa484c3..9073649 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -30,8 +30,8 @@ const uint16_t FILTER_COEFFS_LEN = 51U;
 
 
 CFM::CFM() :
-m_filter(),
-m_filterState(),
+m_filterBuffer(NULL),
+m_filterPosition(0U),
 m_callsign(),
 m_rfAck(),
 m_ctcssRX(),
@@ -48,11 +48,7 @@ m_ackMinTimer(),
 m_ackDelayTimer(),
 m_hangTimer()
 {
-  ::memset(m_filterState, 0x00U, 230U * sizeof(q15_t));
-
-  m_filter.numTaps = FILTER_COEFFS_LEN;
-  m_filter.pState  = m_filterState;
-  m_filter.pCoeffs = FILTER_COEFFS;
+  m_filterBuffer = new q15_t[FILTER_COEFFS_LEN];
 }
 
 void CFM::samples(q15_t* samples, uint8_t length)
@@ -98,8 +94,7 @@ void CFM::samples(q15_t* samples, uint8_t length)
     if (!m_callsign.isRunning() && !m_rfAck.isRunning())
       currentSample += m_timeoutTone.getAudio();
 
-    //ToDo Filtering
-    //::arm_fir_fast_q15(&m_filter, samples + i, &currentSample, 1);
+    currentSample = filter(currentSample);
 
     currentSample += m_ctcssTX.getAudio();
 
@@ -395,3 +390,26 @@ 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 8fd4bde..b348ee9 100644
--- a/FM.h
+++ b/FM.h
@@ -52,8 +52,8 @@ 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:
-  arm_fir_instance_q15 m_filter;
-  q15_t                m_filterState[130U];           // NoTaps + BlockSize - 1, 101 + 20 - 1 plus some spare
+  q15_t*               m_filterBuffer;
+  uint8_t              m_filterPosition;
   CFMKeyer             m_callsign;
   CFMKeyer             m_rfAck;
   CFMCTCSSRX           m_ctcssRX;
@@ -81,6 +81,8 @@ private:
 
   void sendCallsign();
   void beginRelaying();
+
+  q15_t filter(q15_t sample);
 };
 
 #endif