From 63847ebe626f14ad631328a05d16a75176caab41 Mon Sep 17 00:00:00 2001 From: Geoffrey Merck Date: Wed, 22 Apr 2020 11:04:27 +0200 Subject: [PATCH] One loop to rule them all ! Change CFM::Samples and CFMCTCSSRX::process to be able to process all stuff in one and only loop. --- FM.cpp | 39 +++++++++++++++++++++++++++++++-------- FMCTCSSRX.cpp | 40 ++++++++++++++++++++++++---------------- FMCTCSSRX.h | 43 ++++++++++++++++++++++++++++++++++++------- 3 files changed, 91 insertions(+), 31 deletions(-) diff --git a/FM.cpp b/FM.cpp index f3a8fa5..a34dc61 100644 --- a/FM.cpp +++ b/FM.cpp @@ -60,17 +60,39 @@ m_hangTimer() void CFM::samples(q15_t* samples, uint8_t length) { - bool validCTCSS = m_ctcssRX.process(samples, length); - - stateMachine(validCTCSS, length); - - if (m_modemState != STATE_FM) - return; + CTCSSState ctcssState; + bool validCTCSS = false; q15_t currentSample; - for (uint8_t i = 0U; i < length; i++) { + uint8_t i = 0; + + for (; i < length; i++) { currentSample = samples[i];//save to a local variable to avoid indirection on every access + ctcssState = m_ctcssRX.process(currentSample); + + if(CTCSS_NOT_READY(ctcssState) && m_modemState != STATE_FM) { + //Not enough samples to determine if you have CTCSS, just carry on + continue; + } else if(CTCSS_READY(ctcssState) && m_modemState != STATE_FM) { + //we had enough samples for CTCSS and we are in some other mode than FM + validCTCSS = CTCSS_VALID(ctcssState); + stateMachine(validCTCSS, i + 1U); + if (m_modemState != STATE_FM) + continue; + } else if(CTCSS_READY(ctcssState) && m_modemState == STATE_FM) { + //We had enough samples for CTCSS and we are in FM mode, trigger the state machine + validCTCSS = CTCSS_VALID(ctcssState); + stateMachine(validCTCSS, i + 1U); + if (m_modemState != STATE_FM) + break; + } else if(CTCSS_NOT_READY(ctcssState) && m_modemState == STATE_FM && i == length - 1) { + //Not enough samples for CTCSS but we already are in FM, trigger the state machine + //but do not trigger the state machine on every single sample, save CPU! + validCTCSS = CTCSS_VALID(ctcssState); + stateMachine(validCTCSS, i + 1U); + } + // Only let audio through when relaying audio if (m_state != FS_RELAYING && m_state != FS_KERCHUNK) currentSample = 0U; @@ -92,7 +114,8 @@ void CFM::samples(q15_t* samples, uint8_t length) samples[i] = currentSample; } - io.write(STATE_FM, samples, length); + if(m_modemState == STATE_FM) + io.write(STATE_FM, samples, i);//only write the actual number of processed samples to IO } void CFM::process() diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp index 84fcc3c..7f24333 100644 --- a/FMCTCSSRX.cpp +++ b/FMCTCSSRX.cpp @@ -86,7 +86,7 @@ m_threshold(0.0F), m_count(0U), m_q0(0.0F), m_q1(0.0F), -m_result(false) +m_result((CTCSSState)0U) { } @@ -107,31 +107,39 @@ uint8_t CFMCTCSSRX::setParams(uint8_t frequency, uint8_t threshold) return 0U; } -bool CFMCTCSSRX::process(q15_t* samples, uint8_t length) +CTCSSState CFMCTCSSRX::process(q15_t sample) { - for (unsigned int i = 0U; i < length; i++) { - float32_t q2 = m_q1; - m_q1 = m_q0; - m_q0 = m_coeff * m_q1 - q2 + float32_t(samples[i]) / 32768.0F; + m_result = m_result & (~CTS_READY); + float32_t q2 = m_q1; + m_q1 = m_q0; + m_q0 = m_coeff * m_q1 - q2 + float32_t(sample) / 32768.0F; - m_count++; - if (m_count == N) { - float32_t value = m_q0 * m_q0 + m_q1 * m_q1 - m_q0 * m_q1 * m_coeff; - m_result = value >= m_threshold; - //DEBUG4("CTCSS value / threshold / result", value, m_threshold, m_result); - m_count = 0U; - m_q0 = 0.0F; - m_q1 = 0.0F; - } + m_count++; + if (m_count == N) { + float32_t value = m_q0 * m_q0 + m_q1 * m_q1 - m_q0 * m_q1 * m_coeff; + m_result = m_result | CTS_READY; + if(value >= m_threshold) + m_result = m_result | CTS_VALID; + else + m_result = m_result & ~CTS_VALID; + + m_count = 0U; + m_q0 = 0.0F; + m_q1 = 0.0F; } return m_result; } +CTCSSState CFMCTCSSRX::getState() +{ + return m_result; +} + void CFMCTCSSRX::reset() { m_q0 = 0.0F; m_q1 = 0.0F; - m_result = false; + m_result = (CTCSSState)0U; m_count = 0U; } diff --git a/FMCTCSSRX.h b/FMCTCSSRX.h index b1d7c02..5238d7b 100644 --- a/FMCTCSSRX.h +++ b/FMCTCSSRX.h @@ -21,23 +21,52 @@ #include "Config.h" +enum CTCSSState : uint8_t +{ + CTS_READY = 1, + CTS_VALID = 2, + CTS_READY_VALID = CTS_READY | CTS_VALID +}; + +inline CTCSSState operator|(CTCSSState a, CTCSSState b) +{ + return static_cast(static_cast(a) | static_cast(b)); +} + +inline CTCSSState operator&(CTCSSState a, CTCSSState b) +{ + return static_cast(static_cast(a) & static_cast(b)); +} + +inline CTCSSState operator~(CTCSSState a) +{ + return static_cast(~(static_cast(a))); +} + +#define CTCSS_READY(a) ((a & CTS_READY) != 0) +#define CTCSS_NOT_READY(a) ((a & CTS_READY) == 0) +#define CTCSS_VALID(a) ((a & CTS_VALID) != 0) +#define CTCSS_NOT_VALID(a) ((a & CTS_VALID) == 0) + class CFMCTCSSRX { public: CFMCTCSSRX(); uint8_t setParams(uint8_t frequency, uint8_t threshold); + + CTCSSState process(q15_t samples); - bool process(q15_t* samples, uint8_t length); + CTCSSState getState(); void reset(); private: - float32_t m_coeff; - float32_t m_threshold; - uint16_t m_count; - float32_t m_q0; - float32_t m_q1; - bool m_result; + float32_t m_coeff; + float32_t m_threshold; + uint16_t m_count; + float32_t m_q0; + float32_t m_q1; + CTCSSState m_result; }; #endif