Filtro Digital com Séries de Taylor e Cepstrum para Detecção de Assobio em RP2040

7 — Código completo e funcional (BitDogLab / RP2040) — filtro Taylor + cepstrum + LED

Abaixo está um main.c único, pronto para compilar com o Pico SDK, usando:

ADC (GPIO26 / ADC0 por padrão)
Amostragem em 8 kHz via repeating_timer
Filtro digital baseado em Séries de Taylor
Cepstrum: FFT → log(|X|) com ln aproximado por Taylor → IFFT
Janela Hann + gate por energia + EMA + histerese
LED (GPIO25 por padrão)

Ajustes esperados por placa: se a BitDogLab usar outro GPIO para LED ou outro ADC, é só trocar os #define.

/**
 * main.c — Detector de assobio (RP2040 / BitDogLab)
 *
 * Pipeline:
 * ADC -> remove offset -> Filtro passa-faixa (Taylor) -> frame 256 -> Hann ->
 * FFT -> log(|X|) via ln_approx (Taylor) -> IFFT -> cepstrum -> pico (q) ->
 * decisão com histerese + EMA -> LED
 *
 * Compilar com Pico SDK.
 */

#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 Vermelho na BitDogLb
#define LED_GPIO    13

// Detector: limites e filtros
#define Q_MIN       3
#define Q_MAX       20

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

// Histerese (ajuste prático)
#define TH_ON       0.35f
#define TH_OFF      0.25f

// EMA do pico
#define EMA_ALPHA   0.85f

// Filtro Taylor (ajuste prático)
#define TAYLOR_A1   0.5f
#define TAYLOR_A2   0.25f

// =============================
// Tipos e utilitários
// =============================
typedef struct {
    float re;
    float im;
} cpx_t;

static inline cpx_t cpx_add(cpx_t a, cpx_t b) { return (cpx_t){a.re + b.re, a.im + b.im}; }
static inline cpx_t cpx_sub(cpx_t a, cpx_t b) { return (cpx_t){a.re - b.re, a.im - b.im}; }
static inline cpx_t cpx_mul(cpx_t a, cpx_t b) {
    return (cpx_t){
        a.re*b.re - a.im*b.im,
        a.re*b.im + a.im*b.re
    };
}

// =============================
// 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;
    }
}

// =============================
// Filtro passa-faixa por Taylor
// y[n] = x[n] - a1*(x[n]-x[n-1]) - a2*(x[n]-2x[n-1]+x[n-2])
// =============================
typedef struct {
    float x1;
    float x2;
} taylor_filter_t;

static inline float taylor_bandpass_step(taylor_filter_t *f, float x)
{
    float dx1 = x - f->x1;
    float dx2 = x - 2.0f * f->x1 + f->x2;

    float y = x - (TAYLOR_A1 * dx1) - (TAYLOR_A2 * dx2);

    f->x2 = f->x1;
    f->x1 = x;
    return y;
}

// =============================
// Série de Taylor para ln(1+u), u em [0,1)
// ln(1+u) ≈ u - u²/2 + u³/3 - u⁴/4 + u⁵/5
// e redução: ln(x)=ln(m)+e*ln2 com frexpf
// =============================
static inline float ln_taylor_1pu(float u)
{
    float u2 = u * u;
    float u3 = u2 * u;
    float u4 = u2 * u2;
    float u5 = u4 * u;
    return (u)
         - (0.5f * u2)
         + (0.3333333f * u3)
         - (0.25f * u4)
         + (0.2f * u5);
}

static inline float ln_approx(float x)
{
    // x > 0
    int e = 0;
    float m = frexpf(x, &e); // x = m*2^e, m in [0.5,1)
    float m2 = m * 2.0f;     // m2 in [1,2)
    int e2 = e - 1;

    const float LN2 = 0.69314718056f;
    float u = m2 - 1.0f;     // u in [0,1)
    return ln_taylor_1pu(u) + ((float)e2) * LN2;
}

// =============================
// FFT radix-2 (N=256)
// =============================
static cpx_t g_tw[FRAME_N/2];

static void fft_init_twiddles(void)
{
    for (int k = 0; k < FRAME_N/2; k++) {
        float ang = -2.0f * (float)M_PI * (float)k / (float)FRAME_N;
        g_tw[k].re = cosf(ang);
        g_tw[k].im = sinf(ang);
    }
}

static inline uint32_t bitrev8(uint32_t x)
{
    x = ((x & 0xF0u) >> 4) | ((x & 0x0Fu) << 4);
    x = ((x & 0xCCu) >> 2) | ((x & 0x33u) << 2);
    x = ((x & 0xAAu) >> 1) | ((x & 0x55u) << 1);
    return x;
}

