From 77e0deee76d5a2a7cac9078cb1f767af541e125c Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Fri, 8 May 2020 09:01:00 +0200
Subject: [PATCH] Make whole FM chain RX level independant

---
 FM.cpp        | 11 ++++++++---
 FM.h          |  1 +
 FMCTCSSRX.cpp | 23 ++++-------------------
 FMCTCSSRX.h   |  5 +----
 4 files changed, 14 insertions(+), 26 deletions(-)

diff --git a/FM.cpp b/FM.cpp
index 70b6b6a..ca09af9 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -46,7 +46,8 @@ m_blanking(),
 m_useCOS(true),
 m_cosInvert(false),
 m_rfAudioBoost(1U),
-m_downsampler(128)//Size might need adjustement
+m_downsampler(128),//Size might need adjustement
+m_rxLevel(1)
 {
 }
 
@@ -63,7 +64,8 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 
   uint8_t i = 0U;
   for (; i < length; i++) {
-    q15_t currentSample = samples[i];//save to a local variable to avoid indirection on every access
+    // ARMv7-M has hardware integer division 
+    q15_t currentSample = q15_t((q31_t(samples[i]) << 8) / m_rxLevel);
 
     uint8_t ctcssState = m_ctcssRX.process(currentSample);
 
@@ -188,7 +190,9 @@ uint8_t CFM::setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFreque
   m_timeoutTone.setParams(timeoutLevel);
   m_blanking.setParams(maxDev, timeoutLevel);
 
-  uint8_t ret = m_ctcssRX.setParams(ctcssFrequency, ctcssThreshold, rxLevel);
+  m_rxLevel = rxLevel; //q15_t(255)/q15_t(rxLevel >> 1);
+
+  uint8_t ret = m_ctcssRX.setParams(ctcssFrequency, ctcssThreshold);
   if (ret != 0U)
     return ret;
 
@@ -438,3 +442,4 @@ void CFM::beginRelaying()
   m_timeoutTimer.start();
   m_ackMinTimer.start();
 }
+
diff --git a/FM.h b/FM.h
index 9e15c8a..fc528af 100644
--- a/FM.h
+++ b/FM.h
@@ -84,6 +84,7 @@ private:
   bool                 m_cosInvert;
   q15_t                m_rfAudioBoost;
   CFMDownsampler       m_downsampler;
+  q15_t                m_rxLevel;
 
   void stateMachine(bool validSignal);
   void listeningState(bool validSignal);
diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp
index ba0c03f..8b22fcd 100644
--- a/FMCTCSSRX.cpp
+++ b/FMCTCSSRX.cpp
@@ -86,15 +86,12 @@ m_threshold(0),
 m_count(0U),
 m_q0(0),
 m_q1(0),
-m_result(CTS_NONE),
-m_rxLevelInverse(1)
+m_result(CTS_NONE)
 {
 }
 
-uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold, uint8_t level)
+uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
 {
-  m_rxLevelInverse = q15Division(65535, q15_t(level * 128));
-
   m_coeffDivTwo = 0;
 
   for (uint8_t i = 0U; i < CTCSS_TABLE_DATA_LEN; i++) {
@@ -114,7 +111,8 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold, uint8_t leve
 
 uint8_t CFMCTCSSRX::process(q15_t sample)
 {
-  q31_t sample31 = q31_t(sample) * m_rxLevelInverse;
+  //get more dynamic into the decoder by multiplying the sample by 1.5
+  q31_t sample31 = q31_t(sample) +  (q31_t(sample) >> 1);
 
   m_result &= ~CTS_READY;
 
@@ -174,16 +172,3 @@ void CFMCTCSSRX::reset()
   m_result = CTS_NONE;
   m_count  = 0U;
 }
-
-//Taken from https://en.wikipedia.org/wiki/Q_(number_format)#Division
-q15_t CFMCTCSSRX::q15Division(q15_t a, q15_t divisor)
-{
-  q31_t a31 = q31_t(a) << 16;
-
-  if (((a >> 31) & 1) == ((divisor >> 15) & 1))
-    a31 += divisor >> 1;
-  else
-    a31 -= divisor >> 1;
-
-  return a31 / divisor;
-}
diff --git a/FMCTCSSRX.h b/FMCTCSSRX.h
index 2caa9e6..6b56ba4 100644
--- a/FMCTCSSRX.h
+++ b/FMCTCSSRX.h
@@ -34,7 +34,7 @@ class CFMCTCSSRX {
 public:
   CFMCTCSSRX();
 
-  uint8_t setParams(uint8_t frequency, uint8_t threshold, uint8_t level);
+  uint8_t setParams(uint8_t frequency, uint8_t threshold);
   
   uint8_t process(q15_t sample);
 
@@ -47,9 +47,6 @@ private:
   q31_t    m_q0;
   q31_t    m_q1;
   uint8_t  m_result;
-  q15_t    m_rxLevelInverse;
-
-  q15_t q15Division(q15_t a, q15_t divisor);
 };
 
 #endif