diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 389bda5..10cfcaa 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -5,6 +5,7 @@ pub type Complex = (T, T); pub mod iir; pub mod lockin; pub mod pll; +pub mod unwrap; #[cfg(test)] mod testing; diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs index 8cef99b..8f060c2 100644 --- a/dsp/src/pll.rs +++ b/dsp/src/pll.rs @@ -29,7 +29,7 @@ use serde::{Deserialize, Serialize}; /// 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. #[derive(Copy, Clone, Default, Deserialize, Serialize)] -pub struct PLLState { +pub struct PLL { // last input phase x: i32, // filtered frequency @@ -38,7 +38,7 @@ pub struct PLLState { y: i32, } -impl PLLState { +impl PLL { /// Update the PLL with a new phase sample. /// /// Args: @@ -49,7 +49,7 @@ impl PLLState { /// Returns: /// A tuple of instantaneous phase and frequency (the current phase increment). pub fn update(&mut self, x: i32, shift: u8) -> (i32, i32) { - debug_assert!(shift >= 1 && shift <= 31); + debug_assert!((1..=30).contains(&shift)); let bias = 1i32 << shift; let e = x.wrapping_sub(self.f); self.f = self.f.wrapping_add( @@ -57,7 +57,7 @@ impl PLLState { ); self.x = x; let f = self.f.wrapping_add( - bias.wrapping_add(e).wrapping_sub(self.y) >> shift - 1, + bias.wrapping_add(e).wrapping_sub(self.y) >> (shift - 1), ); self.y = self.y.wrapping_add(f); (self.y, f) @@ -69,9 +69,28 @@ mod tests { use super::*; #[test] fn mini() { - let mut p = PLLState::default(); + let mut p = PLL::default(); let (y, f) = p.update(0x10000, 10); assert_eq!(y, 0xc2); assert_eq!(f, y); } + + #[test] + fn converge() { + let mut p = PLL::default(); + let f0 = 0x71f63049_i32; + let shift = 10; + let n = 31 << shift + 2; + let mut x = 0i32; + for i in 0..n { + x = x.wrapping_add(f0); + let (y, f) = p.update(x, shift); + if i > n / 4 { + assert_eq!(f.wrapping_sub(f0).abs() <= 1, true); + } + if i > n / 2 { + assert_eq!(y.wrapping_sub(x).abs() < 1 << 18, true); + } + } + } } diff --git a/dsp/src/unwrap.rs b/dsp/src/unwrap.rs new file mode 100644 index 0000000..8bc5ffd --- /dev/null +++ b/dsp/src/unwrap.rs @@ -0,0 +1,73 @@ +use serde::{Deserialize, Serialize}; + +/// Get phase wrap from x to y. +/// +/// Phases are modulo integer overflow. +/// +/// Args: +/// * `x`: Old phase sample +/// * `y`: New phase sample +/// +/// Returns: +/// A tuple containg the (wrapped) phase difference and +/// one times the direction of the wrap. +pub fn get_wrap(x: i32, y: i32) -> (i32, i8) { + let delta = y.wrapping_sub(x); + let wrap = (delta >= 0) as i8 - (y >= x) as i8; + (delta, wrap) +} + +/// Phase unwrapper. +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct Unwrapper { + // last input + x: i32, + // last wraps + v: i32, +} + +impl Unwrapper { + /// Unwrap a new sample from a phase sequence and update the + /// unwrapper state. + /// + /// Args: + /// * `x`: New phase sample + /// + /// Returns: + /// A tuple containing the (wrapped) phase difference + /// and the signed number of phase wraps corresponding to the new sample. + pub fn update(&mut self, x: i32) -> (i32, i32) { + let (dx, v) = get_wrap(self.x, x); + self.x = x; + self.v = self.v.wrapping_add(v as i32); + (dx, self.v) + } +} + +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn mini() { + for (x0, x1, v) in [ + (0i32, 0i32, 0i8), + (1, 1, 0), + (-1, -1, 0), + (1, -1, 0), + (-1, 1, 0), + (0, 0x7fff_ffff, 0), + (-1, 0x7fff_ffff, -1), + (0, -0x8000_0000, 0), + (1, -0x8000_0000, 1), + (-0x6000_0000, 0x6000_0000, -1), + (0x6000_0000, -0x6000_0000, 1), + (0x6000_0000, -0x6000_0000, 1), + (0x6000_0000, -0x6000_0000, 1), + ] + .iter() + { + let (_dx, w) = get_wrap(*x0, *x1); + assert_eq!(*v, w, " = get_wrap({:#x}, {:#x})", *x0, *x1); + } + } +}