Merge pull request #243 from F4FXL/FM

Fix CTCSS value is different when using RXLevel 50 to 99
g4klx · May 1, 2020 · b027e1d · b027e1d
2 parents 5b32fec + 0a5c496
commit b027e1d
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 1 deletion.
diff --git 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
@@ -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;