75 lines
888 B
C
75 lines
888 B
C
//
|
|
// Created by bruno on 13. 7. 2026.
|
|
//
|
|
|
|
#include "goertzel.h"
|
|
#include <math.h>
|
|
|
|
|
|
void goertzel_init(
|
|
Goertzel *g,
|
|
float freq,
|
|
float sampleRate,
|
|
int samples)
|
|
{
|
|
|
|
float omega =
|
|
2.0f * M_PI * freq / sampleRate;
|
|
|
|
|
|
g->coeff =
|
|
2.0f*cosf(omega);
|
|
|
|
|
|
g->s1=0;
|
|
g->s2=0;
|
|
|
|
}
|
|
|
|
|
|
|
|
void goertzel_add(
|
|
Goertzel *g,
|
|
float sample)
|
|
{
|
|
|
|
float s =
|
|
sample +
|
|
g->coeff*g->s1 -
|
|
g->s2;
|
|
|
|
|
|
g->s2=g->s1;
|
|
g->s1=s;
|
|
|
|
}
|
|
|
|
|
|
|
|
float goertzel_energy(
|
|
Goertzel *g)
|
|
{
|
|
|
|
float power =
|
|
g->s1*g->s1 +
|
|
g->s2*g->s2 -
|
|
g->coeff*g->s1*g->s2;
|
|
|
|
|
|
g->s1=0;
|
|
g->s2=0;
|
|
|
|
|
|
return power;
|
|
|
|
}
|
|
|
|
int goertzel_bin_freq(
|
|
int freq,
|
|
int sampleRate,
|
|
int samples)
|
|
{
|
|
int bin = (freq * samples + sampleRate / 2) / sampleRate;
|
|
|
|
return bin * sampleRate / samples;
|
|
} |