idsp/iir/
wdf.rs

1//! Wave digital filters
2
3#[cfg(not(feature = "std"))]
4#[allow(unused_imports)]
5use num_traits::float::FloatCore as _;
6
7use dsp_fixedpoint::Q32;
8use dsp_process::{SplitInplace, SplitProcess};
9
10/// Two port adapter architecture
11///
12/// Each architecture is a nibble in the const generic of [`Wdf`].
13#[derive(Copy, Clone, Debug, PartialEq, Eq, PartialOrd, Ord)]
14#[repr(u8)]
15pub enum Tpa {
16    /// terminate
17    Z = 0x0,
18    /// 1 > g > 1/2: a = g - 1
19    A = 0xA,
20    /// 1/2 >= g > 0: a = -g
21    B = 0xB,
22    /// alternative to B
23    B1 = 0xE,
24    /// g = 0
25    X = 0x1,
26    /// -1/2 <= g < 0: a = g
27    C = 0xC,
28    /// alternative to C
29    C1 = 0xF,
30    /// -1 < g < -1/2: a = -(1 + g)
31    D = 0xD,
32}
33
34impl From<u8> for Tpa {
35    #[inline]
36    fn from(value: u8) -> Self {
37        match value {
38            0xa => Tpa::A,
39            0xb => Tpa::B,
40            0xe => Tpa::B1,
41            0x1 => Tpa::X,
42            0xc => Tpa::C,
43            0xf => Tpa::C1,
44            0xd => Tpa::D,
45            _ => Tpa::Z,
46        }
47    }
48}
49
50impl Tpa {
51    fn quantize(self, g: f64) -> Option<Q32<32>> {
52        // Use negative -0.5 <= a <= 0 instead of the usual positive
53        // as -0.5 just fits the Q32 fixed point range.
54        let a = match self {
55            Self::Z => 0.0,
56            Self::A => g - 1.0,
57            Self::B | Self::B1 => -g,
58            Self::X => 0.0,
59            Self::C | Self::C1 => g,
60            Self::D => -1.0 - g,
61        };
62        (-0.5..=0.0).contains(&a).then_some(Q32::from_f64(a))
63    }
64
65    #[inline]
66    fn adapt(&self, x: [i32; 2], a: Q32<32>) -> [i32; 2] {
67        match self {
68            Tpa::A => {
69                let c = x[1] - x[0];
70                let y = (c * a).wrapping_add(x[1]);
71                [y.wrapping_add(c), y]
72            }
73            Tpa::B => {
74                let c = x[0] - x[1];
75                let y = (c * a).wrapping_add(x[1]);
76                [y, y.wrapping_add(c)]
77            }
78            Tpa::B1 => {
79                let c = x[0] - x[1];
80                let y = c * a;
81                [y.wrapping_add(x[1]), y.wrapping_add(x[0])]
82            }
83            Tpa::X => [x[1], x[0]],
84            Tpa::C => {
85                let c = x[1] - x[0];
86                let y = (c * a).wrapping_sub(x[1]);
87                [y, y.wrapping_add(c)]
88            }
89            Tpa::C1 => {
90                let c = x[1] - x[0];
91                let y = c * a;
92                [y.wrapping_sub(x[1]), y.wrapping_sub(x[0])]
93            }
94            Tpa::D => {
95                let c = x[0] - x[1];
96                let y = (c * a).wrapping_sub(x[1]);
97                [y.wrapping_add(c), y]
98            }
99            Tpa::Z => x,
100        }
101    }
102}
103
104/// Wave digital filter, order N, configuration M
105///
106/// Allpass
107///
108/// The M const generic enforces compile time knowledge about
109/// the two port adapter architecture. Each nibble is one TPA.
110#[derive(Debug, Clone)]
111pub struct Wdf<const N: usize, const M: u32> {
112    /// Filter coefficient
113    pub a: [Q32<32>; N],
114}
115
116impl<const N: usize, const M: u32> Default for Wdf<N, M> {
117    fn default() -> Self {
118        Self {
119            a: [Default::default(); N],
120        }
121    }
122}
123
124impl<const N: usize, const M: u32> Wdf<N, M> {
125    /// Quantize and scale filter coefficients
126    ///
127    /// The coefficients are the allpass poles.
128    /// The type (configuration nibbles M) must match the
129    /// optimal scaled architecture, see [`Tpa`].
130    pub fn quantize(g: &[f64; N]) -> Option<Self> {
131        let mut a = [Default::default(); N];
132        let mut m = M;
133        for (a, g) in a.iter_mut().zip(g) {
134            *a = Tpa::from((m & 0xf) as u8).quantize(*g)?;
135            m >>= 4;
136        }
137        debug_assert_eq!(m, 0);
138        Some(Self { a })
139    }
140}
141
142/// Wave digital filter state, order N
143#[derive(Clone, Debug)]
144pub struct WdfState<const N: usize> {
145    /// Filter state
146    pub z: [i32; N],
147}
148
149impl<const N: usize> Default for WdfState<N> {
150    fn default() -> Self {
151        Self { z: [0; N] }
152    }
153}
154
155impl<const N: usize, const M: u32> SplitProcess<i32, i32, WdfState<N>> for Wdf<N, M> {
156    #[inline]
157    fn process(&self, state: &mut WdfState<N>, x: i32) -> i32 {
158        let mut y = 0;
159        let (m, x, z) =
160            self.a
161                .iter()
162                .zip(state.z.iter_mut())
163                .fold((M, x, &mut y), |(m, mut x, y), (a, z)| {
164                    [*y, x] = Tpa::from((m & 0xf) as u8).adapt([x, *z], *a);
165                    (m >> 4, x, z)
166                });
167        debug_assert_eq!(m, 0);
168        *z = x;
169        y
170    }
171}
172
173impl<const N: usize, const M: u32> SplitInplace<i32, WdfState<N>> for Wdf<N, M> {}
idsp/iir/wdf.rs

idsp/iir/
wdf.rs
