pounder_test/dsp/src/iir_int.rs

104 lines
3.2 KiB
Rust

use core::f32::consts::PI;
use serde::{Deserialize, Serialize};
/// Generic vector for integer IIR filter.
/// This struct is used to hold the x/y input/output data vector or the b/a coefficient
/// vector.
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct Vec5(pub [i32; 5]);
impl Vec5 {
/// Lowpass biquad filter using cutoff and sampling frequencies. Taken from:
/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
///
/// # Args
/// * `f` - Corner frequency, or 3dB cutoff frequency (in units of sample rate).
/// This is only accurate for low corner frequencies less than ~0.01.
/// * `q` - Quality factor (1/sqrt(2) for critical).
/// * `k` - DC gain.
///
/// # Returns
/// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1.
pub fn lowpass(f: f32, q: f32, k: f32) -> Self {
// 3rd order Taylor approximation of sin and cos.
let f = f * 2. * PI;
let f2 = f * f * 0.5;
let fcos = 1. - f2;
let fsin = f * (1. - f2 / 3.);
let alpha = fsin / (2. * q);
// IIR uses Q2.30 fixed point
let a0 = (1. + alpha) / (1 << IIR::SHIFT) as f32;
let b0 = (k / 2. * (1. - fcos) / a0) as _;
let a1 = (2. * fcos / a0) as _;
let a2 = ((alpha - 1.) / a0) as _;
Self([b0, 2 * b0, b0, a1, a2])
}
}
fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
// Rounding bias, half up
let y0 = ((y0 as i64) << shift) + (1 << (shift - 1));
let y = x
.iter()
.zip(a)
.map(|(x, a)| *x as i64 * *a as i64)
.fold(y0, |y, xa| y + xa);
(y >> shift) as i32
}
/// Integer biquad IIR
///
/// See `dsp::iir::IIR` for general implementation details.
/// Offset and limiting disabled to suit lowpass applications.
/// Coefficient scaling fixed and optimized.
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct IIR {
pub ba: Vec5,
// pub y_offset: i32,
// pub y_min: i32,
// pub y_max: i32,
}
impl IIR {
/// Coefficient fixed point format: signed Q2.30.
/// Tailored to low-passes, PI, II etc.
pub const SHIFT: u32 = 30;
/// Feed a new input value into the filter, update the filter state, and
/// return the new output. Only the state `xy` is modified.
///
/// # Arguments
/// * `xy` - Current filter state.
/// * `x0` - New input.
pub fn update(&self, xy: &mut Vec5, x0: i32) -> i32 {
let n = self.ba.0.len();
debug_assert!(xy.0.len() == n);
// `xy` contains x0 x1 y0 y1 y2
// Increment time x1 x2 y1 y2 y3
// Shift x1 x1 x2 y1 y2
// This unrolls better than xy.rotate_right(1)
xy.0.copy_within(0..n - 1, 1);
// Store x0 x0 x1 x2 y1 y2
xy.0[0] = x0;
// Compute y0 by multiply-accumulate
let y0 = macc(0, &xy.0, &self.ba.0, IIR::SHIFT);
// Limit y0
// let y0 = y0.max(self.y_min).min(self.y_max);
// Store y0 x0 x1 y0 y1 y2
xy.0[n / 2] = y0;
y0
}
}
#[cfg(test)]
mod test {
use super::Vec5;
#[test]
fn lowpass_gen() {
let ba = Vec5::lowpass(1e-3, 1. / 2f32.sqrt(), 2.);
println!("{:?}", ba.0);
}
}