static void fft_bitreverse(cpx_t *a)
{
    for (uint32_t i = 0; i < FRAME_N; i++) {
        uint32_t j = bitrev8(i);
        if (j > i) {
            cpx_t tmp = a[i];
            a[i] = a[j];
            a[j] = tmp;
        }
    }
}

static void fft_forward(cpx_t *a)
{
    fft_bitreverse(a);

    for (uint32_t len = 2; len <= FRAME_N; len <<= 1) {
        uint32_t half = len >> 1;
        uint32_t step = (FRAME_N / len);

        for (uint32_t i = 0; i < FRAME_N; i += len) {
            uint32_t tw = 0;
            for (uint32_t j = 0; j < half; j++) {
                cpx_t u = a[i + j];
                cpx_t v = cpx_mul(a[i + j + half], g_tw[tw]);

                a[i + j]        = cpx_add(u, v);
                a[i + j + half] = cpx_sub(u, v);

                tw += step;
            }
        }
    }
}

static void fft_inverse(cpx_t *a)
{
    for (int i = 0; i < FRAME_N; i++) a[i].im = -a[i].im;
    fft_forward(a);
    for (int i = 0; i < FRAME_N; i++) {
        a[i].re =  a[i].re / (float)FRAME_N;
        a[i].im = -a[i].im / (float)FRAME_N;
    }
}

// =============================
// Janela Hann
// =============================
static float g_hann[FRAME_N];

static void window_init_hann(void)
{
    for (int n = 0; n < FRAME_N; n++) {
        float a = 2.0f * (float)M_PI * (float)n / (float)(FRAME_N - 1);
        g_hann[n] = 0.5f * (1.0f - cosf(a));
    }
}

// =============================
// Cepstrum + detecção
// =============================
static void build_log_magnitude(cpx_t *spec)
{
    const float eps = 1e-12f;

    for (int k = 0; k < FRAME_N; k++) {
        float p = spec[k].re*spec[k].re + spec[k].im*spec[k].im + eps;
        float ln_p = ln_approx(p);
        float ln_mag = 0.5f * ln_p; // ln(sqrt(p)) = 0.5 ln(p)

        spec[k].re = ln_mag;
        spec[k].im = 0.0f;
    }
}

static float compute_cepstrum_peak(const float *frame, int *best_q_out)
{
    static cpx_t buf[FRAME_N];

    // Carrega frame com Hann
    for (int i = 0; i < FRAME_N; i++) {
        buf[i].re = frame[i] * g_hann[i];
        buf[i].im = 0.0f;
    }

    fft_forward(buf);
    build_log_magnitude(buf);
    fft_inverse(buf);

    float best = -1e9f;
    int best_q = Q_MIN;

    for (int q = Q_MIN; q <= Q_MAX; q++) {
        float v = buf[q].re;
        if (v > best) {
            best = v;
            best_q = q;
        }
    }

    if (best_q_out) *best_q_out = best_q;
    return best;
}

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;
}

// =============================
// Hardware / amostragem
// =============================
static repeating_timer_t g_timer;
static taylor_filter_t   g_filter = {0};
static frame_buf_t       g_frame;

static bool  g_whistle_state = false;
static float g_peak_filt = 0.0f;

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();
    float y = taylor_bandpass_step(&g_filter, x);
    frame_buf_push(&g_frame, y);

    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 whistle_update_led(float peak, int q)
{
    // EMA do pico
    g_peak_filt = EMA_ALPHA * g_peak_filt + (1.0f - EMA_ALPHA) * peak;

    float f0 = (float)FS_HZ / (float)q;
    bool f_ok = (f0 >= 700.0f && f0 <= 3500.0f);

    if (!g_whistle_state) {
        if (f_ok && g_peak_filt > TH_ON) g_whistle_state = true;
    } else {
        if (!f_ok || g_peak_filt < TH_OFF) g_whistle_state = false;
    }

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

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

    float E = frame_energy(g_frame.data);
    if (E < E_MIN) {
        // “silêncio”: ajuda a desligar com suavidade
        whistle_update_led(0.0f, 10);
        return;
    }

    int q = 0;
    float peak = compute_cepstrum_peak(g_frame.data, &q);
    whistle_update_led(peak, q);
}

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

    frame_buf_init(&g_frame);
    fft_init_twiddles();
    window_init_hann();

    // amostragem estável: 1/FS
    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 (RP2040 / BitDogLab)
 *
 * Pipeline:
 * ADC -> remove offset -> Filtro passa-faixa (Taylor) -> frame 256 -> Hann ->
 * FFT -> log(|X|) via ln_approx (Taylor) -> IFFT -> cepstrum -> pico (q) ->
 * decisão com histerese + EMA -> LED
 *
 * Compilar com Pico SDK.
 */

