Merge branch 'FM' of https://github.com/g4klx/MMDVM into FM

FMCTCSSRX.cpp

@@ -113,6 +113,15 @@ CTCSSState CFMCTCSSRX::process(q15_t sample)
 {
   m_result = m_result & (~CTS_READY);
 
+  // sample / rxLevel
+  q15_t rxLevel = io.getRxLevel();
+  q31_t sample31 = q31_t(sample) << 16;
+  if (((sample31 >> 31) & 1) == ((rxLevel >> 15) & 1))
+    sample31 += rxLevel >> 1;
+  else
+    sample31 -= rxLevel >> 1;
+  sample31 /= rxLevel;
+
   q31_t q2 = m_q1;
   m_q1 = m_q0;
 
@@ -122,7 +131,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) {
@@ -143,7 +152,6 @@ CTCSSState CFMCTCSSRX::process(q15_t sample)
 
     // value = m_q0 * m_q0 + m_q1 * m_q1 - m_q0 * m_q1 * m_coeffDivTwo * 2
     q31_t value = t2 + t4 - t9;
-
     m_result = m_result | CTS_READY;
     if (value >= m_threshold)
       m_result = m_result | CTS_VALID;

IO.cpp

@@ -592,3 +592,8 @@ bool CIO::hasLockout() const
 {
   return m_lockout;
 }
+
+q15_t CIO::getRxLevel() const
+{
+  return m_rxLevel;
+}

IO.h

@@ -56,6 +56,8 @@ public:
   
   void selfTest();
 
+  q15_t getRxLevel() const;
+
 private:
   bool                 m_started;