130 lines
3.9 KiB
C
130 lines
3.9 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/BlockBiquad.h"
|
|
#include "shared-module/synthio/BlockBiquad.h"
|
|
#include "shared-module/synthio/block.h"
|
|
|
|
typedef struct {
|
|
mp_float_t s, c;
|
|
} sincos_result_t;
|
|
|
|
#define FOUR_OVER_PI (4 / M_PI)
|
|
static void fast_sincos(mp_float_t theta, sincos_result_t *result) {
|
|
mp_float_t x = (theta * FOUR_OVER_PI) - 1;
|
|
mp_float_t x2 = x * x, x3 = x2 * x, x4 = x2 * x2, x5 = x2 * x3;
|
|
mp_float_t c0 = 0.70708592,
|
|
c1x = -0.55535724 * x,
|
|
c2x2 = -0.21798592 * x2,
|
|
c3x3 = 0.05707685 * x3,
|
|
c4x4 = 0.0109 * x4,
|
|
c5x5 = -0.00171961 * x5;
|
|
|
|
mp_float_t evens = c4x4 + c2x2 + c0, odds = c5x5 + c3x3 + c1x;
|
|
result->c = evens + odds;
|
|
result->s = evens - odds;
|
|
}
|
|
|
|
mp_obj_t common_hal_synthio_block_biquad_new(synthio_filter_mode mode, mp_obj_t f0, mp_obj_t Q) {
|
|
synthio_block_biquad_t *self = mp_obj_malloc(synthio_block_biquad_t, &synthio_block_biquad_type_obj);
|
|
self->mode = mode;
|
|
synthio_block_assign_slot(f0, &self->f0, MP_QSTR_frequency);
|
|
synthio_block_assign_slot(Q, &self->Q, MP_QSTR_Q);
|
|
return MP_OBJ_FROM_PTR(self);
|
|
}
|
|
|
|
synthio_filter_mode common_hal_synthio_block_biquad_get_mode(synthio_block_biquad_t *self) {
|
|
return self->mode;
|
|
}
|
|
|
|
mp_obj_t common_hal_synthio_block_biquad_get_Q(synthio_block_biquad_t *self) {
|
|
return self->Q.obj;
|
|
}
|
|
|
|
void common_hal_synthio_block_biquad_set_Q(synthio_block_biquad_t *self, mp_obj_t Q) {
|
|
synthio_block_assign_slot(Q, &self->Q, MP_QSTR_Q);
|
|
}
|
|
|
|
mp_obj_t common_hal_synthio_block_biquad_get_frequency(synthio_block_biquad_t *self) {
|
|
return self->f0.obj;
|
|
}
|
|
|
|
void common_hal_synthio_block_biquad_set_frequency(synthio_block_biquad_t *self, mp_obj_t frequency) {
|
|
synthio_block_assign_slot(frequency, &self->f0, MP_QSTR_frequency);
|
|
}
|
|
|
|
static int32_t biquad_scale_arg_float(mp_float_t arg) {
|
|
return (int32_t)MICROPY_FLOAT_C_FUN(round)(MICROPY_FLOAT_C_FUN(ldexp)(arg, BIQUAD_SHIFT));
|
|
}
|
|
|
|
static int float_equal_or_update(
|
|
mp_float_t *cached,
|
|
mp_float_t new) {
|
|
// uses memcmp to avoid error about equality float comparison
|
|
if (memcmp(cached, &new, sizeof(mp_float_t))) {
|
|
*cached = new;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void common_hal_synthio_block_biquad_tick(mp_obj_t self_in, biquad_filter_state *filter_state) {
|
|
synthio_block_biquad_t *self = MP_OBJ_TO_PTR(self_in);
|
|
|
|
mp_float_t W0 = synthio_block_slot_get(&self->f0) * synthio_global_W_scale;
|
|
mp_float_t Q = synthio_block_slot_get(&self->Q);
|
|
|
|
// n.b., assumes that the `mode` field is read-only
|
|
// n.b., use of `&` is deliberate, avoids short-circuiting behavior
|
|
if (float_equal_or_update(&self->cached_W0, W0) & float_equal_or_update(&self->cached_Q, Q)) {
|
|
return;
|
|
}
|
|
|
|
sincos_result_t sc;
|
|
fast_sincos(W0, &sc);
|
|
|
|
mp_float_t alpha = sc.s / (2 * Q);
|
|
|
|
mp_float_t a0, a1, a2, b0, b1, b2;
|
|
|
|
a0 = 1 + alpha;
|
|
a1 = -2 * sc.c;
|
|
a2 = 1 - alpha;
|
|
|
|
switch (self->mode) {
|
|
default:
|
|
case SYNTHIO_LOW_PASS:
|
|
b2 = b0 = (1 - sc.c) * .5;
|
|
b1 = 1 - sc.c;
|
|
break;
|
|
|
|
case SYNTHIO_HIGH_PASS:
|
|
b2 = b0 = (1 + sc.c) * .5;
|
|
b1 = -(1 + sc.c);
|
|
break;
|
|
|
|
case SYNTHIO_BAND_PASS:
|
|
b0 = alpha;
|
|
b1 = 0;
|
|
b2 = -b0;
|
|
break;
|
|
|
|
case SYNTHIO_NOTCH:
|
|
b0 = 1;
|
|
b1 = -2 * sc.c;
|
|
b2 = 1;
|
|
}
|
|
|
|
mp_float_t recip_a0 = 1 / a0;
|
|
|
|
filter_state->a1 = biquad_scale_arg_float(a1 * recip_a0);
|
|
filter_state->a2 = biquad_scale_arg_float(a2 * recip_a0);
|
|
filter_state->b0 = biquad_scale_arg_float(b0 * recip_a0);
|
|
filter_state->b1 = biquad_scale_arg_float(b1 * recip_a0);
|
|
filter_state->b2 = biquad_scale_arg_float(b2 * recip_a0);
|
|
}
|