113 lines
3.2 KiB
C++
113 lines
3.2 KiB
C++
/***************************************************************************
|
|
* 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 */
|