init

goertzel.c

@@ -0,0 +1,75 @@
+//
+// 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;
+}