idsp/
lowpass.rs

1use crate::Filter;
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(Copy, Clone)]
13pub struct Lowpass<const N: usize>(pub(crate) [i64; N]);
14impl<const N: usize> Filter for Lowpass<N> {
15    /// The filter configuration `Config` contains the filter gains.
16    ///
17    /// For the first-order lowpass this is a single element array `[k]` with
18    /// the corner frequency in scaled Q31:
19    /// `k = pi*(1 << 31)*f0/fn` where
20    /// `f0` is the 3dB corner frequency and
21    /// `fn` is the Nyquist frequency.
22    /// The corner frequency is warped in the usual way.
23    ///
24    /// For the second-order lowpass this is `[k**2/(1 << 32), -k/q]` with `q = 1/sqrt(2)`
25    /// for a Butterworth response.
26    ///
27    /// In addition to the poles at the corner frequency the filters have zeros at Nyquist.
28    ///
29    /// The first-order lowpass works fine and accurate for any positive gain
30    /// `1 <= k <= (1 << 31) - 1`.
31    /// The second-order lowpass works and is accurate for
32    /// `1 << 16 <= k <= q*(1 << 31)`.
33    type Config = [i32; N];
34    fn update(&mut self, x: i32, k: &Self::Config) -> i32 {
35        let mut d = x.saturating_sub(self.get()) as i64 * k[0] as i64;
36        let y;
37        if N == 1 {
38            self.0[0] += d;
39            y = self.get();
40            self.0[0] += d;
41        } else if N == 2 {
42            d += (self.0[1] >> 32) * k[1] as i64;
43            self.0[1] += d;
44            self.0[0] += self.0[1];
45            y = self.get();
46            // This creates the double Nyquist zero,
47            // compensates the gain lost in the signed i32 as (i32 as i64)*(i64 >> 32)
48            // multiplication while keeping the lowest bit significant, and
49            // copes better with wrap-around than Nyquist averaging.
50            self.0[0] += self.0[1];
51            self.0[1] += d;
52        } else {
53            unimplemented!()
54        }
55        y
56    }
57
58    fn get(&self) -> i32 {
59        (self.0[0] >> 32) as i32
60    }
61
62    fn set(&mut self, x: i32) {
63        self.0[0] = (x as i64) << 32;
64    }
65}
66
67impl<const N: usize> Default for Lowpass<N> {
68    fn default() -> Self {
69        Self([0; N])
70    }
71}
72
73/// First order lowpass
74pub type Lowpass1 = Lowpass<1>;
75/// Second order lowpass
76pub type Lowpass2 = Lowpass<2>;
idsp/lowpass.rs

idsp/
lowpass.rs
