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

/**
 * main.c — Detector de assobio com Goertzel (RP2040 / BitDogLab)
 *
 * Estratégia:
 * ADC -> DC-block -> frame 256 -> varredura Goertzel (800..3000 Hz)
 * -> candidato f0 -> harmônicos 2f0/3f0 -> score -> EMA + histerese -> LED
 *
 * Pico SDK libs: pico_stdlib, hardware_adc, pico_time
 */

#include <stdio.h>
#include <stdbool.h>
#include <math.h>

#include "pico/stdlib.h"
#include "pico/time.h"
#include "hardware/adc.h"

// =============================
// Configurações principais
// =============================
#define FS_HZ       8000
#define FRAME_N     256

// ADC
#define ADC_GPIO    28
#define ADC_CH      0

// LED: no Pico costuma ser 25; na BitDogLab pode variar
#define LED_GPIO    13

// Varredura em Hz
#define F_MIN       800.0f
#define F_MAX       3000.0f

// Quantidade máxima de bins na varredura
#define MAX_BINS    64

// Gate por energia (ajuste)
#define E_MIN       0.0008f

// EMA do score
#define EMA_ALPHA   0.85f

// Histerese do score (ajuste)
#define S_ON        25.0f
#define S_OFF       18.0f

// Pesos do score (ajuste)
#define L2          0.7f
#define L3          0.35f

// =============================
// Buffer de frame
// =============================
typedef struct {
    float data[FRAME_N];
    uint32_t idx;
    bool full;
} frame_buf_t;

static inline void frame_buf_init(frame_buf_t *b) {
    b->idx = 0;
    b->full = false;
}

static inline void frame_buf_push(frame_buf_t *b, float v) {
    b->data[b->idx++] = v;
    if (b->idx >= FRAME_N) {
        b->idx = 0;
        b->full = true;
    }
}

// =============================
// DC-block (remoção de offset)
// =============================
typedef struct {
    float mean;
} dc_block_t;

static inline void dc_block_init(dc_block_t *d) { d->mean = 0.0f; }

static inline float dc_block_step(dc_block_t *d, float x)
{
    const float beta = 0.0015f; // ajuste leve
    d->mean += beta * (x - d->mean);
    return x - d->mean;
}

// =============================
// Goertzel: bins pré-calculados
// =============================
typedef struct {
    float f;      // frequência em Hz
    float coeff;  // 2*cos(2*pi*k/N)
} goertzel_bin_t;

static goertzel_bin_t g_bins[MAX_BINS];
static int g_bins_count = 0;

static float goertzel_power_coeff(const float *x, float coeff)
{
    float s1 = 0.0f;
    float s2 = 0.0f;

    for (int n = 0; n < FRAME_N; n++) {
        float s0 = x[n] + coeff * s1 - s2;
        s2 = s1;
        s1 = s0;
    }

    return (s1*s1) + (s2*s2) - (coeff*s1*s2);
}

static inline float coeff_for_freq(float f)
{
    const float fs = (float)FS_HZ;
    int k = (int)(0.5f + ((float)FRAME_N * f) / fs);
    float w = (2.0f * (float)M_PI * (float)k) / (float)FRAME_N;
    return 2.0f * cosf(w);
}

static inline float power_at_freq(const float *frame, float f)
{
    float c = coeff_for_freq(f);
    return goertzel_power_coeff(frame, c);
}

static void goertzel_bins_init(void)
{
    const float fs = (float)FS_HZ;
    const float df = fs / (float)FRAME_N; // ~31.25 Hz
    const float step = 2.0f * df;         // ~62.5 Hz

    g_bins_count = 0;

    for (float f = F_MIN; f <= F_MAX && g_bins_count < MAX_BINS; f += step) {
        int k = (int)(0.5f + ((float)FRAME_N * f) / fs);
        float w = (2.0f * (float)M_PI * (float)k) / (float)FRAME_N;

        g_bins[g_bins_count].f = f;
        g_bins[g_bins_count].coeff = 2.0f * cosf(w);
        g_bins_count++;
    }
}

