forked from adafruit/circuitpython
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathBiquad.c
More file actions
131 lines (114 loc) · 4.27 KB
/
Copy pathBiquad.c
File metadata and controls
131 lines (114 loc) · 4.27 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
131
// 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-bindings/synthio/BlockBiquad.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);
}
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) {
return;
}
if (mp_obj_is_type(biquad_obj, &synthio_block_biquad_type_obj)) {
return;
}
if (mp_obj_is_type(biquad_obj, (const mp_obj_type_t *)&synthio_biquad_type_obj)) {
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]);
return;
}
mp_raise_TypeError_varg(MP_ERROR_TEXT("%q must be of type %q or %q, not %q"), MP_QSTR_filter, MP_QSTR_Biquad, MP_QSTR_BlockBiquad, mp_obj_get_type(biquad_obj)->name);
}
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;
}