BlockBiquad: Add shelf & peaking eq filters with "A" parameter

The formulae are untested but taken directly from the Cookbook.

The `sqrt(A)` term is given a quick approximation via the famous
Quake reciprocal square root routine.

Author: jepler

shared-bindings/synthio/BlockBiquad.c

@@ -21,18 +21,30 @@
 //|     """A band-pass filter"""
 //|     NOTCH: FilterMode
 //|     """A notch filter"""
+//|     LOW_SHELF: FilterMode
+//|     """A notch filter"""
+//|     HIGH_SHELF: FilterMode
+//|     """A notch filter"""
+//|     PEAKING_EQ: FilterMode
+//|     """A notch filter"""
 //|
 
 MAKE_ENUM_VALUE(synthio_filter_mode_type, mode, LOW_PASS, SYNTHIO_LOW_PASS);
 MAKE_ENUM_VALUE(synthio_filter_mode_type, mode, HIGH_PASS, SYNTHIO_HIGH_PASS);
 MAKE_ENUM_VALUE(synthio_filter_mode_type, mode, BAND_PASS, SYNTHIO_BAND_PASS);
 MAKE_ENUM_VALUE(synthio_filter_mode_type, mode, NOTCH, SYNTHIO_NOTCH);
+MAKE_ENUM_VALUE(synthio_filter_mode_type, mode, LOW_SHELF, SYNTHIO_LOW_SHELF);
+MAKE_ENUM_VALUE(synthio_filter_mode_type, mode, HIGH_SHELF, SYNTHIO_HIGH_SHELF);
+MAKE_ENUM_VALUE(synthio_filter_mode_type, mode, PEAKING_EQ, SYNTHIO_PEAKING_EQ);
 
 MAKE_ENUM_MAP(synthio_filter_mode) {
     MAKE_ENUM_MAP_ENTRY(mode, LOW_PASS),
     MAKE_ENUM_MAP_ENTRY(mode, HIGH_PASS),
     MAKE_ENUM_MAP_ENTRY(mode, BAND_PASS),
     MAKE_ENUM_MAP_ENTRY(mode, NOTCH),
+    MAKE_ENUM_MAP_ENTRY(mode, LOW_SHELF),
+    MAKE_ENUM_MAP_ENTRY(mode, HIGH_SHELF),
+    MAKE_ENUM_MAP_ENTRY(mode, PEAKING_EQ),
 };
 
 static MP_DEFINE_CONST_DICT(synthio_filter_mode_locals_dict, synthio_filter_mode_locals_table);