static float find_best_f0_fast(const float *frame, float *p0_out)
{
    float best_f = g_bins[0].f;
    float best_p = -1.0f;

    for (int i = 0; i < g_bins_count; i++) {
        float p = goertzel_power_coeff(frame, g_bins[i].coeff);
        if (p > best_p) {
            best_p = p;
            best_f = g_bins[i].f;
        }
    }

    if (p0_out) *p0_out = best_p;
    return best_f;
}

// =============================
// Energia do frame (gate)
// =============================
static float frame_energy(const float *x)
{
    float acc = 0.0f;
    for (int i = 0; i < FRAME_N; i++) acc += x[i] * x[i];
    return acc / (float)FRAME_N;
}

// =============================
// Score do assobio (fundamental + harmônicos)
// =============================
static float compute_whistle_score(const float *frame, float *f0_out)
{
    const float eps = 1e-9f;

    float E = frame_energy(frame);
    if (E < E_MIN) {
        if (f0_out) *f0_out = 0.0f;
        return 0.0f;
    }

    float p0 = 0.0f;
    float f0 = find_best_f0_fast(frame, &p0);

    const float nyq = 0.5f * (float)FS_HZ;

    float f2 = 2.0f * f0;
    float f3 = 3.0f * f0;

    float p2 = (f2 <= nyq) ? power_at_freq(frame, f2) : 0.0f;
    float p3 = (f3 <= nyq) ? power_at_freq(frame, f3) : 0.0f;

    float s0 = p0 / (E + eps);
    float r2 = p2 / (p0 + eps);
    float r3 = p3 / (p0 + eps);

    float S = s0 + (L2 * r2) + (L3 * r3);

    if (f0_out) *f0_out = f0;
    return S;
}

// =============================
// Hardware e tempo real
// =============================
static repeating_timer_t g_timer;
static frame_buf_t g_frame;
static dc_block_t  g_dc;

static float g_S_filt = 0.0f;
static bool  g_state  = false;

static inline float read_adc_normalized(void)
{
    uint16_t raw = adc_read(); // 0..4095
    return ((float)raw - 2048.0f) / 2048.0f;
}

static bool sample_timer_cb(repeating_timer_t *rt)
{
    (void)rt;

    float x = read_adc_normalized();
    x = dc_block_step(&g_dc, x);

    frame_buf_push(&g_frame, x);
    return true;
}

static void hw_init(void)
{
    stdio_init_all();

    // LED
    gpio_init(LED_GPIO);
    gpio_set_dir(LED_GPIO, GPIO_OUT);
    gpio_put(LED_GPIO, 0);

    // ADC
    adc_init();
    adc_gpio_init(ADC_GPIO);
    adc_select_input(ADC_CH);
}

static void update_led_from_score(float S, float f0)
{
    // EMA do score
    g_S_filt = EMA_ALPHA * g_S_filt + (1.0f - EMA_ALPHA) * S;

    // sanity: faixa típica do assobio
    bool f_ok = (f0 >= 700.0f && f0 <= 3500.0f);

    if (!g_state) {
        if (f_ok && g_S_filt > S_ON) g_state = true;
    } else {
        if (!f_ok || g_S_filt < S_OFF) g_state = false;
    }

    gpio_put(LED_GPIO, g_state ? 1 : 0);
}

static void process_frame_if_ready(void)
{
    if (!g_frame.full) return;
    g_frame.full = false;

    float f0 = 0.0f;
    float S = compute_whistle_score(g_frame.data, &f0);

    update_led_from_score(S, f0);
}

// =============================
// main
// =============================
int main(void)
{
    hw_init();

    frame_buf_init(&g_frame);
    dc_block_init(&g_dc);

    goertzel_bins_init();

    // amostragem estável
    const int sample_period_us = 1000000 / FS_HZ;
    add_repeating_timer_us(-sample_period_us, sample_timer_cb, NULL, &g_timer);

    while (true) {
        process_frame_if_ready();
        sleep_ms(1);
    }
}

