5 — Código completo e funcional (Goertzel + aprendizado estatístico) para BitDogLab / RP2040
A seguir está um main.c único, pronto para compilar com o Pico SDK, integrando Goertzel + extração de features + modelo estatístico adaptativo + LED.
O código mantém o mesmo padrão dos artigos anteriores: determinístico, leve, explicável e calibrável.
Ajustes esperados por placa: revise
LED_GPIO,ADC_GPIOeADC_CHconforme a BitDogLab utilizada.
/**
* main.c — Detector de assobio com Goertzel + Aprendizado Estatístico
* Plataforma: RP2040 (BitDogLab / Pico SDK)
*
* Pipeline:
* ADC -> DC-block -> frame (256)
* -> Goertzel (800..3000 Hz + harmônicos)
* -> vetor de features
* -> modelo estatístico online (media/variancia)
* -> decisão probabilística (histerese)
* -> LED
*/
#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
#define ADC_GPIO 28
#define ADC_CH 0
#define LED_GPIO 13
#define F_MIN 800.0f
#define F_MAX 3000.0f
#define MAX_BINS 64
#define E_MIN 0.0008f
// Estatística
#define ALPHA_STAT 0.02f
#define D_ON 9.0f
#define D_OFF 12.0f
// =============================
// 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
// =============================
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;
d->mean += beta * (x - d->mean);
return x - d->mean;
}
// =============================
// Goertzel (bins pré-calculados)
// =============================
typedef struct {
float f;
float coeff;
} 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, 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){
int k = (int)(0.5f + ((float)FRAME_N * f) / (float)FS_HZ);
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 *x, float f){
return goertzel_power_coeff(x, coeff_for_freq(f));
}
static void goertzel_bins_init(void){
float df = (float)FS_HZ / (float)FRAME_N;
float step = 2.0f * df;
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) / (float)FS_HZ);
float w = (2.0f * (float)M_PI * (float)k) / (float)FRAME_N;
g_bins[g_bins_count++] = (goertzel_bin_t){ f, 2.0f * cosf(w) };
}
}
static float find_best_f0(const float *x, 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(x, 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;
}
// =============================
// Features
// =============================
typedef struct {
float f1, f2, f3, f4;
} feature_vec_t;
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;
}
// =============================
// Modelo estatístico
// =============================
typedef struct { float mean, var; } stat_feat_t;
typedef struct {
stat_feat_t f1, f2, f3, f4;
} stat_model_t;
static inline void stat_feat_init(stat_feat_t *s, float m, float v){
s->mean = m; s->var = v;
}
static void stat_model_init(stat_model_t *m){
stat_feat_init(&m->f1, 0.5f, 0.1f);
stat_feat_init(&m->f2, 0.1f, 0.05f);
stat_feat_init(&m->f3, 0.05f, 0.05f);
stat_feat_init(&m->f4, 0.0f, 50.0f);
}
static inline void stat_update(stat_feat_t *s, float x){
float d = x - s->mean;
s->mean += ALPHA_STAT * d;
s->var = (1.0f - ALPHA_STAT) * s->var + ALPHA_STAT * (d * d);
}
static inline float zscore(const stat_feat_t *s, float x){
const float eps = 1e-6f;
return (x - s->mean) / sqrtf(s->var + eps);
}
static float stat_distance(const stat_model_t *m, const feature_vec_t *v){
float z1 = zscore(&m->f1, v->f1);
float z2 = zscore(&m->f2, v->f2);
float z3 = zscore(&m->f3, v->f3);
float z4 = zscore(&m->f4, v->f4);
return z1*z1 + z2*z2 + z3*z3 + z4*z4;
}
// =============================
// Hardware e tempo real
// =============================
static frame_buf_t g_frame;
static dc_block_t g_dc;
static stat_model_t g_model;
static bool g_state = false;
static float g_f0_prev = 0.0f;
static repeating_timer_t g_timer;
static inline float read_adc_norm(void){
uint16_t raw = adc_read();
return ((float)raw - 2048.0f) / 2048.0f;
}
static bool sample_timer_cb(repeating_timer_t *rt){
(void)rt;
float x = dc_block_step(&g_dc, read_adc_norm());
frame_buf_push(&g_frame, x);
return true;
}
static void hw_init(void){
stdio_init_all();
gpio_init(LED_GPIO);
gpio_set_dir(LED_GPIO, GPIO_OUT);
gpio_put(LED_GPIO, 0);
adc_init();
adc_gpio_init(ADC_GPIO);
adc_select_input(ADC_CH);
}
// =============================
// Processamento do frame
// =============================
static bool build_features(const float *x, feature_vec_t *v){
const float eps = 1e-9f;
float E = frame_energy(x);
if (E < E_MIN) return false;
float p0 = 0.0f;
float f0 = find_best_f0(x, &p0);
float nyq = 0.5f * (float)FS_HZ;
float p2 = (2.0f*f0 <= nyq) ? power_at_freq(x, 2.0f*f0) : 0.0f;
float p3 = (3.0f*f0 <= nyq) ? power_at_freq(x, 3.0f*f0) : 0.0f;
v->f1 = p0 / (E + eps);
v->f2 = p2 / (p0 + eps);
v->f3 = p3 / (p0 + eps);
v->f4 = fabsf(f0 - g_f0_prev);
g_f0_prev = f0;
return true;
}
static void process_frame(void){
if (!g_frame.full) return;
g_frame.full = false;
feature_vec_t v;
if (!build_features(g_frame.data, &v)){
g_state = false;
gpio_put(LED_GPIO, 0);
return;
}
float D = stat_distance(&g_model, &v);
bool new_state = g_state ? (D < D_OFF) : (D < D_ON);
g_state = new_state;
if (g_state){
stat_update(&g_model.f1, v.f1);
stat_update(&g_model.f2, v.f2);
stat_update(&g_model.f3, v.f3);
stat_update(&g_model.f4, v.f4);
}
gpio_put(LED_GPIO, g_state ? 1 : 0);
}
// =============================
// main
// =============================
int main(void){
hw_init();
frame_buf_init(&g_frame);
dc_block_init(&g_dc);
stat_model_init(&g_model);
goertzel_bins_init();
int period_us = 1000000 / FS_HZ;
add_repeating_timer_us(-period_us, sample_timer_cb, NULL, &g_timer);
while (true){
process_frame();
sleep_ms(1);
}
}
