diff --git a/CHANGELOG.md b/CHANGELOG.md index 7121adc..402b302 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,7 +7,11 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] ### Added +* Getter methods for `PLL`, `RPLL`, `Lowpass`, `Unwrapper` ### Changed +* `Accu`, `Unwrapper`, `overflowing_sub` are now generic. +* Revert `to PLL::update()` returning the phase increment as that has less bias + (while it does have more noise). ### Removed ## [0.6.0] - 2022-01-19 diff --git a/benches/micro.rs b/benches/micro.rs index 9edbc32..d24ed78 100644 --- a/benches/micro.rs +++ b/benches/micro.rs @@ -43,11 +43,11 @@ fn rpll_bench() { fn pll_bench() { let mut dut = PLL::default(); println!( - "PLL::update(t, 12, 12): {}", + "PLL::update(Some(t), 12, 12): {}", bench_env(Some(0x241), |x| dut.update(*x, 12, 12)) ); println!( - "PLL::update(t, sf, sp): {}", + "PLL::update(Some(t), sf, sp): {}", bench_env((Some(0x241), 21, 20), |(x, p, q)| dut.update(*x, *p, *q)) ); } diff --git a/src/accu.rs b/src/accu.rs index 954c35f..41a6dad 100644 --- a/src/accu.rs +++ b/src/accu.rs @@ -1,20 +1,25 @@ +use num_traits::ops::wrapping::WrappingAdd; + #[derive(Copy, Clone, Default, PartialEq, Debug)] -pub struct Accu { - state: i32, - step: i32, +pub struct Accu { + state: T, + step: T, } -impl Accu { - pub fn new(state: i32, step: i32) -> Self { +impl Accu { + pub fn new(state: T, step: T) -> Self { Self { state, step } } } -impl Iterator for Accu { - type Item = i32; - fn next(&mut self) -> Option { +impl Iterator for Accu +where + T: WrappingAdd + Copy, +{ + type Item = T; + fn next(&mut self) -> Option { let s = self.state; - self.state = self.state.wrapping_add(self.step); + self.state = s.wrapping_add(&self.step); Some(s) } } diff --git a/src/lowpass.rs b/src/lowpass.rs index e7037fe..a30a54d 100644 --- a/src/lowpass.rs +++ b/src/lowpass.rs @@ -34,6 +34,11 @@ impl Lowpass { *y += dy; x = *y - (dy >> 1); } - x.saturating_add((self.y.len() as i32) << (k - 1).max(0)) + x.saturating_add((N as i32) << (k - 1).max(0)) + } + + /// Return the current filter output + pub fn output(&self) -> i32 { + self.y[N - 1] } } diff --git a/src/pll.rs b/src/pll.rs index 2385948..357e332 100644 --- a/src/pll.rs +++ b/src/pll.rs @@ -3,8 +3,8 @@ 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 transfer function is I^2,I from input phase to output phase and P,I from input phase to -/// output frequency. +/// 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. /// @@ -14,8 +14,8 @@ use serde::{Deserialize, Serialize}; /// /// 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 within one turn, there is a typically very small frequency -/// overshoot. +/// 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 @@ -26,9 +26,10 @@ use serde::{Deserialize, Serialize}; /// 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 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 affects dynamic range, gain, and noise accordingly. +/// 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. @@ -54,7 +55,7 @@ impl PLL { /// per update. A good value is typically `shift_frequency - 1`. /// /// Returns: - /// A tuple of instantaneous phase and frequency. + /// A tuple of instantaneous phase and frequency estimates. pub fn update(&mut self, x: Option, shift_frequency: u32, shift_phase: u32) -> (i32, i32) { debug_assert!((1..=30).contains(&shift_frequency)); debug_assert!((1..=30).contains(&shift_phase)); @@ -74,13 +75,23 @@ impl PLL { >> shift_phase; self.y = self.y.wrapping_add(dy); let y = self.y.wrapping_sub(dy >> 1); - (y, f) + (y, f.wrapping_add(dy)) } else { self.x = self.x.wrapping_add(self.f); self.y = self.y.wrapping_add(self.f); (self.y, self.f) } } + + /// Return the current phase estimate + pub fn phase(&self) -> i32 { + self.y + } + + /// Return the current frequency estimate + pub fn frequency(&self) -> i32 { + self.f + } } #[cfg(test)] @@ -91,7 +102,7 @@ mod tests { let mut p = PLL::default(); let (y, f) = p.update(Some(0x10000), 8, 4); assert_eq!(y, 0x87c); - assert_eq!(f, 0x80); + assert_eq!(f, 0x1078); } #[test] diff --git a/src/rpll.rs b/src/rpll.rs index 2d1a2c5..e307d96 100644 --- a/src/rpll.rs +++ b/src/rpll.rs @@ -78,6 +78,16 @@ impl RPLL { } (self.y, self.f) } + + /// Return the current phase estimate + pub fn phase(&self) -> i32 { + self.y + } + + /// Return the current frequency estimate + pub fn frequency(&self) -> u32 { + self.f + } } #[cfg(test)] diff --git a/src/unwrap.rs b/src/unwrap.rs index bf5c2b8..b500a4e 100644 --- a/src/unwrap.rs +++ b/src/unwrap.rs @@ -1,3 +1,5 @@ +use core::cmp::PartialOrd; +use num_traits::{identities::Zero, ops::wrapping::WrappingSub}; use serde::{Deserialize, Serialize}; /// Subtract `y - x` with signed overflow. @@ -10,9 +12,12 @@ use serde::{Deserialize, Serialize}; /// A tuple containg the (wrapped) difference `y - x` and the signum of the /// overflow. #[inline(always)] -pub fn overflowing_sub(y: i32, x: i32) -> (i32, i32) { - let delta = y.wrapping_sub(x); - let wrap = (delta >= 0) as i32 - (y >= x) as i32; +pub fn overflowing_sub(y: T, x: T) -> (T, i32) +where + T: WrappingSub + Zero + PartialOrd, +{ + let delta = y.wrapping_sub(&x); + let wrap = (delta >= T::zero()) as i32 - (y >= x) as i32; (delta, wrap) } @@ -42,14 +47,17 @@ pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 { /// This is unwrapping as in the phase and overflow unwrapping context, not /// unwrapping as in the `Result`/`Option` context. #[derive(Copy, Clone, Default, Deserialize, Serialize)] -pub struct Unwrapper { +pub struct Unwrapper { // last input - x: i32, + x: T, // last wraps w: i32, } -impl Unwrapper { +impl Unwrapper +where + T: WrappingSub + Zero + PartialOrd + Copy, +{ /// Unwrap a new sample from a sequence and update the unwrapper state. /// /// Args: @@ -58,12 +66,22 @@ impl Unwrapper { /// Returns: /// A tuple containing the (wrapped) difference `x - x_old` and the /// signed number of wraps accumulated by the new sample. - pub fn update(&mut self, x: i32) -> (i32, i32) { + pub fn update(&mut self, x: T) -> (T, i32) { let (dx, dw) = overflowing_sub(x, self.x); self.x = x; self.w = self.w.wrapping_add(dw as i32); (dx, self.w) } + + /// Return the current number of wraps + pub fn wraps(&self) -> i32 { + self.w + } + + /// Return the last known phase + pub fn phase(&self) -> T { + self.x + } } #[cfg(test)]