@@ -51,8 +63,17 @@ static synthio_filter_mode validate_synthio_filter_mode(mp_obj_t obj, qstr arg_n
 //|         mode: FilterMode,
 //|         frequency: BlockInput,
 //|         Q: BlockInput = 0.7071067811865475,
+//|         A: BlockInput = None,
 //|     ) -> None:
-//|         """Construct a biquad filter object with dynamic center frequency & q factor
+//|         """Construct a biquad filter object with given settings.
+//|
+//|         ``frequency`` gives the center frequency or corner frequency of the filter,
+//|         depending on the mode.
+//|
+//|         ``Q`` gives the gain or sharpness of the filter.
+//|
+//|         ``A`` controls the gain of peaking and shelving filters according to the
+//|         formula ``A = 10^(dBgain/40)``. For other filter types it is ignored.
 //|
 //|         Since ``frequency`` and ``Q`` are `BlockInput` objects, they can
 //|         be varied dynamically. Internally, this is evaluated as "direct form 1"
@@ -68,6 +89,7 @@ static const mp_arg_t block_biquad_properties[] = {
     { MP_QSTR_mode, MP_ARG_OBJ | MP_ARG_REQUIRED, {.u_obj = MP_OBJ_NULL } },
     { MP_QSTR_frequency, MP_ARG_OBJ | MP_ARG_REQUIRED, {.u_obj = MP_OBJ_NULL } },
     { MP_QSTR_Q, MP_ARG_OBJ, {.u_obj = MP_OBJ_NULL } },
+    { MP_QSTR_A, MP_ARG_OBJ, {.u_obj = MP_ROM_NONE } },
 };
 
 static mp_obj_t synthio_block_biquad_make_new(const mp_obj_type_t *type_in, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {
@@ -138,10 +160,35 @@ MP_PROPERTY_GETSET(synthio_block_biquad_Q_obj,
     (mp_obj_t)&synthio_block_biquad_get_Q_obj,
     (mp_obj_t)&synthio_block_biquad_set_Q_obj);
 
+//|
+//|     A: BlockInput
+//|     """The gain (A) of the filter
+//|
+//|     This setting only has an effect for peaking and shelving EQ filters. It is related
+//|     to the filter gain according to the formula ``A = 10^(dBgain/40)``.
+//|     """
+//|
+static mp_obj_t synthio_block_biquad_get_A(mp_obj_t self_in) {
+    synthio_block_biquad_t *self = MP_OBJ_TO_PTR(self_in);
+    return common_hal_synthio_block_biquad_get_A(self);
+}
+MP_DEFINE_CONST_FUN_OBJ_1(synthio_block_biquad_get_A_obj, synthio_block_biquad_get_A);
+
+static mp_obj_t synthio_block_biquad_set_A(mp_obj_t self_in, mp_obj_t arg) {
+    synthio_block_biquad_t *self = MP_OBJ_TO_PTR(self_in);
+    common_hal_synthio_block_biquad_set_A(self, arg);
+    return mp_const_none;
+}
+MP_DEFINE_CONST_FUN_OBJ_2(synthio_block_biquad_set_A_obj, synthio_block_biquad_set_A);
+MP_PROPERTY_GETSET(synthio_block_biquad_A_obj,
+    (mp_obj_t)&synthio_block_biquad_get_A_obj,
+    (mp_obj_t)&synthio_block_biquad_set_A_obj);
+
 static const mp_rom_map_elem_t synthio_block_biquad_locals_dict_table[] = {
     { MP_ROM_QSTR(MP_QSTR_mode), MP_ROM_PTR(&synthio_block_biquad_mode_obj) },
     { MP_ROM_QSTR(MP_QSTR_frequency), MP_ROM_PTR(&synthio_block_biquad_frequency_obj) },
     { MP_ROM_QSTR(MP_QSTR_Q), MP_ROM_PTR(&synthio_block_biquad_Q_obj) },
+    { MP_ROM_QSTR(MP_QSTR_A), MP_ROM_PTR(&synthio_block_biquad_A_obj) },
 };
 static MP_DEFINE_CONST_DICT(synthio_block_biquad_locals_dict, synthio_block_biquad_locals_dict_table);
 

shared-bindings/synthio/BlockBiquad.h

@@ -13,10 +13,15 @@ extern const mp_obj_type_t synthio_filter_mode_type;
 typedef struct synthio_block_biquad synthio_block_biquad_t;
 
 typedef enum {
-    SYNTHIO_LOW_PASS, SYNTHIO_HIGH_PASS, SYNTHIO_BAND_PASS, SYNTHIO_NOTCH
+    SYNTHIO_LOW_PASS, SYNTHIO_HIGH_PASS, SYNTHIO_BAND_PASS, SYNTHIO_NOTCH,
+    // filters beyond this line use the "A" parameter (in addition to f0 and Q)
+    SYNTHIO_PEAKING_EQ, SYNTHIO_LOW_SHELF, SYNTHIO_HIGH_SHELF
 } synthio_filter_mode;
 
 
+mp_obj_t common_hal_synthio_block_biquad_get_A(synthio_block_biquad_t *self);
+void common_hal_synthio_block_biquad_set_A(synthio_block_biquad_t *self, mp_obj_t A);
+
 mp_obj_t common_hal_synthio_block_biquad_get_Q(synthio_block_biquad_t *self);
 void common_hal_synthio_block_biquad_set_Q(synthio_block_biquad_t *self, mp_obj_t Q);
 

shared-module/synthio/BlockBiquad.c

@@ -14,6 +14,24 @@ typedef struct {
     mp_float_t s, c;
 } sincos_result_t;
 
+#include <stdint.h>  // uint32_t
+
+// The famous Quake approximate square root function
+static mp_float_t Q_rsqrt(mp_float_t number_in) {
+    float number = (float)number_in;
+    union {
+        float f;
+        uint32_t i;
+    } conv = { .f = (float)number };
+    conv.i = 0x5f3759df - (conv.i >> 1);
+    conv.f *= 1.5F - (number * 0.5F * conv.f * conv.f);
+    return (mp_float_t)conv.f;
+}
+
+static mp_float_t fast_sqrt(mp_float_t number) {
+    return number * Q_rsqrt(number);
+}
+
 #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;
@@ -48,6 +66,14 @@ void common_hal_synthio_block_biquad_set_Q(synthio_block_biquad_t *self, mp_obj_
     synthio_block_assign_slot(Q, &self->Q, MP_QSTR_Q);
 }
 
+mp_obj_t common_hal_synthio_block_biquad_get_A(synthio_block_biquad_t *self) {
+    return self->A.obj;
+}
+
+void common_hal_synthio_block_biquad_set_A(synthio_block_biquad_t *self, mp_obj_t A) {
+    synthio_block_assign_slot(A, &self->A, MP_QSTR_A);
+}
+
 mp_obj_t common_hal_synthio_block_biquad_get_frequency(synthio_block_biquad_t *self) {
     return self->f0.obj;
 }
@@ -76,10 +102,14 @@ void common_hal_synthio_block_biquad_tick(mp_obj_t self_in, biquad_filter_state
 
     mp_float_t W0 = synthio_block_slot_get(&self->f0) * synthio_global_W_scale;
     mp_float_t Q = synthio_block_slot_get(&self->Q);
+    mp_float_t A =
+        (self->mode >= SYNTHIO_PEAKING_EQ) ? synthio_block_slot_get(&self->A) : 0;
 
     // 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)) {
+    if (float_equal_or_update(&self->cached_W0, W0)
+        & float_equal_or_update(&self->cached_Q, Q)
+        & float_equal_or_update(&self->cached_A, A)) {
         return;
     }
 
@@ -90,34 +120,69 @@ void common_hal_synthio_block_biquad_tick(mp_obj_t self_in, biquad_filter_state
 
     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;
+            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;
+            }
 
-        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;
+        case SYNTHIO_PEAKING_EQ:
+            b0 = 1 + alpha * A;
+            b1 = -2 * sc.c;
+            b2 = 1 + alpha * A;
+            a0 = 1 + alpha / A;
+            a1 = -2 * sc.c;
+            a2 = 1 - alpha / A;
             break;
 
-        case SYNTHIO_NOTCH:
-            b0 = 1;
-            b1 = -2 * sc.c;
-            b2 = 1;
+        case SYNTHIO_LOW_SHELF: {
+            mp_float_t sqrt_A = fast_sqrt(A);
+            b0 = A * ((A + 1) - (A - 1) * sc.c + 2 * sqrt_A * alpha);
+            b1 = 2 * A * ((A - 1) - (A + 1) * sc.c);
+            b2 = A * ((A + 1) - (A - 1) * sc.c - 2 * sqrt_A * alpha);
+            a0 = (A + 1) + (A - 1) * sc.c + 2 * sqrt_A * alpha;
+            a1 = -2 * ((A - 1) + (A + 1) * sc.c);
+            a2 = (A + 1) + (A - 1) * sc.c - 2 * sqrt_A * alpha;
+        }
+        break;
+
+        case SYNTHIO_HIGH_SHELF: {
+            mp_float_t sqrt_A = fast_sqrt(A);
+            b0 = A * ((A + 1) + (A - 1) * sc.c + 2 * sqrt_A * alpha);
+            b1 = -2 * A * ((A - 1) + (A + 1) * sc.c);
+            b2 = A * ((A + 1) + (A - 1) * sc.c - 2 * sqrt_A * alpha);
+            a0 = (A + 1) - (A - 1) * sc.c + 2 * sqrt_A * alpha;
+            a1 = 2 * ((A - 1) - (A + 1) * sc.c);
+            a2 = (A + 1) - (A - 1) * sc.c - 2 * sqrt_A * alpha;
+        }
+        break;
     }
-
     mp_float_t recip_a0 = 1 / a0;
 
     filter_state->a1 = biquad_scale_arg_float(a1 * recip_a0);

shared-module/synthio/BlockBiquad.h

@@ -14,8 +14,8 @@
 typedef struct synthio_block_biquad {
     mp_obj_base_t base;
     synthio_filter_mode mode;
-    synthio_block_slot_t f0, Q;
-    mp_float_t cached_W0, cached_Q;
+    synthio_block_slot_t f0, Q, A;
+    mp_float_t cached_W0, cached_Q, cached_A;
 } synthio_block_biquad_t;
 
 void common_hal_synthio_block_biquad_tick(mp_obj_t self_in, biquad_filter_state *filter_state);