forked from adafruit/circuitpython
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathBlockBiquad.c
More file actions
130 lines (104 loc) · 3.94 KB
/
Copy pathBlockBiquad.c
File metadata and controls
130 lines (104 loc) · 3.94 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
// 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);
}