idsp/
lowpass.rs

1use dsp_process::{SplitInplace, SplitProcess};
2
3/// Arbitrary order, high dynamic range, wide coefficient range,
4/// lowpass filter implementation. DC gain is 1.
5///
6/// Type argument N is the filter order. N must be `1` or `2`.
7///
8/// The filter will cleanly saturate towards the `i32` range.
9///
10/// Both filters have been optimized for accuracy, dynamic range, and
11/// speed on Cortex-M7.
12#[derive(Clone, Debug)]
13pub struct Lowpass<const N: usize>(pub [i32; N]);
14
15/// Lowpass filter state
16#[derive(Clone, Debug)]
17pub struct LowpassState<const N: usize>(pub [i64; N]);
18
19impl<const N: usize> Default for LowpassState<N>
20where
21    [i64; N]: Default,
22{
23    fn default() -> Self {
24        Self(Default::default())
25    }
26}
27
28impl<const N: usize> SplitProcess<i32, i32, LowpassState<N>> for Lowpass<N> {
29    /// The filter configuration `Config` contains the filter gains.
30    ///
31    /// For the first-order lowpass this is a single element array `[k]` with
32    /// the corner frequency in scaled Q31:
33    /// `k = pi*(1 << 31)*f0/fn` where
34    /// `f0` is the 3dB corner frequency and
35    /// `fn` is the Nyquist frequency.
36    /// The corner frequency is warped in the usual way.
37    ///
38    /// For the second-order lowpass this is `[k**2/(1 << 32), -k/q]` with `q = 1/sqrt(2)`
39    /// for a Butterworth response.
40    ///
41    /// In addition to the poles at the corner frequency the filters have zeros at Nyquist.
42    ///
43    /// The first-order lowpass works fine and accurate for any positive gain
44    /// `1 <= k <= (1 << 31) - 1`.
45    /// The second-order lowpass works and is accurate for
46    /// `1 << 16 <= k <= q*(1 << 31)`.
47    fn process(&self, state: &mut LowpassState<N>, x: i32) -> i32 {
48        // d = (x0 - p1)*k0
49        // p0 = p1 + 2d
50        // y0 = p1 + d
51        //
52        // d = (x0 - p1)*k0 + q1*k1
53        // q0 = q1 + 2d
54        // p0 = p1 + 2q1 + 2d
55        // y0 = p1 + q1 + d
56        let mut d = x.saturating_sub((state.0[0] >> 32) as i32) as i64 * self.0[0] as i64;
57        let y;
58        if N == 1 {
59            state.0[0] += d;
60            y = (state.0[0] >> 32) as i32;
61            state.0[0] += d;
62        } else if N == 2 {
63            d += (state.0[1] >> 32) * self.0[1] as i64;
64            state.0[1] += d;
65            state.0[0] += state.0[1];
66            y = (state.0[0] >> 32) as i32;
67            // This creates the double Nyquist zero,
68            // compensates the gain lost in the signed i32 as (i32 as i64)*(i64 >> 32)
69            // multiplication while keeping the lowest bit significant, and
70            // copes better with wrap-around than Nyquist averaging.
71            state.0[0] += state.0[1];
72            state.0[1] += d;
73        } else {
74            unimplemented!()
75        }
76        y
77    }
78}
79
80impl<const N: usize> SplitInplace<i32, LowpassState<N>> for Lowpass<N> {}
81
82impl<const N: usize> Default for Lowpass<N> {
83    fn default() -> Self {
84        Self([0; N])
85    }
86}
87
88/// First order lowpass
89pub type Lowpass1 = Lowpass<1>;
90/// Second order lowpass
91pub type Lowpass2 = Lowpass<2>;
idsp/lowpass.rs

idsp/
lowpass.rs
