diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs index c3588df..a449339 100644 --- a/dsp/benches/micro.rs +++ b/dsp/benches/micro.rs @@ -44,11 +44,11 @@ fn pll_bench() { let mut dut = PLL::default(); println!( "PLL::update(t, 12, 12): {}", - bench_env(0x241, |x| dut.update(*x, 12, 12)) + bench_env(Some(0x241), |x| dut.update(*x, 12, 12)) ); println!( "PLL::update(t, sf, sp): {}", - bench_env((0x241, 21, 20), |(x, p, q)| dut.update(*x, *p, *q)) + bench_env((Some(0x241), 21, 20), |(x, p, q)| dut.update(*x, *p, *q)) ); } diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs index 989facd..1c6a782 100644 --- a/dsp/src/pll.rs +++ b/dsp/src/pll.rs @@ -45,7 +45,7 @@ impl PLL { /// The signal's phase/frequency is reconstructed relative to the sampling period. /// /// Args: - /// * `x`: New input phase sample. + /// * `x`: New input phase sample or None if a sample has been missed. /// * `shift_frequency`: Frequency error scaling. The frequency gain per update is /// `1/(1 << shift_frequency)`. /// * `shift_phase`: Phase error scaling. The phase gain is `1/(1 << shift_phase)` @@ -55,36 +55,34 @@ impl PLL { /// A tuple of instantaneous phase and frequency (the current phase increment). pub fn update( &mut self, - x: i32, + x: Option, shift_frequency: u8, shift_phase: u8, ) -> (i32, i32) { debug_assert!((1..=30).contains(&shift_frequency)); debug_assert!((1..=30).contains(&shift_phase)); - let e = x.wrapping_sub(self.f); - self.f = self.f.wrapping_add( - (1i32 << (shift_frequency - 1)) - .wrapping_add(e) - .wrapping_sub(self.x) - >> shift_frequency, - ); - self.x = x; - let f = self.f.wrapping_add( - (1i32 << (shift_phase - 1)) - .wrapping_add(e) - .wrapping_sub(self.y) - >> shift_phase, - ); + let f = if let Some(x) = x { + let e = x.wrapping_sub(self.f); + self.f = self.f.wrapping_add( + (1i32 << (shift_frequency - 1)) + .wrapping_add(e) + .wrapping_sub(self.x) + >> shift_frequency, + ); + self.x = x; + self.f.wrapping_add( + (1i32 << (shift_phase - 1)) + .wrapping_add(e) + .wrapping_sub(self.y) + >> shift_phase, + ) + } else { + self.x = self.x.wrapping_add(self.f); + self.f + }; self.y = self.y.wrapping_add(f); (self.y, f) } - - /// Advance the PLL without providing a new timestamp. - pub fn advance(&mut self) -> (i32, i32) { - self.x = self.x.wrapping_add(self.f); - self.y = self.y.wrapping_add(self.f); - (self.y, self.f) - } } #[cfg(test)] @@ -93,7 +91,7 @@ mod tests { #[test] fn mini() { let mut p = PLL::default(); - let (y, f) = p.update(0x10000, 8, 4); + let (y, f) = p.update(Some(0x10000), 8, 4); assert_eq!(y, 0x1100); assert_eq!(f, y); } @@ -107,7 +105,7 @@ mod tests { let mut x = 0i32; for i in 0..n { x = x.wrapping_add(f0); - let (y, f) = p.update(x, shift.0, shift.1); + let (y, f) = p.update(Some(x), shift.0, shift.1); if i > n / 4 { assert_eq!(f.wrapping_sub(f0).abs() <= 1, true); }