From d8f138066ea28ac33e4a9a405a67f7c8f607fe4c Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Tue, 28 Jul 2020 13:55:50 +0100 Subject: [PATCH] Add an experimental noise squelch. --- FM.cpp | 17 +++++-- FM.h | 5 +- FMNoiseSquelch.cpp | 112 +++++++++++++++++++++++++++++++++++++++++++++ FMNoiseSquelch.h | 52 +++++++++++++++++++++ SerialPort.cpp | 10 ++-- 5 files changed, 189 insertions(+), 7 deletions(-) create mode 100644 FMNoiseSquelch.cpp create mode 100644 FMNoiseSquelch.h diff --git a/FM.cpp b/FM.cpp index 09b0e9a..52aa742 100644 --- a/FM.cpp +++ b/FM.cpp @@ -39,6 +39,7 @@ m_rfAck(), m_extAck(), m_ctcssRX(), m_ctcssTX(), +m_squelch(), m_timeoutTone(), m_state(FS_LISTENING), m_callsignAtStart(false), @@ -60,6 +61,7 @@ m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460), m_blanking(), m_accessMode(1U), m_cosInvert(false), +m_noiseSquelch(false), m_rfAudioBoost(1U), m_extAudioBoost(1U), m_downSampler(400U),// 100 ms of audio @@ -87,6 +89,11 @@ void CFM::samples(bool cos, q15_t* samples, uint8_t length) // ARMv7-M has hardware integer division q15_t currentRFSample = q15_t((q31_t(samples[i]) << 8) / m_rxLevel); + if (m_noiseSquelch) { + uint8_t squelchState = m_squelch.process(currentRFSample); + cos = NSQ_VALID(squelchState); + } + q15_t currentExtSample; bool inputExt = m_inputExtRB.getSample(currentExtSample);//always consume the external input data so it does not overflow inputExt = inputExt && m_extEnabled; @@ -281,6 +288,7 @@ void CFM::reset() m_inputExtRB.reset(); m_downSampler.reset(); + m_squelch.reset(); m_needReverse = false; } @@ -313,10 +321,11 @@ uint8_t CFM::setAck(const char* rfAck, uint8_t speed, uint16_t frequency, uint8_ return m_rfAck.setParams(rfAck, speed, frequency, level, level); } -uint8_t CFM::setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFrequency, uint8_t ctcssHighThreshold, uint8_t ctcssLowThreshold, uint8_t ctcssLevel, uint8_t kerchunkTime, uint8_t hangTime, uint8_t accessMode, bool cosInvert, uint8_t rfAudioBoost, uint8_t maxDev, uint8_t rxLevel) +uint8_t CFM::setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFrequency, uint8_t ctcssHighThreshold, uint8_t ctcssLowThreshold, uint8_t ctcssLevel, uint8_t kerchunkTime, uint8_t hangTime, uint8_t accessMode, bool cosInvert, bool noiseSquelch, uint8_t squelchHighThreshold, uint8_t squelchLowThreshold, uint8_t rfAudioBoost, uint8_t maxDev, uint8_t rxLevel) { - m_accessMode = accessMode; - m_cosInvert = cosInvert; + m_accessMode = accessMode; + m_cosInvert = cosInvert; + m_noiseSquelch = noiseSquelch; m_rfAudioBoost = q15_t(rfAudioBoost); @@ -331,6 +340,8 @@ uint8_t CFM::setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFreque m_rxLevel = rxLevel; //q15_t(255)/q15_t(rxLevel >> 1); + m_squelch.setParams(squelchHighThreshold, squelchLowThreshold); + uint8_t ret = m_ctcssRX.setParams(ctcssFrequency, ctcssHighThreshold, ctcssLowThreshold); if (ret != 0U) return ret; diff --git a/FM.h b/FM.h index 2b269a7..2ead4a3 100644 --- a/FM.h +++ b/FM.h @@ -31,6 +31,7 @@ #include "FMDirectForm1.h" #include "FMDownSampler.h" #include "FMUpSampler.h" +#include "FMNoiseSquelch.h" enum FM_STATE { FS_LISTENING, @@ -60,7 +61,7 @@ public: uint8_t setCallsign(const char* callsign, uint8_t speed, uint16_t frequency, uint8_t time, uint8_t holdoff, uint8_t highLevel, uint8_t lowLevel, bool callsignAtStart, bool callsignAtEnd, bool callsignAtLatch); uint8_t setAck(const char* rfAck, uint8_t speed, uint16_t frequency, uint8_t minTime, uint16_t delay, uint8_t level); - uint8_t setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFrequency, uint8_t ctcssHighThreshold, uint8_t ctcssLowThreshold, uint8_t ctcssLevel, uint8_t kerchunkTime, uint8_t hangTime, uint8_t accessMode, bool cosInvert, uint8_t rfAudioBoost, uint8_t maxDev, uint8_t rxLevel); + uint8_t setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFrequency, uint8_t ctcssHighThreshold, uint8_t ctcssLowThreshold, uint8_t ctcssLevel, uint8_t kerchunkTime, uint8_t hangTime, uint8_t accessMode, bool cosInvert, bool noiseSquelch, uint8_t squelchHighThreshold, uint8_t squelchLowThreshold, uint8_t rfAudioBoost, uint8_t maxDev, uint8_t rxLevel); uint8_t setExt(const char* ack, uint8_t audioBoost, uint8_t speed, uint16_t frequency, uint8_t level); uint8_t getSpace() const; @@ -73,6 +74,7 @@ private: CFMKeyer m_extAck; CFMCTCSSRX m_ctcssRX; CFMCTCSSTX m_ctcssTX; + CFMNoiseSquelch m_squelch; CFMTimeout m_timeoutTone; FM_STATE m_state; bool m_callsignAtStart; @@ -94,6 +96,7 @@ private: CFMBlanking m_blanking; uint8_t m_accessMode; bool m_cosInvert; + bool m_noiseSquelch; q15_t m_rfAudioBoost; q15_t m_extAudioBoost; CFMDownSampler m_downSampler; diff --git a/FMNoiseSquelch.cpp b/FMNoiseSquelch.cpp new file mode 100644 index 0000000..1f65fea --- /dev/null +++ b/FMNoiseSquelch.cpp @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2020 by Jonathan Naylor G4KLX + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "Config.h" +#include "Globals.h" +#include "FMNoiseSquelch.h" + +// 4500Hz centre frequency +const q31_t COEFF_DIV_TWO = 821806413; + +// 400Hz bandwidth +const uint16_t N = 24000U / 400U; + +CFMNoiseSquelch::CFMNoiseSquelch() : +m_highThreshold(0), +m_lowThreshold(0), +m_count(0U), +m_q0(0), +m_q1(0), +m_result(NS_NONE) +{ +} + +void CFMNoiseSquelch::setParams(uint8_t highThreshold, uint8_t lowThreshold) +{ + m_highThreshold = q31_t(highThreshold); + m_lowThreshold = q31_t(lowThreshold); +} + +uint8_t CFMNoiseSquelch::process(q15_t sample) +{ + //get more dynamic into the decoder by multiplying the sample by 1.5 + q31_t sample31 = q31_t(sample) + (q31_t(sample) >> 1); + + m_result &= ~NS_READY; + + q31_t q2 = m_q1; + m_q1 = m_q0; + + // Q31 multiplication, t3 = m_coeffDivTwo * 2 * m_q1 + q63_t t1 = COEFF_DIV_TWO * m_q1; + q31_t t2 = __SSAT((t1 >> 31), 31); + q31_t t3 = t2 * 2; + + // m_q0 = m_coeffDivTwo * m_q1 * 2 - q2 + sample + m_q0 = t3 - q2 + sample31; + + m_count++; + if (m_count == N) { + // Q31 multiplication, t2 = m_q0 * m_q0 + q63_t t1 = q63_t(m_q0) * q63_t(m_q0); + q31_t t2 = __SSAT((t1 >> 31), 31); + + // Q31 multiplication, t4 = m_q0 * m_q0 + q63_t t3 = q63_t(m_q1) * q63_t(m_q1); + q31_t t4 = __SSAT((t3 >> 31), 31); + + // Q31 multiplication, t9 = m_q0 * m_q1 * m_coeffDivTwo * 2 + q63_t t5 = q63_t(m_q0) * q63_t(m_q1); + q31_t t6 = __SSAT((t5 >> 31), 31); + q63_t t7 = t6 * COEFF_DIV_TWO; + q31_t t8 = __SSAT((t7 >> 31), 31); + q31_t t9 = t8 * 2; + + // value = m_q0 * m_q0 + m_q1 * m_q1 - m_q0 * m_q1 * m_coeffDivTwo * 2 + q31_t value = t2 + t4 - t9; + + bool previousNSQValid = NSQ_VALID(m_result); + + q31_t threshold = m_highThreshold; + if (previousNSQValid) + threshold = m_lowThreshold; + + m_result |= NS_READY; + if (value >= threshold) + m_result |= NS_VALID; + else + m_result &= ~NS_VALID; + + if (previousNSQValid != NSQ_VALID(m_result)) + DEBUG4("Noise Squelch Value / Threshold / Valid", value, threshold, NSQ_VALID(m_result)); + + m_count = 0U; + m_q0 = 0; + m_q1 = 0; + } + + return m_result; +} + +void CFMNoiseSquelch::reset() +{ + m_q0 = 0; + m_q1 = 0; + m_result = NS_NONE; + m_count = 0U; +} diff --git a/FMNoiseSquelch.h b/FMNoiseSquelch.h new file mode 100644 index 0000000..a17f7a2 --- /dev/null +++ b/FMNoiseSquelch.h @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2020 by Jonathan Naylor G4KLX + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#if !defined(FMNOISESQUELCH_H) +#define FMNOISESQUELCH_H + +#include "Config.h" + +const uint8_t NS_NONE = 0U; +const uint8_t NS_READY = 1U; +const uint8_t NS_VALID = 2U; + +#define NSQ_READY(a) ((a & NS_READY) != 0) +#define NSQ_NOT_READY(a) ((a & NS_READY) == 0) +#define NSQ_VALID(a) ((a & NS_VALID) != 0) +#define NSQ_NOT_VALID(a) ((a & NS_VALID) == 0) + +class CFMNoiseSquelch { +public: + CFMNoiseSquelch(); + + void setParams(uint8_t highThreshold, uint8_t lowThreshold); + + uint8_t process(q15_t sample); + + void reset(); + +private: + q31_t m_highThreshold; + q31_t m_lowThreshold; + uint16_t m_count; + q31_t m_q0; + q31_t m_q1; + uint8_t m_result; +}; + +#endif diff --git a/SerialPort.cpp b/SerialPort.cpp index b6a7cef..c61d883 100644 --- a/SerialPort.cpp +++ b/SerialPort.cpp @@ -109,7 +109,7 @@ const uint8_t MMDVM_DEBUG5 = 0xF5U; #define HW_TYPE "MMDVM" #endif -#define DESCRIPTION "20200714 (D-Star/DMR/System Fusion/P25/NXDN/POCSAG/FM/AX.25)" +#define DESCRIPTION "20200728 (D-Star/DMR/System Fusion/P25/NXDN/POCSAG/FM/AX.25)" #if defined(GITVERSION) #define concat(h, a, b, c) h " " a " " b " GitID #" c "" @@ -452,7 +452,7 @@ uint8_t CSerialPort::setFMParams2(const uint8_t* data, uint16_t length) uint8_t CSerialPort::setFMParams3(const uint8_t* data, uint16_t length) { - if (length < 12U) + if (length < 14U) return 4U; uint16_t timeout = data[0U] * 5U; @@ -467,13 +467,17 @@ uint8_t CSerialPort::setFMParams3(const uint8_t* data, uint16_t length) uint8_t hangTime = data[7U]; uint8_t accessMode = data[8U] & 0x7FU; + bool noiseSquelch = (data[8U] & 0x40U) == 0x40U; bool cosInvert = (data[8U] & 0x80U) == 0x80U; uint8_t rfAudioBoost = data[9U]; uint8_t maxDev = data[10U]; uint8_t rxLevel = data[11U]; - return fm.setMisc(timeout, timeoutLevel, ctcssFrequency, ctcssHighThreshold, ctcssLowThreshold, ctcssLevel, kerchunkTime, hangTime, accessMode, cosInvert, rfAudioBoost, maxDev, rxLevel); + uint8_t squelchHighThreshold = data[12U]; + uint8_t squelchLowThreshold = data[13U]; + + return fm.setMisc(timeout, timeoutLevel, ctcssFrequency, ctcssHighThreshold, ctcssLowThreshold, ctcssLevel, kerchunkTime, hangTime, accessMode, cosInvert, noiseSquelch, squelchHighThreshold, squelchLowThreshold, rfAudioBoost, maxDev, rxLevel); } uint8_t CSerialPort::setFMParams4(const uint8_t* data, uint16_t length)