pounder_test/dsp/src/lowpass.rs

40 lines
1.1 KiB
Rust
Raw Normal View History

/// Arbitrary order, high dynamic range, wide coefficient range,
2021-02-11 21:30:05 +08:00
/// lowpass filter implementation. DC gain is 1.
///
2021-02-11 21:30:05 +08:00
/// Type argument N is the filter order.
2021-05-10 23:31:53 +08:00
#[derive(Clone)]
pub struct Lowpass<const N: usize> {
// IIR state storage
2021-05-10 23:31:53 +08:00
y: [i32; N],
}
impl<const N: usize> Default for Lowpass<N> {
fn default() -> Self {
Lowpass { y: [0i32; N] }
}
}
2021-05-10 23:31:53 +08:00
impl<const N: usize> Lowpass<N> {
/// Update the filter with a new sample.
///
/// # Args
/// * `x`: Input data. Needs 1 bit headroom but will saturate cleanly beyond that.
/// * `k`: Log2 time constant, 1..=31.
///
2021-02-10 16:46:49 +08:00
/// # Return
/// Filtered output y.
2021-02-10 16:46:49 +08:00
pub fn update(&mut self, x: i32, k: u8) -> i32 {
2021-02-11 21:30:05 +08:00
debug_assert!(k & 31 == k);
// This is an unrolled and optimized first-order IIR loop
// that works for all possible time constants.
// Note T-DF-I and the zeros at Nyquist.
let mut x = x;
2021-02-11 21:30:05 +08:00
for y in self.y.iter_mut() {
let dy = x.saturating_sub(*y) >> k;
2021-02-11 21:30:05 +08:00
*y += dy;
2021-02-12 18:06:59 +08:00
x = *y - (dy >> 1);
}
2021-06-23 04:53:51 +08:00
x.saturating_add((self.y.len() as i32) << (k - 1).max(0))
}
}