36 lines
719 B
C
36 lines
719 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;
|
|
} |