#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 Vermelho na BitDogLb
#define LED_GPIO    13

// Detector: limites e filtros
#define Q_MIN       3
#define Q_MAX       20

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

// Histerese (ajuste prático)
#define TH_ON       0.35f
#define TH_OFF      0.25f

// EMA do pico
#define EMA_ALPHA   0.85f

// Filtro Taylor (ajuste prático)
#define TAYLOR_A1   0.5f
#define TAYLOR_A2   0.25f

// =============================
// Tipos e utilitários
// =============================
typedef struct {
    float re;
    float im;
} cpx_t;

static inline cpx_t cpx_add(cpx_t a, cpx_t b) { return (cpx_t){a.re + b.re, a.im + b.im}; }
static inline cpx_t cpx_sub(cpx_t a, cpx_t b) { return (cpx_t){a.re - b.re, a.im - b.im}; }
static inline cpx_t cpx_mul(cpx_t a, cpx_t b) {
    return (cpx_t){
        a.re*b.re - a.im*b.im,
        a.re*b.im + a.im*b.re
    };
}

// =============================
// 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;
    }
}

// =============================
// Filtro passa-faixa por Taylor
// y[n] = x[n] - a1*(x[n]-x[n-1]) - a2*(x[n]-2x[n-1]+x[n-2])
// =============================
typedef struct {
    float x1;
    float x2;
} taylor_filter_t;

static inline float taylor_bandpass_step(taylor_filter_t *f, float x)
{
    float dx1 = x - f->x1;
    float dx2 = x - 2.0f * f->x1 + f->x2;

    float y = x - (TAYLOR_A1 * dx1) - (TAYLOR_A2 * dx2);

    f->x2 = f->x1;
    f->x1 = x;
    return y;
}

// =============================
// Série de Taylor para ln(1+u), u em [0,1)
// ln(1+u) ≈ u - u²/2 + u³/3 - u⁴/4 + u⁵/5
// e redução: ln(x)=ln(m)+e*ln2 com frexpf
// =============================
static inline float ln_taylor_1pu(float u)
{
    float u2 = u * u;
    float u3 = u2 * u;
    float u4 = u2 * u2;
    float u5 = u4 * u;
    return (u)
         - (0.5f * u2)
         + (0.3333333f * u3)
         - (0.25f * u4)
         + (0.2f * u5);
}

static inline float ln_approx(float x)
{
    // x > 0
    int e = 0;
    float m = frexpf(x, &e); // x = m*2^e, m in [0.5,1)
    float m2 = m * 2.0f;     // m2 in [1,2)
    int e2 = e - 1;

    const float LN2 = 0.69314718056f;
    float u = m2 - 1.0f;     // u in [0,1)
    return ln_taylor_1pu(u) + ((float)e2) * LN2;
}

// =============================
// FFT radix-2 (N=256)
// =============================
static cpx_t g_tw[FRAME_N/2];

static void fft_init_twiddles(void)
{
    for (int k = 0; k < FRAME_N/2; k++) {
        float ang = -2.0f * (float)M_PI * (float)k / (float)FRAME_N;
        g_tw[k].re = cosf(ang);
        g_tw[k].im = sinf(ang);
    }
}

static inline uint32_t bitrev8(uint32_t x)
{
    x = ((x & 0xF0u) >> 4) | ((x & 0x0Fu) << 4);
    x = ((x & 0xCCu) >> 2) | ((x & 0x33u) << 2);
    x = ((x & 0xAAu) >> 1) | ((x & 0x55u) << 1);
    return x;
}

static void fft_bitreverse(cpx_t *a)
{
    for (uint32_t i = 0; i < FRAME_N; i++) {
        uint32_t j = bitrev8(i);
        if (j > i) {
            cpx_t tmp = a[i];
            a[i] = a[j];
            a[j] = tmp;
        }
    }
}

static void fft_forward(cpx_t *a)
{
    fft_bitreverse(a);

    for (uint32_t len = 2; len <= FRAME_N; len <<= 1) {
        uint32_t half = len >> 1;
        uint32_t step = (FRAME_N / len);

        for (uint32_t i = 0; i < FRAME_N; i += len) {
            uint32_t tw = 0;
            for (uint32_t j = 0; j < half; j++) {
                cpx_t u = a[i + j];
                cpx_t v = cpx_mul(a[i + j + half], g_tw[tw]);

                a[i + j]        = cpx_add(u, v);
                a[i + j + half] = cpx_sub(u, v);

                tw += step;
            }
        }
    }
}