/**
 * main.c — Detector de assobio com Goertzel (RP2040 / BitDogLab)
 *
 * Estratégia:
 * ADC -> DC-block -> frame 256 -> varredura Goertzel (800..3000 Hz)
 * -> candidato f0 -> harmônicos 2f0/3f0 -> score -> EMA + histerese -> LED
 *
 * Pico SDK libs: pico_stdlib, hardware_adc, pico_time
 */

#include <stdio.h>
#include <stdbool.h>
#include <math.h>

#include "pico/stdlib.h"
#include "pico/time.h"
#include "hardware/adc.h"

// =============================
// Configurações principais
// =============================
#define FS_HZ       8000
#define FRAME_N     256

// ADC
#define ADC_GPIO    28
#define ADC_CH      0

// LED: no Pico costuma ser 25; na BitDogLab pode variar
#define LED_GPIO    13

// Varredura em Hz
#define F_MIN       800.0f
#define F_MAX       3000.0f

// Quantidade máxima de bins na varredura
#define MAX_BINS    64

// Gate por energia (ajuste)
#define E_MIN       0.0008f

// EMA do score
#define EMA_ALPHA   0.85f

// Histerese do score (ajuste)
#define S_ON        25.0f
#define S_OFF       18.0f

// Pesos do score (ajuste)
#define L2          0.7f
#define L3          0.35f

// =============================
// Buffer de frame
// =============================
typedef struct {
    float data[FRAME_N];
    uint32_t idx;
    bool full;
} frame_buf_t;

static inline void frame_buf_init(frame_buf_t *b) {
    b->idx = 0;
    b->full = false;
}

static inline void frame_buf_push(frame_buf_t *b, float v) {
    b->data[b->idx++] = v;
    if (b->idx >= FRAME_N) {
        b->idx = 0;
        b->full = true;
    }
}

// =============================
// DC-block (remoção de offset)
// =============================
typedef struct {
    float mean;
} dc_block_t;

static inline void dc_block_init(dc_block_t *d) { d->mean = 0.0f; }

static inline float dc_block_step(dc_block_t *d, float x)
{
    const float beta = 0.0015f; // ajuste leve
    d->mean += beta * (x - d->mean);
    return x - d->mean;
}

// =============================
// Goertzel: bins pré-calculados
// =============================
typedef struct {
    float f;      // frequência em Hz
    float coeff;  // 2*cos(2*pi*k/N)
} goertzel_bin_t;

static goertzel_bin_t g_bins[MAX_BINS];
static int g_bins_count = 0;

static float goertzel_power_coeff(const float *x, float coeff)
{
    float s1 = 0.0f;
    float s2 = 0.0f;

    for (int n = 0; n < FRAME_N; n++) {
        float s0 = x[n] + coeff * s1 - s2;
        s2 = s1;
        s1 = s0;
    }

    return (s1*s1) + (s2*s2) - (coeff*s1*s2);
}

static inline float coeff_for_freq(float f)
{
    const float fs = (float)FS_HZ;
    int k = (int)(0.5f + ((float)FRAME_N * f) / fs);
    float w = (2.0f * (float)M_PI * (float)k) / (float)FRAME_N;
    return 2.0f * cosf(w);
}

static inline float power_at_freq(const float *frame, float f)
{
    float c = coeff_for_freq(f);
    return goertzel_power_coeff(frame, c);
}

static void goertzel_bins_init(void)
{
    const float fs = (float)FS_HZ;
    const float df = fs / (float)FRAME_N; // ~31.25 Hz
    const float step = 2.0f * df;         // ~62.5 Hz

    g_bins_count = 0;

    for (float f = F_MIN; f <= F_MAX && g_bins_count < MAX_BINS; f += step) {
        int k = (int)(0.5f + ((float)FRAME_N * f) / fs);
        float w = (2.0f * (float)M_PI * (float)k) / (float)FRAME_N;

        g_bins[g_bins_count].f = f;
        g_bins[g_bins_count].coeff = 2.0f * cosf(w);
        g_bins_count++;
    }
}

