From 6d5b864cbaa1ce7d14291d460202bada71ba71c1 Mon Sep 17 00:00:00 2001
From: Jonathan Naylor <naylorjs@yahoo.com>
Date: Sat, 18 Apr 2020 16:07:52 +0100
Subject: [PATCH] Start the Goertzel development.

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

diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp
index b945ea4..c2a28df 100644
--- a/FMCTCSSRX.cpp
+++ b/FMCTCSSRX.cpp
@@ -20,12 +20,83 @@
 #include "Globals.h"
 #include "FMCTCSSRX.h"
 
-CFMCTCSSRX::CFMCTCSSRX()
+const struct CTCSS_TABLE {
+  uint8_t frequency;
+  q31_t   coeffDivTwo;
+} CTCSS_TABLE_DATA[] = {
+  { 67U, 2147475280},
+  { 69U, 2147474696},
+  { 71U, 2147474012},
+  { 74U, 2147473330},
+  { 77U, 2147472596},
+  { 79U, 2147471807},
+  { 82U, 2147470961},
+  { 85U, 2147470053},
+  { 88U, 2147469048},
+  { 91U, 2147468042},
+  { 94U, 2147466895},
+  { 97U, 2147465964},
+  {100U, 2147465007},
+  {103U, 2147463679},
+  {107U, 2147462226},
+  {110U, 2147460722},
+  {114U, 2147459081},
+  {118U, 2147457339},
+  {123U, 2147455446},
+  {127U, 2147453440},
+  {131U, 2147451266},
+  {136U, 2147448916},
+  {141U, 2147446430},
+  {146U, 2147443804},
+  {151U, 2147440919},
+  {156U, 2147437875},
+  {159U, 2147436046},
+  {162U, 2147434605},
+  {165U, 2147432590},
+  {167U, 2147431098},
+  {171U, 2147428948},
+  {173U, 2147427340},
+  {177U, 2147425049},
+  {179U, 2147423318},
+  {183U, 2147420879},
+  {186U, 2147419018},
+  {189U, 2147416424},
+  {192U, 2147414356},
+  {196U, 2147411597},
+  {199U, 2147409456},
+  {203U, 2147406451},
+  {206U, 2147404158},
+  {210U, 2147400892},
+  {218U, 2147394977},
+  {225U, 2147388689},
+  {229U, 2147385807},
+  {233U, 2147381925},
+  {241U, 2147374659},
+  {250U, 2147366861},
+  {254U, 2147363288}};
+
+const uint8_t CTCSS_TABLE_DATA_LEN = 50U;
+
+CFMCTCSSRX::CFMCTCSSRX() :
+m_coeffDivTwo(0),
+m_threshold(0U)
 {
 }
 
 uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold)
 {
+  for (uint8_t i = 0U; i < CTCSS_TABLE_DATA_LEN; i++) {
+    if (CTCSS_TABLE_DATA[i].frequency == frequency) {
+      m_coeffDivTwo = CTCSS_TABLE_DATA[i].coeffDivTwo;
+      break;
+    }
+  }
+
+  if (m_coeffDivTwo == 0)
+    return 4U;
+
+  m_threshold = threshold;
+
   return 0U;
 }
 
diff --git a/FMCTCSSRX.h b/FMCTCSSRX.h
index 34c6e3c..5e08ee6 100644
--- a/FMCTCSSRX.h
+++ b/FMCTCSSRX.h
@@ -30,6 +30,8 @@ public:
   bool process(const q15_t* samples, uint8_t length);
 
 private:
+ q31_t   m_coeffDivTwo;
+ uint8_t m_threshold;
 };
 
 #endif