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