diff --git a/FM.cpp b/FM.cpp
index 0b5d86c..4528d90 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -65,11 +65,11 @@ m_hangTimer()
   m_filter.pCoeffs = FILTER_COEFFS;
 }
 
-void CFM::samples(q15_t* samples, uint8_t length)
+void CFM::samples(bool cos, q15_t* samples, uint8_t length)
 {
-  bool validSignal = m_ctcssRX.process(samples, length);
+  bool validCTCSS = m_ctcssRX.process(samples, length);
 
-  stateMachine(validSignal, length);
+  stateMachine(validCTCSS && cos, length);
 
   if (m_modemState != STATE_FM)
     return;
diff --git a/FM.h b/FM.h
index c788904..26d18d7 100644
--- a/FM.h
+++ b/FM.h
@@ -41,7 +41,7 @@ class CFM {
 public:
   CFM();
 
-  void samples(q15_t* samples, uint8_t length);
+  void samples(bool cos, q15_t* samples, uint8_t length);
 
   void process();
 
diff --git a/IO.cpp b/IO.cpp
index a26b6d1..ceae6ab 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -370,7 +370,8 @@ void CIO::process()
           FMVals[i] = q15_t(__SSAT((res1 >> 15), 16));
         }
 #endif
-        fm.samples(FMVals, RX_BLOCK_SIZE);
+        bool cos = getCOSInt();
+        fm.samples(cos, FMVals, RX_BLOCK_SIZE);
       }
     } else if (m_modemState == STATE_DSTAR) {
       if (m_dstarEnable) {
@@ -443,7 +444,8 @@ void CIO::process()
         FMVals[i] = q15_t(__SSAT((res1 >> 15), 16));
       }
 #endif
-      fm.samples(FMVals, RX_BLOCK_SIZE);
+      bool cos = getCOSInt();
+      fm.samples(cos, FMVals, RX_BLOCK_SIZE);
     } else if (m_modemState == STATE_DSTARCAL) {
       q15_t GMSKVals[RX_BLOCK_SIZE];
       ::arm_fir_fast_q15(&m_gaussianFilter, samples, GMSKVals, RX_BLOCK_SIZE);