From e2781d07f9b656b56f8bb724a392800c19c2cfe0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 8 Jan 2024 22:29:16 +0100 Subject: [PATCH] iir: support fixedpoint --- src/iir.rs | 835 +++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 683 insertions(+), 152 deletions(-) diff --git a/src/iir.rs b/src/iir.rs index d02c9a9..847b9f7 100644 --- a/src/iir.rs +++ b/src/iir.rs @@ -1,7 +1,122 @@ +use core::{ + iter::Sum, + ops::{Add, Neg, Sub}, +}; +use num_traits::{AsPrimitive, Float, FloatConst}; use serde::{Deserialize, Serialize}; -use num_traits::{clamp, Float}; +pub trait FilterNum: + Copy + + Sum + + PartialEq + + Neg + + Sub + + Add +where + Self: 'static, +{ + const ONE: Self; + const NEG_ONE: Self; + const ZERO: Self; + const MIN: Self; + const MAX: Self; + fn macc(self, xa: impl Iterator) -> Self; + fn mul(self, other: Self) -> Self; + fn div(self, other: Self) -> Self; + fn clamp(self, min: Self, max: Self) -> Self; + fn quantize(value: C) -> Self + where + Self: AsPrimitive, + C: Float + AsPrimitive; +} +macro_rules! impl_float { + ($T:ty) => { + impl FilterNum for $T { + const ONE: Self = 1.0; + const NEG_ONE: Self = -Self::ONE; + const ZERO: Self = 0.0; + const MIN: Self = Self::NEG_INFINITY; + const MAX: Self = Self::INFINITY; + fn macc(self, xa: impl Iterator) -> Self { + xa.fold(self, |y, (a, x)| a.mul_add(x, y)) + // xa.fold(self, |y, (a, x)| y + a * x) + } + fn clamp(self, min: Self, max: Self) -> Self { + <$T>::clamp(self, min, max) + } + fn div(self, other: Self) -> Self { + self / other + } + fn mul(self, other: Self) -> Self { + self * other + } + fn quantize>(value: C) -> Self { + value.as_() + } + } + }; +} +impl_float!(f32); +impl_float!(f64); + +macro_rules! impl_int { + ($T:ty, $A:ty, $Q:literal) => { + impl FilterNum for $T { + const ONE: Self = 1 << $Q; + const NEG_ONE: Self = -Self::ONE; + const ZERO: Self = 0; + // Need to avoid `$T::MIN*$T::MIN` overflow. + const MIN: Self = -Self::MAX; + const MAX: Self = Self::MAX; + fn macc(self, xa: impl Iterator) -> Self { + self + (xa.fold(1 << ($Q - 1), |y, (a, x)| y + a as $A * x as $A) >> $Q) as Self + } + fn clamp(self, min: Self, max: Self) -> Self { + Ord::clamp(self, min, max) + } + fn div(self, other: Self) -> Self { + (((self as $A) << $Q) / other as $A) as Self + } + fn mul(self, other: Self) -> Self { + (((1 << ($Q - 1)) + self as $A * other as $A) >> $Q) as Self + } + fn quantize(value: C) -> Self + where + Self: AsPrimitive, + C: Float + AsPrimitive, + { + (value * Self::ONE.as_()).round().as_() + } + } + }; +} +// Q2.X chosen to be able to exactly and inclusively represent -2 as `-1 << X + 1` +impl_int!(i8, i16, 6); +impl_int!(i16, i32, 14); +impl_int!(i32, i64, 30); +impl_int!(i64, i128, 62); + +/// Filter architecture +/// +/// Direct Form 1 (DF1) and Direct Form 2 transposed (DF2T) are the only IIR filter +/// structures with an (effective bin the case of TDF2) single summing junction +/// this allows clamping of the output before feedback. +/// +/// DF1 allows atomic coefficient change because only x/y are pipelined. +/// The summing junctuion pipelining of TDF2 would require incremental +/// coefficient changes and is thus less amenable to online tuning. +/// +/// DF2T needs less state storage (2 instead of 4). This is in addition to the coefficient storage +/// (5 plus 2 limits plus 1 offset) +/// This implementation already saves storage by decoupling coefficients/limits and offset from state +/// and thus supports both (a) sharing a single filter between multiple states ("channels") and (b) +/// rapid switching of filters (tuning, transfer) for a given state without copying. +/// +/// DF2T is less efficient and accurate for fixed-point architectures as quantization +/// happens at each intermediate summing junction in addition to the output quantization. This is +/// especially true for common `i64 + i32 * i32 -> i64` MACC architectures. +/// /// # Coefficients and state /// /// `[T; 5]` is both the IIR state and coefficients type. @@ -47,7 +162,15 @@ use num_traits::{clamp, Float}; /// implementation of transfer functions beyond bequadratic terms. /// /// See also . -#[derive(Copy, Clone, Debug, Deserialize, Serialize, PartialEq, Eq, PartialOrd)] +/// +/// +/// Offset and limiting disabled to suit lowpass applications. +/// Coefficient scaling fixed and optimized such that -2 is representable. +/// Tailored to low-passes, PID, II etc, where the integration rule is [1, -2, 1]. +/// Since the relevant coefficients `a1` and `a2` are negated, we also negate the +/// stored `y1` and `y2` in the state. +/// Note that `xy` contains the negative `y1` and `y2`, such that `-a1` +#[derive(Clone, Debug, Deserialize, Serialize, PartialEq, PartialOrd)] pub struct Biquad { ba: [T; 5], u: T, @@ -55,18 +178,18 @@ pub struct Biquad { max: T, } -impl Default for Biquad { +impl Default for Biquad { fn default() -> Self { Self { - ba: [T::zero(); 5], - u: T::zero(), - min: T::neg_infinity(), - max: T::infinity(), + ba: [T::ZERO; 5], + u: T::ZERO, + min: T::MIN, + max: T::MAX, } } } -impl From<[T; 5]> for Biquad { +impl From<[T; 5]> for Biquad { fn from(ba: [T; 5]) -> Self { Self { ba, @@ -75,66 +198,81 @@ impl From<[T; 5]> for Biquad { } } -/// A unit gain filter -/// -/// ``` -/// # use idsp::iir::*; -/// let x0 = 3.0; -/// let y0 = Biquad::from(identity()).update(&mut [0.0; 5], x0); -/// assert_eq!(y0, x0); -/// ``` -pub fn identity() -> [T; 5] { - proportional(T::one()) +impl From<[C; 6]> for Biquad +where + T: FilterNum + AsPrimitive, + C: Float + AsPrimitive, +{ + fn from(ba: [C; 6]) -> Self { + let ia0 = C::one() / ba[3]; + Self::from([ + T::quantize(ba[0] * ia0), + T::quantize(ba[1] * ia0), + T::quantize(ba[2] * ia0), + // b[3]: a0*ia0 + T::quantize(ba[4] * ia0), + T::quantize(ba[5] * ia0), + ]) + } } -/// A filter with the given proportional gain at all frequencies -/// -/// ``` -/// # use idsp::iir::*; -/// let x0 = 2.0; -/// let k = 5.0; -/// let y0 = Biquad::from(proportional(k)).update(&mut [0.0; 5], x0); -/// assert_eq!(y0, x0 * k); -/// ``` -pub fn proportional(k: T) -> [T; 5] { - let mut ba = [T::zero(); 5]; - ba[0] = k; - ba -} +impl Biquad { + /// A "hold" filter that ingests input and maintains output + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 7.0; + /// let y0 = Biquad::HOLD.update(&mut xy, x0); + /// assert_eq!(y0, -2.0); + /// assert_eq!(xy, [x0, 0.0, -y0, -y0, 3.0]); + /// ``` + pub const HOLD: Self = Self { + ba: [T::ZERO, T::ZERO, T::ZERO, T::NEG_ONE, T::ZERO], + u: T::ZERO, + min: T::MIN, + max: T::MAX, + }; -/// A "hold" filter that ingests input and maintains output -/// -/// ``` -/// # use idsp::iir::*; -/// let mut xy = core::array::from_fn(|i| i as _); -/// let x0 = 7.0; -/// let y0 = Biquad::from(hold()).update(&mut xy, x0); -/// assert_eq!(y0, 2.0); -/// assert_eq!(xy, [x0, 0.0, y0, y0, 3.0]); -/// ``` -pub fn hold() -> [T; 5] { - let mut ba = [T::zero(); 5]; - ba[3] = T::one(); - ba -} + /// A unity gain filter + /// + /// ``` + /// # use idsp::iir::*; + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0); + /// ``` + pub const IDENTITY: Self = Self::proportional(T::ONE); + + /// A filter with the given proportional gain at all frequencies + /// + /// ``` + /// # use idsp::iir::*; + /// let x0 = 2.0; + /// let k = 5.0; + /// let y0 = Biquad::proportional(k).update(&mut [0.0; 5], x0); + /// assert_eq!(y0, x0 * k); + /// ``` + pub const fn proportional(k: T) -> Self { + Self { + ba: [k, T::ZERO, T::ZERO, T::ZERO, T::ZERO], + u: T::ZERO, + min: T::MIN, + max: T::MAX, + } + } -// TODO -// lowpass1 -// highpass1 -// butterworth -// elliptic -// chebychev1/2 -// bessel -// invert // high-to-low/low-to-high -// notch -// PI-notch -// SOS cascades thereoff - -impl Biquad { /// Filter coefficients /// /// IIR filter tap gains (`ba`) are an array `[b0, b1, b2, a1, a2]` such that - /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 + a1*y1 + a2*y2 + u, min, max)`. + /// [`Biquad::update(&mut xy, x0)`] returns + /// `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)`. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::IDENTITY.ba()[0], i32::ONE); + /// assert_eq!(Biquad::::HOLD.ba()[3], -i32::ONE); + /// ``` pub fn ba(&self) -> &[T; 5] { &self.ba } @@ -142,6 +280,13 @@ impl Biquad { /// Mutable reference to the filter coefficients. /// /// See [`Biquad::ba()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.ba_mut()[0] = i32::ONE; + /// assert_eq!(i, Biquad::IDENTITY); + /// ``` pub fn ba_mut(&mut self) -> &mut [T; 5] { &mut self.ba } @@ -159,6 +304,13 @@ impl Biquad { /// Set the summing junction offset /// /// See [`Biquad::u()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_u(5); + /// assert_eq!(i.update(&mut [0; 5], 0), 5); + /// ``` pub fn set_u(&mut self, u: T) { self.u = u; } @@ -168,6 +320,16 @@ impl Biquad { /// Guaranteed minimum output value. /// The value is inclusive. /// The clamping also cleanly affects the feedback terms. + /// + /// Note: For fixed point filters `Biquad`, `T::MIN` should not be passed + /// to `min()` since the `y` samples stored in + /// the filter state are negated. Instead use `-T::MAX` as the lowest + /// possible limit. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::default().min(), -i32::MAX); + /// ``` pub fn min(&self) -> T { self.min } @@ -175,6 +337,13 @@ impl Biquad { /// Set the lower output limit /// /// See [`Biquad::min()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_min(7); + /// assert_eq!(i.update(&mut [0; 5], 0), 7); + /// ``` pub fn set_min(&mut self, min: T) { self.min = min; } @@ -184,6 +353,11 @@ impl Biquad { /// Guaranteed maximum output value. /// The value is inclusive. /// The clamping also cleanly affects the feedback terms. + /// + /// ``` + /// # use idsp::iir::*; + /// assert_eq!(Biquad::::default().max(), i32::MAX); + /// ``` pub fn max(&self) -> T { self.max } @@ -191,6 +365,13 @@ impl Biquad { /// Set the upper output limit /// /// See [`Biquad::max()`]. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::default(); + /// i.set_max(-7); + /// assert_eq!(i.update(&mut [0; 5], 0), -7); + /// ``` pub fn set_max(&mut self, max: T) { self.max = max; } @@ -199,49 +380,58 @@ impl Biquad { /// /// ``` /// # use idsp::iir::*; - /// assert_eq!(Biquad::from(proportional(3.0)).forward_gain(), 3.0); + /// assert_eq!(Biquad::proportional(3.0).forward_gain(), 3.0); /// ``` /// /// # Returns /// The sum of the `b` feed-forward coefficients. pub fn forward_gain(&self) -> T { - self.ba[0] + self.ba[1] + self.ba[2] + self.ba.iter().take(3).copied().sum() } /// Compute input-referred (`x`) offset. /// ``` /// # use idsp::iir::*; - /// let mut i = Biquad::from(proportional(3.0)); - /// i.set_input_offset(2.0); - /// assert_eq!(i.input_offset(), 2.0); + /// let mut i = Biquad::proportional(3); + /// i.set_u(3); + /// assert_eq!(i.input_offset(), i32::ONE); /// ``` pub fn input_offset(&self) -> T { - self.u / self.forward_gain() + self.u.div(self.forward_gain()) } /// Convert input (`x`) offset to equivalent summing junction offset (`u`) and apply. /// /// In the case of a "PID" controller the response behavior of the controller /// to the offset is "stabilizing", and not "tracking": its frequency response - /// is exclusively according to the lowest non-zero [`PidAction`] gain. + /// is exclusively according to the lowest non-zero [`Action`] gain. /// There is no high order ("faster") response as would be the case for a "tracking" /// controller. /// /// ``` /// # use idsp::iir::*; - /// let mut i = Biquad::from(proportional(3.0)); + /// let mut i = Biquad::proportional(3.0); /// i.set_input_offset(2.0); /// let x0 = 0.5; /// let y0 = i.update(&mut [0.0; 5], x0); /// assert_eq!(y0, (x0 + i.input_offset()) * i.forward_gain()); /// ``` /// + /// ``` + /// # use idsp::iir::*; + /// let mut i = Biquad::proportional(-i32::ONE); + /// i.set_input_offset(1); + /// assert_eq!(i.u(), -1); + /// ``` + /// /// # Arguments /// * `offset`: Input (`x`) offset. pub fn set_input_offset(&mut self, offset: T) { - self.u = offset * self.forward_gain(); + self.u = offset.mul(self.forward_gain()); } + /// Direct Form 1 Update + /// /// Ingest a new input value into the filter, update the filter state, and /// return the new output. Only the state `xy` is modified. /// @@ -249,36 +439,358 @@ impl Biquad { /// # use idsp::iir::*; /// let mut xy = core::array::from_fn(|i| i as _); /// let x0 = 3.0; - /// let y0 = Biquad::from(identity()).update(&mut xy, x0); + /// let y0 = Biquad::IDENTITY.update(&mut xy, x0); /// assert_eq!(y0, x0); - /// assert_eq!(xy, [x0, 0.0, y0, 2.0, 3.0]); + /// assert_eq!(xy, [x0, 0.0, -y0, 2.0, 3.0]); /// ``` /// /// # Arguments /// * `xy` - Current filter state. + /// On entry: `[x1, x2, -y1, -y2, -y3]` + /// On exit: `[x0, x1, -y0, -y1, -y2]` /// * `x0` - New input. /// /// # Returns - /// The new output `y0`. + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` /// /// # Panics /// Panics in debug mode if `!(self.min <= self.max)`. pub fn update(&self, xy: &mut [T; 5], x0: T) -> T { - // `xy` contains x0 x1 y0 y1 y2 - // Increment time x1 x2 y1 y2 y3 - // Shift x1 x1 x2 y1 y2 + // `xy` contains x0 x1 -y0 -y1 -y2 + // Increment time x1 x2 -y1 -y2 -y3 + // Shift x1 x1 x2 -y1 -y2 xy.copy_within(0..4, 1); - // Store x0 x0 x1 x2 y1 y2 + // Store x0 x0 x1 x2 -y1 -y2 xy[0] = x0; - let y0 = xy - .iter() - .zip(self.ba.iter()) - .fold(self.u, |y, (x, a)| y + *x * *a); - let y0 = clamp(y0, self.min, self.max); - // Store y0 x0 x1 y0 y1 y2 - xy[2] = y0; + // Compute y0 + let y0 = self + .u + .macc(xy.iter().copied().zip(self.ba.iter().copied())) + .clamp(self.min, self.max); + // Store -y0 x0 x1 -y0 -y1 -y2 + xy[2] = -y0; + y0 + } + + /// Direct Form 1 Update + /// + /// Ingest a new input value into the filter, update the filter state, and + /// return the new output. Only the state `xy` is modified. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update_df1(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [x0, 0.0, -y0, 2.0]); + /// ``` + /// + /// # Arguments + /// * `xy` - Current filter state. + /// On entry: `[x1, x2, -y1, -y2]` + /// On exit: `[x0, x1, -y0, -y1]` + /// * `x0` - New input. + /// + /// # Returns + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` + /// + /// # Panics + /// Panics in debug mode if `!(self.min <= self.max)`. + pub fn update_df1(&self, xy: &mut [T; 4], x0: T) -> T { + // `xy` contains x0 x1 -y0 -y1 + // Increment time x1 x2 -y1 -y2 + // Compute y0 + let y0 = self + .u + .macc( + core::iter::once(x0) + .chain(xy.iter().copied()) + .zip(self.ba.iter().copied()), + ) + .clamp(self.min, self.max); + // Shift x1 x1 -y1 -y2 + xy[1] = xy[0]; + // Store x0 x0 x1 -y1 -y2 + xy[0] = x0; + // Shift x0 x1 -y0 -y1 + xy[3] = xy[2]; + // Store -y0 x0 x1 -y0 -y1 + xy[2] = -y0; y0 } + + /// Ingest new input and perform a Direct Form 2 Transposed update. + /// + /// ``` + /// # use idsp::iir::*; + /// let mut xy = core::array::from_fn(|i| i as _); + /// let x0 = 3.0; + /// let y0 = Biquad::IDENTITY.update_df2t(&mut xy, x0); + /// assert_eq!(y0, x0); + /// assert_eq!(xy, [1.0, 0.0]); + /// ``` + /// + /// # Arguments + /// * `s` - Current filter state. + /// On entry: `[b1*x1 + b2*x2 - a1*y1 - a2*y2, b2*x1 - a2*y1]` + /// On exit: `[b1*x0 + b2*x1 - a1*y0 - a2*y1, b2*x0 - a2*y0]` + /// * `x0` - New input. + /// + /// # Returns + /// The new output `y0 = clamp(b0*x0 + b1*x1 + b2*x2 - a1*y1 - a2*y2 + u, min, max)` + /// + /// # Panics + /// Panics in debug mode if `!(self.min <= self.max)`. + pub fn update_df2t(&self, u: &mut [T; 2], x0: T) -> T { + let y0 = (u[0] + self.ba[0].mul(x0)).clamp(self.min, self.max); + u[0] = u[1] + self.ba[1].mul(x0) - self.ba[3].mul(y0); + u[1] = self.u + self.ba[2].mul(x0) - self.ba[4].mul(y0); + y0 + } +} + +#[derive(Copy, Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] +enum Shape { + InverseQ(T), + Bandwidth(T), + Slope(T), +} + +impl Default for Shape { + fn default() -> Self { + Self::InverseQ(T::SQRT_2()) + } +} + +/// Standard audio biquad filter builder +/// +/// +#[derive(Clone, Debug, PartialEq, PartialOrd, Serialize, Deserialize)] +pub struct Filter { + /// Angular critical frequency (in units of sampling frequency) + /// Corner frequency, or 3dB cutoff frequency, + w0: T, + /// Passband gain + gain: T, + /// Shelf gain (only for peaking, lowshelf, highshelf) + /// Relative to passband gain + shelf: T, + /// Inverse Q + shape: Shape, +} + +impl Default for Filter { + fn default() -> Self { + Self { + w0: T::zero(), + gain: T::one(), + shape: Shape::default(), + shelf: T::one(), + } + } +} + +impl Filter +where + T: 'static + Float + FloatConst, + f32: AsPrimitive, +{ + pub fn frequency(self, critical_frequency: T, sample_frequency: T) -> Self { + self.critical_frequency(critical_frequency / sample_frequency) + } + + pub fn critical_frequency(self, critical_frequency: T) -> Self { + self.angular_critical_frequency(T::TAU() * critical_frequency) + } + + pub fn angular_critical_frequency(mut self, w0: T) -> Self { + self.w0 = w0; + self + } + + pub fn gain(mut self, k: T) -> Self { + self.gain = k; + self + } + + pub fn gain_db(self, k_db: T) -> Self { + self.gain(10.0.as_().powf(k_db / 20.0.as_())) + } + + pub fn shelf(mut self, a: T) -> Self { + self.shelf = a; + self + } + + pub fn shelf_db(self, k_db: T) -> Self { + self.gain(10.0.as_().powf(k_db / 20.0.as_())) + } + + pub fn inverse_q(mut self, qi: T) -> Self { + self.shape = Shape::InverseQ(qi); + self + } + + pub fn q(self, q: T) -> Self { + self.inverse_q(T::one() / q) + } + + /// Set [`FilterBuilder::frequency()`] first. + /// In octaves. + pub fn bandwidth(mut self, bw: T) -> Self { + self.shape = Shape::Bandwidth(bw); + self + } + + /// Set [`FilterBuilder::gain()`] first. + pub fn shelf_slope(mut self, s: T) -> Self { + self.shape = Shape::Slope(s); + self + } + + fn qi(&self) -> T { + match self.shape { + Shape::InverseQ(qi) => qi, + Shape::Bandwidth(bw) => { + 2.0.as_() * (T::LN_2() / 2.0.as_() * bw * self.w0 / self.w0.sin()).sinh() + } + Shape::Slope(s) => { + ((self.gain + T::one() / self.gain) * (T::one() / s - T::one()) + 2.0.as_()).sqrt() + } + } + } + + fn alpha(&self) -> T { + 0.5.as_() * self.w0.sin() * self.qi() + } + + /// Lowpass biquad filter. + /// + /// ``` + /// use idsp::iir::*; + /// let ba = Filter::default().critical_frequency(0.1).lowpass(); + /// println!("{ba:?}"); + /// ``` + pub fn lowpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * 0.5.as_() * (T::one() - fcos); + [ + b, + b + b, + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn highpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * 0.5.as_() * (T::one() + fcos); + [ + b, + -(b + b), + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn bandpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + let b = self.gain * alpha; + [ + b, + T::zero(), + b, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn notch(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + self.gain, + -(fcos + fcos) * self.gain, + self.gain, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn allpass(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + (T::one() - alpha) * self.gain, + -(fcos + fcos) * self.gain, + (T::one() + alpha) * self.gain, + T::one() + alpha, + -(fcos + fcos), + T::one() - alpha, + ] + } + + pub fn peaking(self) -> [T; 6] { + let fcos = self.w0.cos(); + let alpha = self.alpha(); + [ + (T::one() + alpha * self.shelf) * self.gain, + -(fcos + fcos) * self.gain, + (T::one() - alpha * self.shelf) * self.gain, + T::one() + alpha / self.shelf, + -(fcos + fcos), + T::one() - alpha / self.shelf, + ] + } + + pub fn lowshelf(self) -> [T; 6] { + let fcos = self.w0.cos(); + let sp1 = self.shelf + T::one(); + let sm1 = self.shelf - T::one(); + let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); + [ + self.shelf * self.gain * (sp1 - sm1 * fcos + tsa), + 2.0.as_() * self.shelf * self.gain * (sm1 - sp1 * fcos), + self.shelf * self.gain * (sp1 - sm1 * fcos - tsa), + sp1 + sm1 * fcos + tsa, + (-2.0).as_() * (sm1 + sp1 * fcos), + sp1 + sm1 * fcos - tsa, + ] + } + + pub fn highshelf(self) -> [T; 6] { + let fcos = self.w0.cos(); + let sp1 = self.shelf + T::one(); + let sm1 = self.shelf - T::one(); + let tsa = 2.0.as_() * self.shelf.sqrt() * self.alpha(); + [ + self.shelf * self.gain * (sp1 + sm1 * fcos + tsa), + (-2.0).as_() * self.shelf * self.gain * (sm1 + sp1 * fcos), + self.shelf * self.gain * (sp1 + sm1 * fcos - tsa), + sp1 - sm1 * fcos + tsa, + 2.0.as_() * (sm1 - sp1 * fcos), + sp1 - sm1 * fcos - tsa, + ] + } + + // TODO + // PI-notch + // + // SOS cascades: + // butterworth + // elliptic + // chebychev1/2 + // bessel } /// PID controller builder @@ -287,25 +799,25 @@ impl Biquad { /// /// ``` /// # use idsp::iir::*; -/// let b: Biquad = PidBuilder::default() +/// let b: Biquad = Pid::default() /// .period(1e-3) -/// .gain(PidAction::Ki, 1e-3) -/// .gain(PidAction::Kp, 1.0) -/// .gain(PidAction::Kd, 1e2) -/// .limit(PidAction::Ki, 1e3) -/// .limit(PidAction::Kd, 1e1) +/// .gain(Action::Ki, 1e-3) +/// .gain(Action::Kp, 1.0) +/// .gain(Action::Kd, 1e2) +/// .limit(Action::Ki, 1e3) +/// .limit(Action::Kd, 1e1) /// .build() /// .unwrap() /// .into(); /// ``` -#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)] -pub struct PidBuilder { +#[derive(Debug, Clone, PartialEq, PartialOrd, Serialize, Deserialize)] +pub struct Pid { period: T, gains: [T; 5], limits: [T; 5], } -impl Default for PidBuilder { +impl Default for Pid { fn default() -> Self { Self { period: T::one(), @@ -315,7 +827,7 @@ impl Default for PidBuilder { } } -/// [`PidBuilder::build()`] errors +/// [`Pid::build()`] errors #[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] #[non_exhaustive] pub enum PidError { @@ -327,7 +839,7 @@ pub enum PidError { /// /// This enumerates the five possible PID style actions of a [`Biquad`] #[derive(Copy, Clone, Debug, PartialEq, Eq, Ord, PartialOrd, Serialize, Deserialize)] -pub enum PidAction { +pub enum Action { /// Double integrating, -40 dB per decade Kii = 0, /// Integrating, -20 dB per decade @@ -340,7 +852,7 @@ pub enum PidAction { Kdd = 4, } -impl PidBuilder { +impl> Pid { /// Sample period /// /// # Arguments @@ -361,15 +873,15 @@ impl PidBuilder { /// /// Note that inverse time units correspond to angular frequency units. /// Gains are accurate in the low frequency limit. Towards Nyquist, the - /// frequency response is wrapped. + /// frequency response is warped. /// /// ``` /// # use idsp::iir::*; /// let tau = 1e-3; /// let ki = 1e-4; - /// let i: Biquad = PidBuilder::default() + /// let i: Biquad = Pid::default() /// .period(tau) - /// .gain(PidAction::Ki, ki) + /// .gain(Action::Ki, ki) /// .build() /// .unwrap() /// .into(); @@ -381,22 +893,22 @@ impl PidBuilder { /// # Arguments /// * `action`: Action to control /// * `gain`: Gain value - pub fn gain(mut self, action: PidAction, gain: T) -> Self { + pub fn gain(mut self, action: Action, gain: T) -> Self { self.gains[action as usize] = gain; self } /// Gain limit for a given action /// - /// Gain limit units are `output/input`. See also [`PidBuilder::gain()`]. + /// Gain limit units are `output/input`. See also [`Pid::gain()`]. /// Multiple gains and limits may interact and lead to peaking. /// /// ``` /// # use idsp::iir::*; /// let ki_limit = 1e3; - /// let i: Biquad = PidBuilder::default() - /// .gain(PidAction::Ki, 8.0) - /// .limit(PidAction::Ki, ki_limit) + /// let i: Biquad = Pid::default() + /// .gain(Action::Ki, 8.0) + /// .limit(Action::Ki, ki_limit) /// .build() /// .unwrap() /// .into(); @@ -412,7 +924,7 @@ impl PidBuilder { /// # Arguments /// * `action`: Action to limit in gain /// * `limit`: Gain limit - pub fn limit(mut self, action: PidAction, limit: T) -> Self { + pub fn limit(mut self, action: Action, limit: T) -> Self { self.limits[action as usize] = limit; self } @@ -428,15 +940,14 @@ impl PidBuilder { /// /// ``` /// # use idsp::iir::*; - /// let i: Biquad = PidBuilder::default() - /// .gain(PidAction::Kp, 3.0) - /// .build() - /// .unwrap() - /// .into(); - /// assert_eq!(i, Biquad::from(proportional(3.0))); + /// let i: Biquad = Pid::default().gain(Action::Kp, 3.0).build().unwrap().into(); + /// assert_eq!(i, Biquad::proportional(3.0)); /// ``` - pub fn build(self) -> Result<[T; 5], PidError> { - const KP: usize = PidAction::Kp as usize; + pub fn build>(self) -> Result<[C; 5], PidError> + where + T: AsPrimitive, + { + const KP: usize = Action::Kp as usize; // Determine highest denominator (feedback, `a`) order let low = self @@ -452,43 +963,37 @@ impl PidBuilder { // Derivative/integration kernels let kernels = [ - [T::one(), T::zero(), T::zero()], - [T::one(), -T::one(), T::zero()], - [T::one(), -T::one() - T::one(), T::one()], + [C::ONE, C::ZERO, C::ZERO], + [C::ONE, C::NEG_ONE, C::ZERO], + [C::ONE, C::NEG_ONE + C::NEG_ONE, C::ONE], ]; - // Coefficients - let mut b = [T::zero(); 3]; - let mut a = [T::zero(); 3]; - + // Scale gains, compute limits, quantize let mut zi = self.period.powi(low as i32 - KP as i32); - - for ((i, (gi, li)), ki) in self - .gains - .iter() - .zip(self.limits.iter()) - .enumerate() - .skip(low) - .zip(kernels.iter()) - { - // Scale gains and compute limits in place - let gi = *gi * zi; + let mut gl = [[T::zero(); 2]; 3]; + for (gli, (i, (ggi, lli))) in gl.iter_mut().zip( + self.gains + .iter() + .zip(self.limits.iter()) + .enumerate() + .skip(low), + ) { + gli[0] = *ggi * zi; + gli[1] = if i == KP { T::one() } else { gli[0] / *lli }; zi = zi * self.period; - let li = if i == KP { T::one() } else { gi / *li }; - - for (j, (bj, aj)) in b.iter_mut().zip(a.iter_mut()).enumerate() { - *bj = *bj + gi * ki[j]; - *aj = *aj + li * ki[j]; - } } + let a0i = T::one() / gl.iter().map(|gli| gli[1]).sum(); - // Normalize - let a0 = T::one() / a[0]; - for baj in b.iter_mut().chain(a.iter_mut().skip(1)) { - *baj = *baj * a0; + // Coefficients + let mut ba = [[C::ZERO; 2]; 3]; + for (gli, ki) in gl.iter().zip(kernels.iter()) { + let (g, l) = (C::quantize(gli[0] * a0i), C::quantize(gli[1] * a0i)); + for (j, baj) in ba.iter_mut().enumerate() { + *baj = [baj[0] + ki[j].mul(g), baj[1] + ki[j].mul(l)]; + } } - Ok([b[0], b[1], b[2], -a[1], -a[2]]) + Ok([ba[0][0], ba[1][0], ba[2][0], ba[1][1], ba[2][1]]) } } @@ -498,13 +1003,13 @@ mod test { #[test] fn pid() { - let b: Biquad = PidBuilder::default() + let b: Biquad = Pid::default() .period(1.0) - .gain(PidAction::Ki, 1e-3) - .gain(PidAction::Kp, 1.0) - .gain(PidAction::Kd, 1e2) - .limit(PidAction::Ki, 1e3) - .limit(PidAction::Kd, 1e1) + .gain(Action::Ki, 1e-3) + .gain(Action::Kp, 1.0) + .gain(Action::Kd, 1e2) + .limit(Action::Ki, 1e3) + .limit(Action::Kd, 1e1) .build() .unwrap() .into(); @@ -512,8 +1017,8 @@ mod test { 9.18190826, -18.27272561, 9.09090826, - 1.90909074, - -0.90909083, + -1.90909074, + 0.90909083, ]; for (ba_have, ba_want) in b.ba.iter().zip(want.iter()) { assert!( @@ -524,13 +1029,28 @@ mod test { } } + #[test] + fn pid_i32() { + let b: Biquad = Pid::default() + .period(1.0) + .gain(Action::Ki, 1e-5) + .gain(Action::Kp, 1e-2) + .gain(Action::Kd, 1e0) + .limit(Action::Ki, 1e1) + .limit(Action::Kd, 1e-1) + .build() + .unwrap() + .into(); + println!("{b:?}"); + } + #[test] fn units() { let ki = 5e-2; let tau = 3e-3; - let b: Biquad = PidBuilder::default() + let b: Biquad = Pid::default() .period(tau) - .gain(PidAction::Ki, ki) + .gain(Action::Ki, ki) .build() .unwrap() .into(); @@ -544,4 +1064,15 @@ mod test { ); } } + + #[test] + fn lowpass_gen() { + let ba = Biquad::::from( + Filter::default() + .critical_frequency(2e-9f64) + .gain(2e7) + .lowpass(), + ); + println!("{:?}", ba); + } }