1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76

use crate::Filter;

/// Arbitrary order, high dynamic range, wide coefficient range,
/// lowpass filter implementation. DC gain is 1.
///
/// Type argument N is the filter order. N must be `1` or `2`.
///
/// The filter will cleanly saturate towards the `i32` range.
///
/// Both filters have been optimized for accuracy, dynamic range, and
/// speed on Cortex-M7.
#[derive(Copy, Clone)]
pub struct Lowpass<const N: usize>(pub(crate) [i64; N]);
impl<const N: usize> Filter for Lowpass<N> {
    /// The filter configuration `Config` contains the filter gains.
    ///
    /// For the first-order lowpass this is a single element array `[k]` with
    /// the corner frequency in scaled Q31:
    /// `k = pi*(1 << 31)*f0/fn` where
    /// `f0` is the 3dB corner frequency and
    /// `fn` is the Nyquist frequency.
    /// The corner frequency is warped in the usual way.
    ///
    /// For the second-order lowpass this is `[k**2/(1 << 32), -k/q]` with `q = 1/sqrt(2)`
    /// for a Butterworth response.
    ///
    /// In addition to the poles at the corner frequency the filters have zeros at Nyquist.
    ///
    /// The first-order lowpass works fine and accurate for any positive gain
    /// `1 <= k <= (1 << 31) - 1`.
    /// The second-order lowpass works and is accurate for
    /// `1 << 16 <= k <= q*(1 << 31)`.
    type Config = [i32; N];
    fn update(&mut self, x: i32, k: &Self::Config) -> i32 {
        let mut d = x.saturating_sub(self.get()) as i64 * k[0] as i64;
        let y;
        if N == 1 {
            self.0[0] += d;
            y = self.get();
            self.0[0] += d;
        } else if N == 2 {
            d += (self.0[1] >> 32) * k[1] as i64;
            self.0[1] += d;
            self.0[0] += self.0[1];
            y = self.get();
            // This creates the double Nyquist zero,
            // compensates the gain lost in the signed i32 as (i32 as i64)*(i64 >> 32)
            // multiplication while keeping the lowest bit significant, and
            // copes better with wrap-around than Nyquist averaging.
            self.0[0] += self.0[1];
            self.0[1] += d;
        } else {
            unimplemented!()
        }
        y
    }

    fn get(&self) -> i32 {
        (self.0[0] >> 32) as i32
    }

    fn set(&mut self, x: i32) {
        self.0[0] = (x as i64) << 32;
    }
}

impl<const N: usize> Default for Lowpass<N> {
    fn default() -> Self {
        Self([0; N])
    }
}

/// First order lowpass
pub type Lowpass1 = Lowpass<1>;
/// Second order lowpass
pub type Lowpass2 = Lowpass<2>;