Added implementation of CTCSS tone detector
This commit is contained in:
Silvano Seva
parent
0ef8e6eed9
commit
b5ca097c52
|
|
@ -0,0 +1,151 @@
|
|||
/***************************************************************************
|
||||
* Copyright (C) 2025 by Federico Amedeo Izzo IU2NUO, *
|
||||
* Niccolò Izzo IU2KIN *
|
||||
* Frederik Saraci IU2NRO *
|
||||
* Silvano Seva IU2KWO *
|
||||
* *
|
||||
* 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 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 for more details. *
|
||||
* *
|
||||
* You should have received a copy of the GNU General Public License *
|
||||
* along with this program; if not, see <http://www.gnu.org/licenses/> *
|
||||
***************************************************************************/
|
||||
|
||||
#ifndef CTCSS_DETECTOR_H
|
||||
#define CTCSS_DETECTOR_H
|
||||
|
||||
#ifndef __cplusplus
|
||||
#error This header is C++ only!
|
||||
#endif
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <goertzel.hpp>
|
||||
|
||||
/*
|
||||
* Goertzel filter coefficients for the 50 CTCSS tones, computed for a sampling
|
||||
* frequency of 2kHz.
|
||||
*/
|
||||
static constexpr std::array < float, 50 > ctcssCoeffs2k =
|
||||
{
|
||||
1.955858f, 1.952788f, 1.949194f, 1.945616f, 1.941767f, 1.937634f, 1.933200f,
|
||||
1.928450f, 1.923195f, 1.917936f, 1.911955f, 1.907097f, 1.902113f, 1.895202f,
|
||||
1.887648f, 1.879838f, 1.871331f, 1.862315f, 1.852531f, 1.842180f, 1.830988f,
|
||||
1.818907f, 1.806161f, 1.792725f, 1.778002f, 1.762507f, 1.753218f, 1.745912f,
|
||||
1.735704f, 1.728163f, 1.717311f, 1.709207f, 1.697685f, 1.688992f, 1.676770f,
|
||||
1.667463f, 1.654514f, 1.644208f, 1.630498f, 1.619878f, 1.605010f, 1.593692f,
|
||||
1.577610f, 1.548608f, 1.517951f, 1.503955f, 1.485167f, 1.450171f, 1.412880f,
|
||||
1.395880f
|
||||
};
|
||||
|
||||
/**
|
||||
* CTCSS detector class, based on the modified Goertzel filter.
|
||||
* The detector is designed to be called recursively: each time the number of
|
||||
* accumulated samples reaches the detection window, the energy of each CTCSS
|
||||
* frequency is evaluated to find the one with the maximum value. If the ratio
|
||||
* between the peak energy and the average of all the energies is above the
|
||||
* threshold, the tone corresponding to the peak is considered detected.
|
||||
*/
|
||||
class CtcssDetector
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
*
|
||||
* @param coeffs: Goertzel filter coefficients.
|
||||
* @param window: size of the detection window.
|
||||
* @param threshold: detection threshold.
|
||||
*/
|
||||
CtcssDetector(const std::array< float, 50 >& coeffs,
|
||||
const uint32_t window, const float threshold) :
|
||||
threshold(threshold), window(window), goertzel(coeffs),
|
||||
totSamples(0), overThresh(false) { }
|
||||
|
||||
/**
|
||||
* Destructor.
|
||||
*/
|
||||
~CtcssDetector() { }
|
||||
|
||||
/**
|
||||
* Update the internal states of the Goertzel filter.
|
||||
*
|
||||
* @param samples: pointer to new input values.
|
||||
* @param numSamples: number of new input values.
|
||||
*/
|
||||
void update(const int16_t *samples, const size_t numSamples)
|
||||
{
|
||||
goertzel.samples(samples, numSamples);
|
||||
totSamples += numSamples;
|
||||
|
||||
if(totSamples >= window)
|
||||
{
|
||||
analyze();
|
||||
goertzel.reset();
|
||||
totSamples = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if a CTCSS tone is being detected.
|
||||
*
|
||||
* @param toneIdx: index of the CTCSS tone.
|
||||
* @return true if the CTCSS tone is active.
|
||||
*/
|
||||
bool toneDetected(const uint8_t toneIdx)
|
||||
{
|
||||
return (toneIdx == activeToneIdx) && (overThresh == true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset detector state.
|
||||
*/
|
||||
void reset()
|
||||
{
|
||||
goertzel.reset();
|
||||
totSamples = 0;
|
||||
overThresh = false;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Compute signal power and determine if a CTCSS tone is active.
|
||||
*/
|
||||
void analyze()
|
||||
{
|
||||
float avgPower = 0.0f;
|
||||
float maxPower = 0.0f;
|
||||
|
||||
for(size_t i = 0; i < 50; i++)
|
||||
{
|
||||
float power = goertzel.power(i);
|
||||
avgPower += power;
|
||||
if(maxPower < power)
|
||||
{
|
||||
maxPower = power;
|
||||
activeToneIdx = i;
|
||||
}
|
||||
}
|
||||
|
||||
avgPower /= 50.0f;
|
||||
overThresh = (maxPower >= (avgPower * threshold)) ? true : false;
|
||||
}
|
||||
|
||||
const float threshold;
|
||||
const uint32_t window;
|
||||
Goertzel< 50 > goertzel;
|
||||
uint32_t totSamples;
|
||||
uint8_t activeToneIdx;
|
||||
bool overThresh;
|
||||
};
|
||||
|
||||
#endif /* CTCSS_DETECTOR_H */
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
/***************************************************************************
|
||||
* Copyright (C) 2025 by Federico Amedeo Izzo IU2NUO, *
|
||||
* Niccolò Izzo IU2KIN *
|
||||
* Frederik Saraci IU2NRO *
|
||||
* Silvano Seva IU2KWO *
|
||||
* *
|
||||
* 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 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 for more details. *
|
||||
* *
|
||||
* You should have received a copy of the GNU General Public License *
|
||||
* along with this program; if not, see <http://www.gnu.org/licenses/> *
|
||||
***************************************************************************/
|
||||
|
||||
#ifndef GOERTZEL_H
|
||||
#define GOERTZEL_H
|
||||
|
||||
#ifndef __cplusplus
|
||||
#error This header is C++ only!
|
||||
#endif
|
||||
|
||||
#include <array>
|
||||
|
||||
/**
|
||||
* Class for modified Goertzel filter with configurable coefficients.
|
||||
*/
|
||||
template < size_t N >
|
||||
class Goertzel
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
*
|
||||
* @param coeffs: Goertzel coefficients.
|
||||
*/
|
||||
Goertzel(const std::array< float, N >& coeffs) : k(coeffs)
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Destructor.
|
||||
*/
|
||||
~Goertzel() { }
|
||||
|
||||
/**
|
||||
* Update the internal states of the Goertzel filter.
|
||||
*
|
||||
* @param input: input value for the current time step.
|
||||
*/
|
||||
void sample(const int16_t value)
|
||||
{
|
||||
for(size_t i = 0; i < N; i++)
|
||||
{
|
||||
float u = static_cast< float >(value) + (k[i] * u0[i]) - u1[i];
|
||||
u1[i] = u0[i];
|
||||
u0[i] = u;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the internal states of the Goertzel filter.
|
||||
*
|
||||
* @param samples: pointer to new input values.
|
||||
* @param numSamples: number of new input values.
|
||||
*/
|
||||
void samples(const int16_t *samples, const size_t numSamples)
|
||||
{
|
||||
for(size_t i = 0; i < numSamples; i++)
|
||||
sample(samples[i]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get signal power at a given frequency index.
|
||||
*
|
||||
* @param index: frequency index.
|
||||
* @return signal power.
|
||||
*/
|
||||
float power(const size_t freq)
|
||||
{
|
||||
if(freq >= N)
|
||||
return 0;
|
||||
|
||||
return (u0[freq] * u0[freq]) +
|
||||
(u1[freq] * u1[freq]) -
|
||||
(u0[freq] * u1[freq] * k[freq]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset filter history.
|
||||
*/
|
||||
void reset()
|
||||
{
|
||||
u0.fill(0.0f);
|
||||
u1.fill(0.0f);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
const std::array< float, N >& k;
|
||||
std::array< float, N > u0;
|
||||
std::array< float, N > u1;
|
||||
};
|
||||
|
||||
#endif /* GOERTZEL_H */
|
||||
Loading…
Reference in New Issue