static float find_best_f0_fast(const float *frame, float *p0_out)
{
    float best_f = g_bins[0].f;
    float best_p = -1.0f;

    for (int i = 0; i < g_bins_count; i++) {
        float p = goertzel_power_coeff(frame, g_bins[i].coeff);
        if (p > best_p) {
            best_p = p;
            best_f = g_bins[i].f;
        }
    }

    if (p0_out) *p0_out = best_p;
    return best_f;
}

// =============================
// Energia do frame (gate)
// =============================
static float frame_energy(const float *x)
{
    float acc = 0.0f;
    for (int i = 0; i < FRAME_N; i++) acc += x[i] * x[i];
    return acc / (float)FRAME_N;
}

// =============================
// Score do assobio (fundamental + harmônicos)
// =============================
static float compute_whistle_score(const float *frame, float *f0_out)
{
    const float eps = 1e-9f;

    float E = frame_energy(frame);
    if (E < E_MIN) {
        if (f0_out) *f0_out = 0.0f;
        return 0.0f;
    }

    float p0 = 0.0f;
    float f0 = find_best_f0_fast(frame, &p0);

    const float nyq = 0.5f * (float)FS_HZ;

    float f2 = 2.0f * f0;
    float f3 = 3.0f * f0;

    float p2 = (f2 <= nyq) ? power_at_freq(frame, f2) : 0.0f;
    float p3 = (f3 <= nyq) ? power_at_freq(frame, f3) : 0.0f;

    float s0 = p0 / (E + eps);
    float r2 = p2 / (p0 + eps);
    float r3 = p3 / (p0 + eps);

    float S = s0 + (L2 * r2) + (L3 * r3);

    if (f0_out) *f0_out = f0;
    return S;
}

// =============================
// Hardware e tempo real
// =============================
static repeating_timer_t g_timer;
static frame_buf_t g_frame;
static dc_block_t  g_dc;

static float g_S_filt = 0.0f;
static bool  g_state  = false;

static inline float read_adc_normalized(void)
{
    uint16_t raw = adc_read(); // 0..4095
    return ((float)raw - 2048.0f) / 2048.0f;
}

static bool sample_timer_cb(repeating_timer_t *rt)
{
    (void)rt;

    float x = read_adc_normalized();
    x = dc_block_step(&g_dc, x);

    frame_buf_push(&g_frame, x);
    return true;
}

static void hw_init(void)
{
    stdio_init_all();

    // LED
    gpio_init(LED_GPIO);
    gpio_set_dir(LED_GPIO, GPIO_OUT);
    gpio_put(LED_GPIO, 0);

    // ADC
    adc_init();
    adc_gpio_init(ADC_GPIO);
    adc_select_input(ADC_CH);
}

static void update_led_from_score(float S, float f0)
{
    // EMA do score
    g_S_filt = EMA_ALPHA * g_S_filt + (1.0f - EMA_ALPHA) * S;

    // sanity: faixa típica do assobio
    bool f_ok = (f0 >= 700.0f && f0 <= 3500.0f);

    if (!g_state) {
        if (f_ok && g_S_filt > S_ON) g_state = true;
    } else {
        if (!f_ok || g_S_filt < S_OFF) g_state = false;
    }

    gpio_put(LED_GPIO, g_state ? 1 : 0);
}

static void process_frame_if_ready(void)
{
    if (!g_frame.full) return;
    g_frame.full = false;

    float f0 = 0.0f;
    float S = compute_whistle_score(g_frame.data, &f0);

    update_led_from_score(S, f0);
}

// =============================
// main
// =============================
int main(void)
{
    hw_init();

    frame_buf_init(&g_frame);
    dc_block_init(&g_dc);

    goertzel_bins_init();

    // amostragem estável
    const int sample_period_us = 1000000 / FS_HZ;
    add_repeating_timer_us(-sample_period_us, sample_timer_cb, NULL, &g_timer);

    while (true) {
        process_frame_if_ready();
        sleep_ms(1);
    }
}