idsp/
pll.rs

1use core::num::Wrapping as W;
2use dsp_fixedpoint::Q32;
3use dsp_process::{Process, SplitProcess};
4
5use crate::ClampWrap;
6
7/// Type-2, order-3 sampled phase, discrete time PLL
8///
9/// This PLL tracks the frequency and phase of an input signal with respect to the sampling clock.
10/// The open loop transfer function is type 2 (DC double integrator) from input phase to output phase.
11///
12/// The transfer functions (for phase and frequency) contain an additional zero at Nyquist.
13///
14/// The PLL locks to any frequency (i.e. it locks to the alias in the first Nyquist zone) and is
15/// stable for any numerically valid gain (in units of the sample rate: 7e-5 to 5e-2).
16/// It has a single parameter that determines the loop bandwidth.
17/// The gains can be changed freely between updates.
18///
19/// All math is naturally wrapping 32 bit integer. Phase and frequency are understood modulo that
20/// overflow in the first Nyquist zone. Expressing the IIR equations in other ways (e.g. single
21/// (T)-DF-{I,II} biquad/IIR) would break on overflow (i.e. every cycle).
22///
23/// There are no floating point rounding errors. The integer quantization/truncation
24/// error is fed back (first order noise shaping).
25///
26/// This PLL clamps phase wraps accumulated during (frequency) lock acquisition.
27///
28/// The phase detector is symmetric (additive): the loop filter has negative gain.
29/// The output will compensate the input phase: it will settle to the complement.
30/// The output phase increment (the loop filter output, the frequency) is the negative
31/// of the input increment.
32#[derive(Debug, Clone, Default)]
33pub struct PLL {
34    /// Lead lag coefficients
35    ///
36    /// `f0 += b0*y0 + b1*y1 + a1*f1`
37    pub ba: [Q32<32>; 3],
38}
39
40impl PLL {
41    /// Return Pll from zero/pole/gain
42    pub fn from_zpk(zero: f32, pole: f32, gain: f32) -> Self {
43        Self {
44            ba: [gain, -gain * zero, -(1.0 - pole)].map(Q32::from_f32),
45        }
46    }
47
48    /// Given a crossover, create a PLL
49    ///
50    /// About 1.5 dB peaking, 62 deg phase margin for split=4
51    pub fn from_bandwidth(bw: f32, split: f32) -> Self {
52        let a = bw * 2.0 * core::f32::consts::PI;
53        let z = 1.0 - a / split;
54        let p = 1.0 - a * split;
55        let k = -a * a * split;
56        Self::from_zpk(z, p, k)
57    }
58}
59
60/// PLL state
61#[derive(Debug, Clone, Default)]
62pub struct PLLState {
63    /// Input phase difference clamp
64    pub clamp: ClampWrap<W<i32>>,
65    /// Loop filter state: after clamp
66    pub z0: i32,
67    /// After nyquist zero
68    pub y0: i32,
69    /// After lead-lag
70    pub f0: i64,
71    /// After DC pole
72    pub f: W<i64>,
73    /// Current output phase
74    pub y: W<i32>,
75}
76
77impl PLLState {
78    /// Return the current phase estimate
79    pub fn phase(&self) -> W<i32> {
80        self.y
81    }
82
83    /// Return the current frequency estimate
84    pub fn frequency(&self) -> W<i32> {
85        W((self.f.0 >> 32) as _)
86    }
87}
88
89impl SplitProcess<W<i32>, W<i32>, PLLState> for PLL {
90    fn process(&self, state: &mut PLLState, x: W<i32>) -> W<i32> {
91        // advance output phase, oscillator DC pole
92        state.y += state.frequency();
93        // phase error
94        let z0 = state.clamp.process(x + state.y).0 >> 1;
95        // nyquist zero
96        let y0 = z0 + state.z0;
97        state.z0 = z0;
98        // lead lag, wide state
99        state.f0 +=
100            (self.ba[0] * y0 + self.ba[1] * state.y0 + self.ba[2] * (state.f0 >> 32) as i32).inner
101                + ((self.ba[2].inner as i64 * state.f0 as u32 as i64) >> 32);
102        state.y0 = y0;
103        // DC pole, frequency, wide state
104        state.f += W(state.f0);
105        state.y
106    }
107}
108
109#[cfg(test)]
110mod tests {
111    use crate::Accu;
112
113    use super::*;
114    use core::num::Wrapping as W;
115
116    #[test]
117    fn converge_pll() {
118        let p = PLL::from_bandwidth(5e-2, 4.0);
119        println!("{p:?}");
120        let mut s = PLLState::default();
121        let a = Accu::<W<i32>>::new(W(0x0), W(0x71f63049));
122        let n = 1 << 9;
123        for (i, x) in a.take(n).enumerate() {
124            let y = p.process(&mut s, x);
125            println!("x: {x:#010x} y+x: {:#010x}", y + x);
126            if i > n / 2 {
127                assert!((a.step + s.frequency()).0.abs() <= 1);
128                assert!((x + y).0.abs() <= 4);
129            }
130        }
131    }
132
133    #[test]
134    fn converge_narrow() {
135        let p = PLL::from_bandwidth(8e-5, 4.0);
136        println!("{p:?}");
137        let mut s = PLLState::default();
138        let a = Accu::<W<i32>>::new(W(0x0), W(0x140_1235));
139        let n = 1 << 18;
140        for (i, x) in a.take(n).enumerate() {
141            let y = p.process(&mut s, x);
142            println!("x: {x:#010x} y+x: {:#010x}", y + x);
143            if i > n / 2 {
144                assert!((a.step + s.frequency()).0.abs() <= 1 << 16);
145                assert!((x + y).0.abs() <= 1 << 16);
146            }
147        }
148    }
149}
idsp/pll.rs

idsp/
pll.rs
