123 lines
4 KiB
C
123 lines
4 KiB
C
// This file is part of the CircuitPython project: https://circuitpython.org
|
|
//
|
|
// SPDX-FileCopyrightText: Copyright (c) 2023 Jeff Epler for Adafruit Industries
|
|
//
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include <math.h>
|
|
#include "shared-bindings/synthio/Biquad.h"
|
|
#include "shared-module/synthio/Biquad.h"
|
|
|
|
mp_obj_t common_hal_synthio_new_lpf(mp_float_t w0, mp_float_t Q) {
|
|
mp_float_t s = MICROPY_FLOAT_C_FUN(sin)(w0);
|
|
mp_float_t c = MICROPY_FLOAT_C_FUN(cos)(w0);
|
|
mp_float_t alpha = s / (2 * Q);
|
|
mp_float_t a0 = 1 + alpha;
|
|
mp_float_t a1 = -2 * c;
|
|
mp_float_t a2 = 1 - alpha;
|
|
mp_float_t b0 = (1 - c) / 2;
|
|
mp_float_t b1 = 1 - c;
|
|
mp_float_t b2 = (1 - c) / 2;
|
|
|
|
mp_obj_t out_args[] = {
|
|
mp_obj_new_float(a1 / a0),
|
|
mp_obj_new_float(a2 / a0),
|
|
mp_obj_new_float(b0 / a0),
|
|
mp_obj_new_float(b1 / a0),
|
|
mp_obj_new_float(b2 / a0),
|
|
};
|
|
|
|
return namedtuple_make_new((const mp_obj_type_t *)&synthio_biquad_type_obj, MP_ARRAY_SIZE(out_args), 0, out_args);
|
|
}
|
|
|
|
mp_obj_t common_hal_synthio_new_hpf(mp_float_t w0, mp_float_t Q) {
|
|
mp_float_t s = MICROPY_FLOAT_C_FUN(sin)(w0);
|
|
mp_float_t c = MICROPY_FLOAT_C_FUN(cos)(w0);
|
|
mp_float_t alpha = s / (2 * Q);
|
|
mp_float_t a0 = 1 + alpha;
|
|
mp_float_t a1 = -2 * c;
|
|
mp_float_t a2 = 1 - alpha;
|
|
mp_float_t b0 = (1 + c) / 2;
|
|
mp_float_t b1 = -(1 + c);
|
|
mp_float_t b2 = (1 + c) / 2;
|
|
|
|
mp_obj_t out_args[] = {
|
|
mp_obj_new_float(a1 / a0),
|
|
mp_obj_new_float(a2 / a0),
|
|
mp_obj_new_float(b0 / a0),
|
|
mp_obj_new_float(b1 / a0),
|
|
mp_obj_new_float(b2 / a0),
|
|
};
|
|
|
|
return namedtuple_make_new((const mp_obj_type_t *)&synthio_biquad_type_obj, MP_ARRAY_SIZE(out_args), 0, out_args);
|
|
}
|
|
|
|
mp_obj_t common_hal_synthio_new_bpf(mp_float_t w0, mp_float_t Q) {
|
|
mp_float_t s = MICROPY_FLOAT_C_FUN(sin)(w0);
|
|
mp_float_t c = MICROPY_FLOAT_C_FUN(cos)(w0);
|
|
mp_float_t alpha = s / (2 * Q);
|
|
mp_float_t a0 = 1 + alpha;
|
|
mp_float_t a1 = -2 * c;
|
|
mp_float_t a2 = 1 - alpha;
|
|
mp_float_t b0 = alpha;
|
|
mp_float_t b1 = 0;
|
|
mp_float_t b2 = -alpha;
|
|
|
|
mp_obj_t out_args[] = {
|
|
mp_obj_new_float(a1 / a0),
|
|
mp_obj_new_float(a2 / a0),
|
|
mp_obj_new_float(b0 / a0),
|
|
mp_obj_new_float(b1 / a0),
|
|
mp_obj_new_float(b2 / a0),
|
|
};
|
|
|
|
return namedtuple_make_new((const mp_obj_type_t *)&synthio_biquad_type_obj, MP_ARRAY_SIZE(out_args), 0, out_args);
|
|
}
|
|
|
|
#define BIQUAD_SHIFT (15)
|
|
static int32_t biquad_scale_arg_obj(mp_obj_t arg) {
|
|
return (int32_t)MICROPY_FLOAT_C_FUN(round)(MICROPY_FLOAT_C_FUN(ldexp)(mp_obj_get_float(arg), BIQUAD_SHIFT));
|
|
}
|
|
void synthio_biquad_filter_assign(biquad_filter_state *st, mp_obj_t biquad_obj) {
|
|
if (biquad_obj != mp_const_none) {
|
|
mp_arg_validate_type(biquad_obj, (const mp_obj_type_t *)&synthio_biquad_type_obj, MP_QSTR_filter);
|
|
mp_obj_tuple_t *biquad = (mp_obj_tuple_t *)MP_OBJ_TO_PTR(biquad_obj);
|
|
st->a1 = biquad_scale_arg_obj(biquad->items[0]);
|
|
st->a2 = biquad_scale_arg_obj(biquad->items[1]);
|
|
st->b0 = biquad_scale_arg_obj(biquad->items[2]);
|
|
st->b1 = biquad_scale_arg_obj(biquad->items[3]);
|
|
st->b2 = biquad_scale_arg_obj(biquad->items[4]);
|
|
}
|
|
}
|
|
|
|
void synthio_biquad_filter_reset(biquad_filter_state *st) {
|
|
memset(&st->x, 0, 4 * sizeof(int16_t));
|
|
}
|
|
|
|
void synthio_biquad_filter_samples(biquad_filter_state *st, int32_t *buffer, size_t n_samples) {
|
|
int32_t a1 = st->a1;
|
|
int32_t a2 = st->a2;
|
|
int32_t b0 = st->b0;
|
|
int32_t b1 = st->b1;
|
|
int32_t b2 = st->b2;
|
|
|
|
int32_t x0 = st->x[0];
|
|
int32_t x1 = st->x[1];
|
|
int32_t y0 = st->y[0];
|
|
int32_t y1 = st->y[1];
|
|
|
|
for (size_t n = n_samples; n; --n, ++buffer) {
|
|
int32_t input = *buffer;
|
|
int32_t output = (b0 * input + b1 * x0 + b2 * x1 - a1 * y0 - a2 * y1 + (1 << (BIQUAD_SHIFT - 1))) >> BIQUAD_SHIFT;
|
|
|
|
x1 = x0;
|
|
x0 = input;
|
|
y1 = y0;
|
|
y0 = output;
|
|
*buffer = output;
|
|
}
|
|
st->x[0] = x0;
|
|
st->x[1] = x1;
|
|
st->y[0] = y0;
|
|
st->y[1] = y1;
|
|
}
|