Detecção de Assobio Humano com o Algoritmo de Goertzel no RP2040

3 — Implementação básica do algoritmo de Goertzel em C (isolado, incremental e testável)

Nesta seção vamos materializar a teoria da Seção 2 em código C simples, direto e verificável, antes de qualquer integração com ADC, timer ou GPIO.
A ideia é a mesma do artigo anterior: construir o algoritmo em camadas, validando cada uma.

3.1 Estrutura de estado do Goertzel

O algoritmo precisa apenas de dois estados:
\(s[n-1]) e (s[n-2]\).

Definimos uma estrutura explícita:

typedef struct {
    float s1;   // s[n-1]
    float s2;   // s[n-2]
    float coeff; // c = 2*cos(2*pi*k/N)
} goertzel_t;

typedef struct {
    float s1;   // s[n-1]
    float s2;   // s[n-2]
    float coeff; // c = 2*cos(2*pi*k/N)
} goertzel_t;

Essa estrutura deixa claro que:

• O algoritmo é determinístico
• Não depende de buffers grandes
• Não usa números complexos durante o loop

3.2 Inicialização do detector Goertzel

Para uma frequência alvo (f_0), calculamos:

\[
k = \text{round}\left(\frac{f_0}{f_s} \cdot N\right)
\quad,\quad
c = 2\cos\left(\frac{2\pi k}{N}\right)
\]

Código correspondente:

#include <math.h>

void goertzel_init(goertzel_t *g, float f0, float fs, int N)
{
    int k = (int)(0.5f + ((float)N * f0) / fs);
    float w = (2.0f * (float)M_PI * (float)k) / (float)N;

    g->coeff = 2.0f * cosf(w);
    g->s1 = 0.0f;
    g->s2 = 0.0f;
}

#include <math.h>

void goertzel_init(goertzel_t *g, float f0, float fs, int N)
{
    int k = (int)(0.5f + ((float)N * f0) / fs);
    float w = (2.0f * (float)M_PI * (float)k) / (float)N;

    g->coeff = 2.0f * cosf(w);
    g->s1 = 0.0f;
    g->s2 = 0.0f;
}

Aqui usamos cosf() apenas uma vez, fora do loop de tempo real.

3.3 Passo recursivo do Goertzel (núcleo do algoritmo)

A equação central é:

\[
s[n] = x[n] + c,s[n-1] – s[n-2]
\]

Implementação direta:

static inline void goertzel_step(goertzel_t *g, float x)
{
    float s0 = x + g->coeff * g->s1 - g->s2;
    g->s2 = g->s1;
    g->s1 = s0;
}

static inline void goertzel_step(goertzel_t *g, float x)
{
    float s0 = x + g->coeff * g->s1 - g->s2;
    g->s2 = g->s1;
    g->s1 = s0;
}

Características importantes:

• Apenas 2 multiplicações
• Apenas 2 somas
• Extremamente eficiente em MCU

3.4 Cálculo da energia da frequência

Após processar (N) amostras, calculamos:

\[
|X[k]|^2 = s_1^2 + s_2^2 – c,s_1,s_2
\]

Implementação:

static inline float goertzel_power(const goertzel_t *g)
{
    return (g->s1 * g->s1) +
           (g->s2 * g->s2) -
           (g->coeff * g->s1 * g->s2);
}

static inline float goertzel_power(const goertzel_t *g)
{
    return (g->s1 * g->s1) +
           (g->s2 * g->s2) -
           (g->coeff * g->s1 * g->s2);
}

Esse valor:

• Independe da fase
• Cresce rapidamente quando a frequência bate
• É ideal para comparação com limiar

3.5 Reset do estado (para novo frame)

Após cada bloco de (N) amostras, precisamos resetar:

static inline void goertzel_reset(goertzel_t *g)
{
    g->s1 = 0.0f;
    g->s2 = 0.0f;
}

static inline void goertzel_reset(goertzel_t *g)
{
    g->s1 = 0.0f;
    g->s2 = 0.0f;
}

3.6 Teste isolado com sinal sintético (validação conceitual)

Antes de usar microfone, sempre valide com sinal conhecido.

Exemplo: senoide pura em (f_0).

void goertzel_test(void)
{
    const float fs = 8000.0f;
    const int   N  = 256;
    const float f0 = 1500.0f;

    goertzel_t g;
    goertzel_init(&g, f0, fs, N);

    for (int n = 0; n < N; n++) {
        float x = sinf(2.0f * (float)M_PI * f0 * ((float)n / fs));
        goertzel_step(&g, x);
    }

    float power = goertzel_power(&g);
    printf("Power @ %.1f Hz = %f\n", f0, power);
}

void goertzel_test(void)
{
    const float fs = 8000.0f;
    const int   N  = 256;
    const float f0 = 1500.0f;

    goertzel_t g;
    goertzel_init(&g, f0, fs, N);

    for (int n = 0; n < N; n++) {
        float x = sinf(2.0f * (float)M_PI * f0 * ((float)n / fs));
        goertzel_step(&g, x);
    }

    float power = goertzel_power(&g);
    printf("Power @ %.1f Hz = %f\n", f0, power);
}

Se você:

• Trocar f0 por outra frequência no seno
• Manter o detector em 1500 Hz

👉 o valor de power cai drasticamente.
Isso confirma que o detector está corretamente sintonizado.

3.7 O que aprendemos até aqui

Até este ponto, temos:

• Um detector espectral pontual
• Sem FFT
• Sem números complexos no loop
• Custo previsível e baixo
• Ideal para RP2040

Mas ainda não temos um sistema real, pois:

• Não há ADC
• Não há frame de tempo real
• Não há critério de decisão
• Não há múltiplas frequências

Tudo isso entra a partir da próxima seção.