idsp/iir/
normal.rs

1//! Normal form second order section
2
3use core::ops::{Add, Mul, Neg};
4use dsp_process::{SplitInplace, SplitProcess};
5
6use num_traits::AsPrimitive;
7
8#[cfg(not(feature = "std"))]
9#[allow(unused_imports)]
10use num_traits::float::Float as _;
11
12use crate::Complex;
13
14use super::DirectForm1;
15
16/// Normal form second order section
17///
18/// Also known as Rader Gold oscillator, or Chamberlain form IIR.
19/// A direct form implementation has bad pole resolution near the real axis.
20/// The normal form has constant pole resolution in the plane.
21///
22/// This implementation includes a second order all-zeros before the all-poles section.
23///
24/// The `y0`/`y1` fields of [`DirectForm1`] hold the in-phase and quadrature
25/// components of the current output.
26#[derive(Debug, Clone, Default, serde::Serialize, serde::Deserialize)]
27pub struct Normal<C> {
28    /// Feed forward coefficients
29    pub b: [C; 3],
30    /// Pole
31    ///
32    /// Conjugate pole pair at: `p.re() +- 1j*p.im()`
33    pub p: Complex<C>,
34}
35
36/// The y1, y2 aren't DF1 but y1.re() and y1.im()
37impl<
38    C: Copy + Mul<T, Output = A> + Neg<Output = C>,
39    A: Add<Output = A> + AsPrimitive<T>,
40    T: 'static + Copy,
41> SplitProcess<T, T, DirectForm1<T>> for Normal<C>
42{
43    fn process(&self, state: &mut DirectForm1<T>, x0: T) -> T {
44        let y1 = (self.b[0] * x0
45            + self.b[1] * state.x[0]
46            + self.b[2] * state.x[1]
47            + self.p.re() * state.y[0][1]
48            + -self.p.im() * state.y[0][0])
49            .as_();
50        let y0: T = (self.p.im() * state.y[0][1] + self.p.re() * state.y[0][0]).as_();
51        state.x = [x0, state.x[0]];
52        state.y[0] = [y0, y1];
53        y0
54    }
55}
56
57impl<C, T: Copy> SplitInplace<T, DirectForm1<T>> for Normal<C> where
58    Self: SplitProcess<T, T, DirectForm1<T>>
59{
60}
61
62impl<C: Neg<Output = C> + From<f64>> From<&[[f64; 3]; 2]> for Normal<C> {
63    fn from(ba: &[[f64; 3]; 2]) -> Self {
64        let a0 = 1.0 / ba[1][0];
65        let b = [ba[0][0] * a0, ba[0][1] * a0, ba[0][2] * a0];
66        // Roots of a0 * z * z + a1 * z + a2
67        let p2 = -0.5 * ba[1][1];
68        let pq = ba[1][0] * ba[1][2] - p2.powi(2);
69        assert!(pq >= 0.0);
70        let p = [p2 * a0, pq.sqrt() * a0];
71        Self {
72            b: b.map(Into::into),
73            p: Complex(p.map(Into::into)),
74        }
75    }
76}
idsp/iir/normal.rs

idsp/iir/
normal.rs
