Add some delay to squelch

FMNoiseSquelch.cpp

@@ -26,26 +26,31 @@ const q31_t COEFF_DIV_TWO = 821806413;
 // 400Hz bandwidth
 const uint16_t N = 24000U / 400U;
 
+//const q63_t ALPHA = 1073741824; //0.5 as q31 but stored in a q64 to avoid overflow
+const q63_t ALPHA = 536870912; //0.25 as q31 but stored in a q64 to avoid overflow
+
 CFMNoiseSquelch::CFMNoiseSquelch() :
 m_highThreshold(0),
 m_lowThreshold(0),
 m_count(0U),
 m_q0(0),
 m_q1(0),
-m_state(false)
+m_state(false),
+m_validCount(0U)
 {
+
 }
 
 void CFMNoiseSquelch::setParams(uint8_t highThreshold, uint8_t lowThreshold)
 {
-  m_highThreshold = q31_t(highThreshold) * 20;
+  m_highThreshold = q31_t(highThreshold);
-  m_lowThreshold  = q31_t(lowThreshold) * 20;
+  m_lowThreshold  = q31_t(lowThreshold);
 }
 
 bool CFMNoiseSquelch::process(q15_t sample)
 {
   //get more dynamic into the decoder by multiplying the sample by 1.5
-  q31_t sample31 = q31_t(sample) +  (q31_t(sample) >> 1);
+  q31_t sample31 = q31_t(sample) << 6; //+  (q31_t(sample) >> 1);
 
   q31_t q2 = m_q1;
   m_q1 = m_q0;
@@ -84,10 +89,29 @@ bool CFMNoiseSquelch::process(q15_t sample)
     if (previousState)
       threshold = m_lowThreshold;
 
-    m_state = value < threshold;
+    if (!m_state) {
+      if (value < threshold)
+        m_validCount++;
+      else
+        m_validCount = 0U;
+    }
 
-    if (previousState != m_state)
+    if (m_state) {
+      if (value >= threshold)
+        m_invalidCount++;
+      else
+        m_invalidCount = 0U;
+    }
+
+    m_state = m_validCount >= 10U && m_invalidCount < 10U;
+
+    if(previousState && !m_state)
+      m_invalidCount = 0U;
+
+    if (previousState != m_state) {
       DEBUG4("Noise Squelch Value / Threshold / Valid", value, threshold, m_state);
+      DEBUG3("Valid Count / Invalid Count", m_validCount, m_invalidCount);
+    }
 
     m_count = 0U;
     m_q0 = 0;

FMNoiseSquelch.h

@@ -38,6 +38,8 @@ private:
   q31_t    m_q0;
   q31_t    m_q1;
   bool     m_state;
+  uint8_t  m_validCount;
+  uint8_t  m_invalidCount;
 };
 
 #endif