Move division to CTCSSRX, change sample unscaling to 1/rxLevel

FM.cpp

@@ -55,7 +55,7 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length)
   for (; i < length; i++) {
     q15_t currentSample = samples[i];//save to a local variable to avoid indirection on every access
 
-    CTCSSState ctcssState = m_ctcssRX.process(getUnscaledSample(currentSample));
+    CTCSSState ctcssState = m_ctcssRX.process(currentSample);
 
     if (CTCSS_NOT_READY(ctcssState) && m_modemState != STATE_FM) {
       //Not enough samples to determine if you have CTCSS, just carry on
@@ -158,7 +158,7 @@ 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);
+  uint8_t ret = m_ctcssRX.setParams(ctcssFrequency, ctcssThreshold, m_rxLevel);
   if (ret != 0U)
     return ret;
 
@@ -400,17 +400,4 @@ void CFM::beginRelaying()
   m_ackMinTimer.start();
 }
 
-q15_t CFM::getUnscaledSample(q15_t sample)
-{
-  // sample / rxLevel
-  q31_t sample31 = q31_t(sample) << 16;
 
-  if (((sample31 >> 31) & 1) == ((m_rxLevel >> 15) & 1))
-    sample31 += m_rxLevel >> 1;
-  else
-    sample31 -= m_rxLevel >> 1;
-
-  sample31 /= m_rxLevel;
-
-  return q15_t(sample31);
-}

FM.h

@@ -91,8 +91,6 @@ private:
 
   void sendCallsign();
   void beginRelaying();
-
-  q15_t getUnscaledSample(q15_t sample);
 };
 
 #endif

FMCTCSSRX.cpp

@@ -86,12 +86,16 @@ m_threshold(0),
 m_count(0U),
 m_q0(0),
 m_q1(0),
-m_result(CTS_NONE)
+m_result(CTS_NONE),
+m_rxLevelInverse(1)
 {
 }
 
-uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
+uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold, q15_t rxLevel)
 {
+  // Calculate 1/rxLevel
+  m_rxLevelInverse = q31_t(q15Division(65535 /* This value should be 32767 (q15 1). But this does not work.*/, rxLevel));
+
   m_coeffDivTwo = 0;
 
   for (uint8_t i = 0U; i < CTCSS_TABLE_DATA_LEN; i++) {
@@ -111,6 +115,8 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
 
 CTCSSState CFMCTCSSRX::process(q15_t sample)
 {
+  q63_t sample31 = q31_t(sample) * m_rxLevelInverse;
+
   m_result = m_result & (~CTS_READY); 
 
   q31_t q2 = m_q1;
@@ -122,7 +128,7 @@ CTCSSState CFMCTCSSRX::process(q15_t sample)
   q31_t t3 = t2 * 2;
 
   // m_q0 = m_coeffDivTwo * m_q1 * 2 - q2 + sample
-  m_q0 = t3 - q2 + q31_t(sample);
+  m_q0 = t3 - q2 + sample31;
 
   m_count++;
   if (m_count == N) {

FMCTCSSRX.h

@@ -53,7 +53,7 @@ class CFMCTCSSRX {
 public:
   CFMCTCSSRX();
 
-  uint8_t setParams(uint8_t frequency, uint8_t threshold);
+  uint8_t setParams(uint8_t frequency, uint8_t threshold, q15_t rxLevel);
   
   CTCSSState process(q15_t sample);
 
@@ -62,12 +62,25 @@ public:
   void reset();
 
 private:
- q63_t      m_coeffDivTwo;
+  static inline q15_t q15Division(q15_t a, q15_t divisor)
- q31_t      m_threshold;
+  {
- uint16_t   m_count;
+    q31_t a31 = q31_t(a) << 16;
- q31_t      m_q0;
+
- q31_t      m_q1;
+    if (((a >> 31) & 1) == ((divisor >> 15) & 1))
- CTCSSState m_result;
+      a31 += divisor >> 1;
+    else
+      a31 -= divisor >> 1;
+
+    return a31 / divisor;
+  }
+
+  q63_t      m_coeffDivTwo;
+  q31_t      m_threshold;
+  uint16_t   m_count;
+  q31_t      m_q0;
+  q31_t      m_q1;
+  CTCSSState m_result;
+  q31_t m_rxLevelInverse;
 };
 
 #endif