Merge pull request #223 from F4FXL/FM

One loop to rule them all !

FM.cpp

@@ -60,17 +60,39 @@ m_hangTimer()
 
 void CFM::samples(q15_t* samples, uint8_t length)
 {
-  bool validCTCSS = m_ctcssRX.process(samples, length);
+  CTCSSState ctcssState;
-
-  stateMachine(validCTCSS, length);
-
-  if (m_modemState != STATE_FM)
-    return;
 
+  bool validCTCSS = false;
   q15_t currentSample;
-  for (uint8_t i = 0U; i < length; i++) {
+  uint8_t i = 0;
+
+  for (; i < length; i++) {
     currentSample = samples[i];//save to a local variable to avoid indirection on every access
 
+    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
+      continue;
+    } else if(CTCSS_READY(ctcssState) && m_modemState != STATE_FM) {
+      //we had enough samples for CTCSS and we are in some other mode than FM
+      validCTCSS = CTCSS_VALID(ctcssState);
+      stateMachine(validCTCSS, i + 1U);
+      if (m_modemState != STATE_FM)
+        continue;
+    } else if(CTCSS_READY(ctcssState) && m_modemState == STATE_FM) {
+      //We had enough samples for CTCSS and we are in FM mode, trigger the state machine
+      validCTCSS = CTCSS_VALID(ctcssState);
+      stateMachine(validCTCSS, i + 1U);
+      if (m_modemState != STATE_FM)
+        break;
+    } else if(CTCSS_NOT_READY(ctcssState) && m_modemState == STATE_FM && i == length - 1) {
+      //Not enough samples for CTCSS but we already are in FM, trigger the state machine
+      //but do not trigger the state machine on every single sample, save CPU!
+        validCTCSS = CTCSS_VALID(ctcssState);
+        stateMachine(validCTCSS, i + 1U);
+    }
+    
     // Only let audio through when relaying audio
     if (m_state != FS_RELAYING && m_state != FS_KERCHUNK)
       currentSample = 0U;
@@ -92,7 +114,8 @@ void CFM::samples(q15_t* samples, uint8_t length)
     samples[i] = currentSample;
   }
 
-  io.write(STATE_FM, samples, length);
+  if(m_modemState == STATE_FM)
+    io.write(STATE_FM, samples, i);//only write the actual number of processed samples to IO
 }
 
 void CFM::process()

FMCTCSSRX.cpp

@@ -86,7 +86,7 @@ m_threshold(0.0F),
 m_count(0U),
 m_q0(0.0F),
 m_q1(0.0F),
-m_result(false)
+m_result((CTCSSState)0U)
 {
 }
 
@@ -107,31 +107,39 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
   return 0U;
 }
 
-bool CFMCTCSSRX::process(q15_t* samples, uint8_t length)
+CTCSSState CFMCTCSSRX::process(q15_t sample)
 {
-  for (unsigned int i = 0U; i < length; i++) {
+  m_result = m_result & (~CTS_READY);
   float32_t q2 = m_q1;
   m_q1 = m_q0;
-    m_q0 = m_coeff * m_q1 - q2 + float32_t(samples[i]) / 32768.0F;
+  m_q0 = m_coeff * m_q1 - q2 + float32_t(sample) / 32768.0F;
 
   m_count++;
   if (m_count == N) {
     float32_t value = m_q0 * m_q0 + m_q1 * m_q1 - m_q0 * m_q1 * m_coeff;
-      m_result = value >= m_threshold;
+    m_result = m_result | CTS_READY;
-      //DEBUG4("CTCSS value / threshold / result", value, m_threshold, m_result);
+    if(value >= m_threshold)
+      m_result = m_result | CTS_VALID;
+    else
+      m_result = m_result & ~CTS_VALID;
+
     m_count = 0U;
     m_q0 = 0.0F;
     m_q1 = 0.0F;
   }
-  }
 
   return m_result;
 }
 
+CTCSSState CFMCTCSSRX::getState()
+{
+  return m_result;
+}
+
 void CFMCTCSSRX::reset()
 {
   m_q0 = 0.0F;
   m_q1 = 0.0F;
-  m_result = false;
+  m_result = (CTCSSState)0U;
   m_count  = 0U;
 }

FMCTCSSRX.h

@@ -21,13 +21,42 @@
 
 #include "Config.h"
 
+enum CTCSSState : uint8_t
+{
+  CTS_READY = 1,
+  CTS_VALID = 2,
+  CTS_READY_VALID = CTS_READY | CTS_VALID
+};
+
+inline CTCSSState operator|(CTCSSState a, CTCSSState b)
+{
+  return static_cast<CTCSSState>(static_cast<uint8_t>(a) | static_cast<uint8_t>(b));
+}
+
+inline CTCSSState operator&(CTCSSState a, CTCSSState b)
+{
+  return static_cast<CTCSSState>(static_cast<uint8_t>(a) & static_cast<uint8_t>(b));
+}
+
+inline CTCSSState operator~(CTCSSState a)
+{
+  return static_cast<CTCSSState>(~(static_cast<uint8_t>(a)));
+}
+
+#define CTCSS_READY(a) ((a & CTS_READY) != 0)
+#define CTCSS_NOT_READY(a) ((a & CTS_READY) == 0)
+#define CTCSS_VALID(a) ((a & CTS_VALID) != 0)
+#define CTCSS_NOT_VALID(a) ((a & CTS_VALID) == 0)
+
 class CFMCTCSSRX {
 public:
   CFMCTCSSRX();
 
   uint8_t setParams(uint8_t frequency, uint8_t threshold);
   
-  bool process(q15_t* samples, uint8_t length);
+  CTCSSState process(q15_t samples);
+
+  CTCSSState getState();
 
   void reset();
 
@@ -37,7 +66,7 @@ private:
  uint16_t    m_count;
  float32_t   m_q0;
  float32_t   m_q1;
- bool      m_result;
+ CTCSSState m_result;
 };
 
 #endif