From d973238596176a10cb4e970f6f86968443a18063 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Sat, 25 Apr 2020 22:12:16 +0200
Subject: [PATCH 1/2] CTCSS threshold is level agnostic, experimental

(cherry picked from commit d2d50e21e1125771e28a8a3d8fc338e79ca1ae57)
---
 FMCTCSSRX.cpp | 16 ++++++++++++++--
 IO.cpp        |  5 +++++
 IO.h          |  2 ++
 3 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp
index 164af75..5e3ecd4 100644
--- a/FMCTCSSRX.cpp
+++ b/FMCTCSSRX.cpp
@@ -109,10 +109,21 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
   return 0U;
 }
 
+char bla[256];
 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 +133,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 +154,8 @@ 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;
-
+    sprintf(bla, "CTCSS Value / Threshold %d %d", value, m_threshold);
+    DEBUG1(bla);
     m_result = m_result | CTS_READY;
     if (value >= m_threshold)
       m_result = m_result | CTS_VALID;
diff --git a/IO.cpp b/IO.cpp
index a737693..afd56b0 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -592,3 +592,8 @@ bool CIO::hasLockout() const
 {
   return m_lockout;
 }
+
+q15_t CIO::getRxLevel() const
+{
+  return m_rxLevel;
+}
diff --git a/IO.h b/IO.h
index 5ad4b1d..f3ac4f9 100644
--- a/IO.h
+++ b/IO.h
@@ -56,6 +56,8 @@ public:
   
   void selfTest();
 
+  q15_t getRxLevel() const;
+
 private:
   bool                 m_started;
 

From 103116e410a6dd5098a1bd25b5c54d4d7772aeb9 Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Sun, 26 Apr 2020 07:26:35 +0200
Subject: [PATCH 2/2] Add debug essage when CTCSS status changes

---
 FMCTCSSRX.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp
index 5e3ecd4..7baadab 100644
--- a/FMCTCSSRX.cpp
+++ b/FMCTCSSRX.cpp
@@ -109,7 +109,6 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
   return 0U;
 }
 
-char bla[256];
 CTCSSState CFMCTCSSRX::process(q15_t sample)
 {
   m_result = m_result & (~CTS_READY);
@@ -154,14 +153,17 @@ 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;
-    sprintf(bla, "CTCSS Value / Threshold %d %d", value, m_threshold);
-    DEBUG1(bla);
+
+    bool previousCTCSSValid = CTCSS_VALID(m_result);
     m_result = m_result | CTS_READY;
     if (value >= m_threshold)
       m_result = m_result | CTS_VALID;
     else
       m_result = m_result & ~CTS_VALID;
 
+    if(previousCTCSSValid != CTCSS_VALID(m_result))
+      DEBUG4("CTCSS Value / Threshold / Valid", value, m_threshold, CTCSS_VALID(m_result));
+
     m_count = 0U;
     m_q0 = 0;
     m_q1 = 0;