static void fft_inverse(cpx_t *a)
{
    for (int i = 0; i < FRAME_N; i++) a[i].im = -a[i].im;
    fft_forward(a);
    for (int i = 0; i < FRAME_N; i++) {
        a[i].re =  a[i].re / (float)FRAME_N;
        a[i].im = -a[i].im / (float)FRAME_N;
    }
}

// =============================
// Janela Hann
// =============================
static float g_hann[FRAME_N];

static void window_init_hann(void)
{
    for (int n = 0; n < FRAME_N; n++) {
        float a = 2.0f * (float)M_PI * (float)n / (float)(FRAME_N - 1);
        g_hann[n] = 0.5f * (1.0f - cosf(a));
    }
}

// =============================
// Cepstrum + detecção
// =============================
static void build_log_magnitude(cpx_t *spec)
{
    const float eps = 1e-12f;

    for (int k = 0; k < FRAME_N; k++) {
        float p = spec[k].re*spec[k].re + spec[k].im*spec[k].im + eps;
        float ln_p = ln_approx(p);
        float ln_mag = 0.5f * ln_p; // ln(sqrt(p)) = 0.5 ln(p)

        spec[k].re = ln_mag;
        spec[k].im = 0.0f;
    }
}

static float compute_cepstrum_peak(const float *frame, int *best_q_out)
{
    static cpx_t buf[FRAME_N];

    // Carrega frame com Hann
    for (int i = 0; i < FRAME_N; i++) {
        buf[i].re = frame[i] * g_hann[i];
        buf[i].im = 0.0f;
    }

    fft_forward(buf);
    build_log_magnitude(buf);
    fft_inverse(buf);

    float best = -1e9f;
    int best_q = Q_MIN;

    for (int q = Q_MIN; q <= Q_MAX; q++) {
        float v = buf[q].re;
        if (v > best) {
            best = v;
            best_q = q;
        }
    }

    if (best_q_out) *best_q_out = best_q;
    return best;
}

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;
}

// =============================
// Hardware / amostragem
// =============================
static repeating_timer_t g_timer;
static taylor_filter_t   g_filter = {0};
static frame_buf_t       g_frame;

static bool  g_whistle_state = false;
static float g_peak_filt = 0.0f;

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();
    float y = taylor_bandpass_step(&g_filter, x);
    frame_buf_push(&g_frame, y);

    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 whistle_update_led(float peak, int q)
{
    // EMA do pico
    g_peak_filt = EMA_ALPHA * g_peak_filt + (1.0f - EMA_ALPHA) * peak;

    float f0 = (float)FS_HZ / (float)q;
    bool f_ok = (f0 >= 700.0f && f0 <= 3500.0f);

    if (!g_whistle_state) {
        if (f_ok && g_peak_filt > TH_ON) g_whistle_state = true;
    } else {
        if (!f_ok || g_peak_filt < TH_OFF) g_whistle_state = false;
    }

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

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

    float E = frame_energy(g_frame.data);
    if (E < E_MIN) {
        // “silêncio”: ajuda a desligar com suavidade
        whistle_update_led(0.0f, 10);
        return;
    }

    int q = 0;
    float peak = compute_cepstrum_peak(g_frame.data, &q);
    whistle_update_led(peak, q);
}

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

    frame_buf_init(&g_frame);
    fft_init_twiddles();
    window_init_hann();

    // amostragem estável: 1/FS
    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);
    }
}

7.1 Como compilar (visão prática)

Coloque esse main.c dentro do seu projeto Pico SDK.
Garanta que seu CMakeLists.txt linke:
- pico_stdlib
- hardware_adc
- pico_time

Exemplo mínimo (se você já tem um projeto Pico):

target_link_libraries(seu_target
    pico_stdlib
    hardware_adc
    pico_time
)

target_link_libraries(seu_target
    pico_stdlib
    hardware_adc
    pico_time
)

7.2 Ajustes que você provavelmente vai fazer na BitDogLab

LED_GPIO pode mudar (dependendo do LED da placa)
O microfone pode não estar diretamente no ADC0/GPIO26
O ganho/offset do sinal pode exigir:
- Ajustar E_MIN
- Ajustar TH_ON / TH_OFF
- Ajustar TAYLOR_A1 / TAYLOR_A2

7 — Código completo e funcional (BitDogLab / RP2040) — filtro Taylor + cepstrum + LED

7.1 Como compilar (visão prática)

7.2 Ajustes que você provavelmente vai fazer na BitDogLab

Related Post

Detecção de Assobio com Goertzel e Aprendizado Estatístico no RP2040Detecção de Assobio com Goertzel e Aprendizado Estatístico no RP2040

Filtros Estatísticos em Sistemas Embarcados: EMA, Wiener e Kalman Aplicados ao RP2040Filtros Estatísticos em Sistemas Embarcados: EMA, Wiener e Kalman Aplicados ao RP2040

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