diff --git a/FM.cpp b/FM.cpp
index 33938d8..ad8b5f5 100644
--- a/FM.cpp
+++ b/FM.cpp
@@ -54,7 +54,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(currentSample);
+    CTCSSState ctcssState = m_ctcssRX.process(getUnscaledSample(currentSample));
 
     if (CTCSS_NOT_READY(ctcssState) && m_modemState != STATE_FM) {
       //Not enough samples to determine if you have CTCSS, just carry on
@@ -397,3 +397,17 @@ void CFM::beginRelaying()
   m_timeoutTimer.start();
   m_ackMinTimer.start();
 }
+
+q15_t CFM::getUnscaledSample(q15_t sample)
+{
+  // 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;
+
+  return q15_t(sample31);
+}
diff --git a/FM.h b/FM.h
index ad75877..f2190d7 100644
--- a/FM.h
+++ b/FM.h
@@ -90,6 +90,8 @@ private:
 
   void sendCallsign();
   void beginRelaying();
+
+  q15_t getUnscaledSample(q15_t sample);
 };
 
 #endif
diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp
index 7baadab..57fbfbe 100644
--- a/FMCTCSSRX.cpp
+++ b/FMCTCSSRX.cpp
@@ -111,17 +111,7 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
 
 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;
-  
+  m_result = m_result & (~CTS_READY); 
 
   q31_t q2 = m_q1;
   m_q1 = m_q0;
@@ -132,7 +122,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 + sample31;
+  m_q0 = t3 - q2 + q31_t(sample);
 
   m_count++;
   if (m_count == N) {
diff --git a/FMCTCSSRX.h b/FMCTCSSRX.h
index 8d2adaa..a848cb9 100644
--- a/FMCTCSSRX.h
+++ b/FMCTCSSRX.h
@@ -55,7 +55,7 @@ public:
 
   uint8_t setParams(uint8_t frequency, uint8_t threshold);
   
-  CTCSSState process(q15_t samples);
+  CTCSSState process(q15_t sample);
 
   CTCSSState getState();