idsp/
pll.rs

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

idsp/
pll.rs
