idsp/
pll.rs

1use serde::{Deserialize, Serialize};
2
3/// Type-II, sampled phase, discrete time PLL
4///
5/// This PLL tracks the frequency and phase of an input signal with respect to the sampling clock.
6/// The open loop transfer function is I^2,I from input phase to output phase and P,I from input
7/// phase to output frequency.
8///
9/// The transfer functions (for phase and frequency) contain an additional zero at Nyquist.
10///
11/// The PLL locks to any frequency (i.e. it locks to the alias in the first Nyquist zone) and is
12/// stable for any gain (1 <= shift <= 30). It has a single parameter that determines the loop
13/// bandwidth in octave steps. The gain can be changed freely between updates.
14///
15/// The frequency and phase settling time constants for a frequency/phase jump are `1 << shift`
16/// update cycles. The loop bandwidth is `1/(2*pi*(1 << shift))` in units of the sample rate.
17/// While the phase is being settled after settling the frequency, there is a typically very
18/// small frequency overshoot.
19///
20/// All math is naturally wrapping 32 bit integer. Phase and frequency are understood modulo that
21/// overflow in the first Nyquist zone. Expressing the IIR equations in other ways (e.g. single
22/// (T)-DF-{I,II} biquad/IIR) would break on overflow (i.e. every cycle).
23///
24/// There are no floating point rounding errors here. But there is integer quantization/truncation
25/// error of the `shift` lowest bits leading to a phase offset for very low gains. Truncation
26/// bias is applied. Rounding is "half up". The phase truncation error can be removed very
27/// efficiently by dithering.
28///
29/// This PLL does not unwrap phase slips accumulated during (frequency) lock acquisition.
30/// This can and should be implemented elsewhere by unwrapping and scaling the input phase
31/// and un-scaling and wrapping output phase and frequency. This then affects dynamic range,
32/// gain, and noise accordingly.
33///
34/// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to
35/// increase resolution for extremely narrowband applications is obvious.
36///
37/// This PLL implements first order noise shaping to reduce quantization errors.
38#[derive(Copy, Clone, Default, Deserialize, Serialize)]
39pub struct PLL {
40    // last input phase
41    x: i32,
42    // last output phase
43    y0: i32,
44    // last output frequency
45    f0: i32,
46    // filtered frequency
47    f: i64,
48    // filtered output phase
49    y: i64,
50}
51
52impl PLL {
53    /// Update the PLL with a new phase sample. This needs to be called (sampled) periodically.
54    /// The signal's phase/frequency is reconstructed relative to the sampling period.
55    ///
56    /// Args:
57    /// * `x`: New input phase sample or None if a sample has been missed.
58    /// * `k`: Feedback gain.
59    ///
60    /// Returns:
61    /// A tuple of instantaneous phase and frequency estimates.
62    pub fn update(&mut self, x: Option<i32>, k: i32) {
63        if let Some(x) = x {
64            let dx = x.wrapping_sub(self.x);
65            self.x = x;
66            let df = dx.wrapping_sub((self.f >> 32) as i32) as i64 * k as i64;
67            self.f = self.f.wrapping_add(df);
68            self.y = self.y.wrapping_add(self.f);
69            self.f = self.f.wrapping_add(df);
70            let dy = x.wrapping_sub((self.y >> 32) as i32) as i64 * k as i64;
71            self.y = self.y.wrapping_add(dy);
72            let y = (self.y >> 32) as i32;
73            self.y = self.y.wrapping_add(dy);
74            self.f0 = y.wrapping_sub(self.y0);
75            self.y0 = y;
76        } else {
77            self.y = self.y.wrapping_add(self.f);
78            self.x = self.x.wrapping_add(self.f0);
79            self.y0 = self.y0.wrapping_add(self.f0);
80        }
81    }
82
83    /// Return the current phase estimate
84    pub fn phase(&self) -> i32 {
85        self.y0
86    }
87
88    /// Return the current frequency estimate
89    pub fn frequency(&self) -> i32 {
90        self.f0
91    }
92}
93
94#[cfg(test)]
95mod tests {
96    use super::*;
97    #[test]
98    fn mini() {
99        let mut p = PLL::default();
100        let k = 1 << 24;
101        p.update(Some(0x10000), k);
102        assert_eq!(p.phase(), 0x1ff);
103        assert_eq!(p.frequency(), 0x1ff);
104    }
105
106    #[test]
107    fn converge() {
108        let mut p = PLL::default();
109        let k = 1 << 24;
110        let f0 = 0x71f63049_i32;
111        let n = 1 << 14;
112        let mut x = 0i32;
113        for i in 0..n {
114            x = x.wrapping_add(f0);
115            p.update(Some(x), k);
116            if i > n / 4 {
117                assert_eq!(p.frequency().wrapping_sub(f0).abs() <= 1, true);
118            }
119            if i > n / 2 {
120                assert_eq!(p.phase().wrapping_sub(x).abs() <= 1, true);
121            }
122        }
123    }
124}
idsp/pll.rs

idsp/
pll.rs
