diff --git a/sdrbase/dsp/ctcssdetector.cpp b/sdrbase/dsp/ctcssdetector.cpp index c9122d3de..df073562f 100644 --- a/sdrbase/dsp/ctcssdetector.cpp +++ b/sdrbase/dsp/ctcssdetector.cpp @@ -1,68 +1,72 @@ -/* - * ctcssdetector.cpp - * - * Created on: Jun 16, 2015 - * Author: f4exb - */ -#include -#include "dsp/ctcssdetector.h" -#include "ctcssfrequencies.h" +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015-2020 Edouard Griffiths, F4EXB // +// // +// See: http://www.embedded.com/design/connectivity/4025660/Detecting-CTCSS-tones-with-Goertzel-s-algorithm // +// // +// 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 as version 3 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 V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +////////////////////////////////////////////////////////////////////////////////////////////////////////////// -#undef M_PI -#define M_PI 3.14159265358979323846 +#define _USE_MATH_DEFINES +#include + +#include "dsp/ctcssdetector.h" CTCSSDetector::CTCSSDetector() : - N(0), - sampleRate(0), - samplesProcessed(0), - maxPowerIndex(0), - toneDetected(false), - maxPower(0.0) + m_N(0), + m_sampleRate(0), + m_samplesProcessed(0), + m_maxPowerIndex(0), + m_toneDetected(false), + m_maxPower(0.0) { - nTones = CTCSSFrequencies::m_nbFreqs; - k = new Real[nTones]; - coef = new Real[nTones]; - toneSet = new Real[nTones]; - u0 = new Real[nTones]; - u1 = new Real[nTones]; - power = new Real[nTones]; - toneSet = CTCSSFrequencies::m_Freqs; + m_k = new Real[CTCSSFrequencies::m_nbFreqs]; + m_coef = new Real[CTCSSFrequencies::m_nbFreqs]; + m_u0 = new Real[CTCSSFrequencies::m_nbFreqs]; + m_u1 = new Real[CTCSSFrequencies::m_nbFreqs]; + m_power = new Real[CTCSSFrequencies::m_nbFreqs]; } CTCSSDetector::CTCSSDetector(int _nTones, Real *tones) : - N(0), - sampleRate(0), - samplesProcessed(0), - maxPowerIndex(0), - toneDetected(false), - maxPower(0.0) + m_N(0), + m_sampleRate(0), + m_samplesProcessed(0), + m_maxPowerIndex(0), + m_toneDetected(false), + m_maxPower(0.0) { - nTones = CTCSSFrequencies::m_nbFreqs; - k = new Real[nTones]; - coef = new Real[nTones]; - toneSet = new Real[nTones]; - u0 = new Real[nTones]; - u1 = new Real[nTones]; - power = new Real[nTones]; - toneSet = CTCSSFrequencies::m_Freqs; + m_k = new Real[CTCSSFrequencies::m_nbFreqs]; + m_coef = new Real[CTCSSFrequencies::m_nbFreqs]; + m_u0 = new Real[CTCSSFrequencies::m_nbFreqs]; + m_u1 = new Real[CTCSSFrequencies::m_nbFreqs]; + m_power = new Real[CTCSSFrequencies::m_nbFreqs]; } CTCSSDetector::~CTCSSDetector() { - delete[] k; - delete[] coef; - delete[] toneSet; - delete[] u0; - delete[] u1; - delete[] power; + delete[] m_k; + delete[] m_coef; + delete[] m_u0; + delete[] m_u1; + delete[] m_power; } -void CTCSSDetector::setCoefficients(int zN, int _samplerate ) +void CTCSSDetector::setCoefficients(int N, int sampleRate) { - N = zN; // save the basic parameters for use during analysis - sampleRate = _samplerate; + m_N = N; // save the basic parameters for use during analysis + m_sampleRate = sampleRate; // for each of the frequencies (tones) of interest calculate // k and the associated filter coefficient as per the Goertzel @@ -71,10 +75,10 @@ void CTCSSDetector::setCoefficients(int zN, int _samplerate ) // for later display. The tone set is specified in the // constructor. Notice that the resulting coefficients are // independent of N. - for (int j = 0; j < nTones; ++j) + for (int j = 0; j < CTCSSFrequencies::m_nbFreqs; ++j) { - k[j] = ((double)N * toneSet[j]) / (double)sampleRate; - coef[j] = 2.0 * cos((2.0 * M_PI * toneSet[j])/(double)sampleRate); + m_k[j] = ((double) m_N * CTCSSFrequencies::m_Freqs[j]) / (double)m_sampleRate; + m_coef[j] = 2.0 * cos((2.0 * M_PI * CTCSSFrequencies::m_Freqs[j])/(double)m_sampleRate); } } @@ -84,12 +88,12 @@ bool CTCSSDetector::analyze(Real *sample) { feedback(*sample); // Goertzel feedback - samplesProcessed += 1; + m_samplesProcessed += 1; - if (samplesProcessed == N) // completed a block of N + if (m_samplesProcessed == m_N) // completed a block of N { - feedForward(); // calculate the power at each tone - samplesProcessed = 0; + feedForward(); // calculate the m_power at each tone + m_samplesProcessed = 0; return true; // have a result } else @@ -104,11 +108,11 @@ void CTCSSDetector::feedback(Real in) Real t; // feedback for each tone - for (int j = 0; j < nTones; ++j) + for (int j = 0; j < CTCSSFrequencies::m_nbFreqs; ++j) { - t = u0[j]; - u0[j] = in + (coef[j] * u0[j]) - u1[j]; - u1[j] = t; + t = m_u0[j]; + m_u0[j] = in + (m_coef[j] * m_u0[j]) - m_u1[j]; + m_u1[j] = t; } } @@ -117,10 +121,10 @@ void CTCSSDetector::feedForward() { initializePower(); - for (int j = 0; j < nTones; ++j) + for (int j = 0; j < CTCSSFrequencies::m_nbFreqs; ++j) { - power[j] = (u0[j] * u0[j]) + (u1[j] * u1[j]) - (coef[j] * u0[j] * u1[j]); - u0[j] = u1[j] = 0.0; // reset for next block. + m_power[j] = (m_u0[j] * m_u0[j]) + (m_u1[j] * m_u1[j]) - (m_coef[j] * m_u0[j] * m_u1[j]); + m_u0[j] = m_u1[j] = 0.0; // reset for next block. } evaluatePower(); @@ -129,23 +133,23 @@ void CTCSSDetector::feedForward() void CTCSSDetector::reset() { - for (int j = 0; j < nTones; ++j) + for (int j = 0; j < CTCSSFrequencies::m_nbFreqs; ++j) { - power[j] = u0[j] = u1[j] = 0.0; // reset + m_power[j] = m_u0[j] = m_u1[j] = 0.0; // reset } - samplesProcessed = 0; - maxPower = 0.0; - maxPowerIndex = 0; - toneDetected = false; + m_samplesProcessed = 0; + m_maxPower = 0.0; + m_maxPowerIndex = 0; + m_toneDetected = false; } void CTCSSDetector::initializePower() { - for (int j = 0; j < nTones; ++j) + for (int j = 0; j < CTCSSFrequencies::m_nbFreqs; ++j) { - power[j] = 0.0; // reset + m_power[j] = 0.0; // reset } } @@ -153,19 +157,19 @@ void CTCSSDetector::initializePower() void CTCSSDetector::evaluatePower() { Real sumPower = 0.0; - Real aboveAvg = 2.0; // Arbitrary max power above average threshold - maxPower = 0.0; + Real aboveAvg = 2.0; // Arbitrary max m_power above average threshold + m_maxPower = 0.0; - for (int j = 0; j < nTones; ++j) + for (int j = 0; j < CTCSSFrequencies::m_nbFreqs; ++j) { - sumPower += power[j]; + sumPower += m_power[j]; - if (power[j] > maxPower) + if (m_power[j] > m_maxPower) { - maxPower = power[j]; - maxPowerIndex = j; + m_maxPower = m_power[j]; + m_maxPowerIndex = j; } } - toneDetected = (maxPower > (sumPower/nTones) + aboveAvg); + m_toneDetected = (m_maxPower > (sumPower/CTCSSFrequencies::m_nbFreqs) + aboveAvg); } diff --git a/sdrbase/dsp/ctcssdetector.h b/sdrbase/dsp/ctcssdetector.h index 0edf3c5a0..b3d4b7736 100644 --- a/sdrbase/dsp/ctcssdetector.h +++ b/sdrbase/dsp/ctcssdetector.h @@ -1,16 +1,28 @@ -/* - * ctcssdetector.h - * - * Created on: Jun 16, 2015 - * Author: f4exb - * See: http://www.embedded.com/design/connectivity/4025660/Detecting-CTCSS-tones-with-Goertzel-s-algorithm - */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2015-2020 Edouard Griffiths, F4EXB // +// // +// See: http://www.embedded.com/design/connectivity/4025660/Detecting-CTCSS-tones-with-Goertzel-s-algorithm // +// // +// 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 as version 3 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 V3 for more details. // +// // +// You should have received a copy of the GNU General Public License // +// along with this program. If not, see . // +////////////////////////////////////////////////////////////////////////////////////////////////////////////// #ifndef INCLUDE_GPL_DSP_CTCSSDETECTOR_H_ #define INCLUDE_GPL_DSP_CTCSSDETECTOR_H_ #include "dsp/dsptypes.h" #include "export.h" +#include "ctcssfrequencies.h" /** CTCSSDetector: Continuous Tone Coded Squelch System * tone detector class based on the Modified Goertzel @@ -18,49 +30,45 @@ */ class SDRBASE_API CTCSSDetector { public: - // Constructors and Destructor CTCSSDetector(); - // allows user defined CTCSS tone set - CTCSSDetector(int _nTones, Real *tones); + CTCSSDetector(int _nTones, Real *tones); //!< allows user defined CTCSS tone set virtual ~CTCSSDetector(); // setup the basic parameters and coefficients void setCoefficients( - int zN, // the algorithm "block" size - int SampleRate); // input signal sample rate + int N, // the algorithm "block" size + int sampleRate // input signal sample rate + ); // set the detection threshold void setThreshold(double thold); - // analyze a sample set and optionally filter - // the tone frequencies. + // analyze a sample set and optionally filter the tone frequencies. bool analyze(Real *sample); // input signal sample // get the number of defined tones. int getNTones() const { - return nTones; + return CTCSSFrequencies::m_nbFreqs; } // get the tone set - const Real *getToneSet() const - { - return toneSet; + const Real *getToneSet() const { + return CTCSSFrequencies::m_Freqs; } // get the currently detected tone, if any bool getDetectedTone(int &maxTone) const { - maxTone = maxPowerIndex; - return toneDetected; + maxTone = m_maxPowerIndex; + return m_toneDetected; } - // Get the max power at the detected tone. - Real getMaxPower() const - { - return maxPower; + // Get the max m_power at the detected tone. + Real getMaxPower() const { + return m_maxPower; } - void reset(); // reset the analysis algorithm + void reset(); // reset the analysis algorithm protected: // Override these to change behavior of the detector @@ -70,19 +78,17 @@ protected: void feedForward(); private: - int N; - int sampleRate; - int nTones; - int samplesProcessed; - int maxPowerIndex; - bool toneDetected; - Real maxPower; - Real *k; - Real *coef; - const float *toneSet; - Real *u0; - Real *u1; - Real *power; + int m_N; + int m_sampleRate; + int m_samplesProcessed; + int m_maxPowerIndex; + bool m_toneDetected; + Real m_maxPower; + Real *m_k; + Real *m_coef; + Real *m_u0; + Real *m_u1; + Real *m_power; };