From 0a5c496a3aec65da14a83d6ac223a7f2dc0b798b Mon Sep 17 00:00:00 2001
From: Geoffrey Merck <F4FXL@users.noreply.github.com>
Date: Fri, 1 May 2020 17:56:45 +0200
Subject: [PATCH] Fix CTCSS value is different when using RXLevel 50 to 99

---
 FMCTCSSRX.cpp | 16 +++++++++++++++-
 FMCTCSSRX.h   |  2 ++
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp
index 9e81d3cc..f3f9047e 100644
--- a/FMCTCSSRX.cpp
+++ b/FMCTCSSRX.cpp
@@ -93,7 +93,7 @@ m_rxLevelInverse(1)
 
 uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold, uint8_t level)
 {
-  m_rxLevelInverse = 511 / q15_t(level);
+  m_rxLevelInverse = q15Division(65535, q15_t(level * 128));
 
   m_coeffDivTwo = 0;
 
@@ -179,3 +179,17 @@ void CFMCTCSSRX::reset()
   m_result = CTS_NONE;
   m_count  = 0U;
 }
+
+//Taken from https://en.wikipedia.org/wiki/Q_(number_format)#Division
+q15_t CFMCTCSSRX::q15Division(q15_t a, q15_t divisor)
+{
+  q31_t a31 = q31_t(a) << 16;
+
+  if (((a >> 31) & 1) == ((divisor >> 15) & 1))
+    a31 += divisor >> 1;
+  else
+    a31 -= divisor >> 1;
+
+  return a31 / divisor;
+}
+
diff --git a/FMCTCSSRX.h b/FMCTCSSRX.h
index 4c1dfc3a..b5fea60f 100644
--- a/FMCTCSSRX.h
+++ b/FMCTCSSRX.h
@@ -62,6 +62,8 @@ class CFMCTCSSRX {
   void reset();
 
 private:
+  q15_t q15Division(q15_t a, q15_t divisor);
+
   q63_t      m_coeffDivTwo;
   q31_t      m_threshold;
   uint16_t   m_count;