From ea7b08fc6420d42e0619b2b9e0302128629ca605 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 26 Jan 2021 14:40:44 +0100 Subject: [PATCH] rpll: refine --- dsp/src/lockin.rs | 15 +++++++---- dsp/src/rpll.rs | 67 +++++++++++++++++++++++++++++++++-------------- src/bin/lockin.rs | 10 +++---- 3 files changed, 63 insertions(+), 29 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 2449a17..b542bd4 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -405,8 +405,7 @@ mod test { "Too many timestamps per batch. Each batch can have at most 1 timestamp." ); - let adc_sample_ticks_log2 = - (internal_frequency).log2().round() as u8; + let adc_sample_ticks_log2 = (internal_frequency).log2().round() as u8; assert!( adc_sample_ticks_log2 + sample_buffer_size_log2 <= 32, "The base-2 log of the number of ADC ticks in a sampling period plus the base-2 log of the sample buffer size must be less than 32." @@ -422,8 +421,10 @@ mod test { 2., ), // DC gain to get to full scale with the image filtered out ); - let mut timestamp_handler = - RPLL::new((adc_sample_ticks_log2 + sample_buffer_size_log2) as u8); + let mut timestamp_handler = RPLL::new( + (adc_sample_ticks_log2 + sample_buffer_size_log2) as u8, + 0, + ); let mut timestamp_start: u64 = 0; let time_constant: f64 = 1. / (2. * PI * corner_frequency); @@ -484,7 +485,11 @@ mod test { timestamp_start += batch_sample_count; let (demodulation_initial_phase, demodulation_frequency) = - timestamp_handler.update(timestamp.map(|t| t as i32), pll_shift_frequency, pll_shift_phase); + timestamp_handler.update( + timestamp.map(|t| t as i32), + pll_shift_frequency, + pll_shift_phase, + ); let output = lockin.update( adc_signal, demodulation_initial_phase as i32, diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 7aa7d2e..ef8a2ac 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -1,20 +1,47 @@ +/// Reciprocal PLL. +/// +/// Consumes noisy, quantized timestamps of a reference signal and reconstructs +/// the phase and frequency of the update() invocations with respect to (and in units of +/// 1 << 32 of) that reference. #[derive(Copy, Clone, Default)] pub struct RPLL { - dt2: u8, - t: i32, - f2: i64, - y1: i32, - xj1: i32, - f1: i32, + dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio + t: i32, // current counter time + x: i32, // previous timestamp + ff: i32, // current frequency estimate from the frequency loop + f: i32, // current frequency estimate from both frequency and phase loop + y: i32, // current phase estimate } impl RPLL { - pub fn new(dt2: u8) -> RPLL { + /// Create a new RPLL instance. + /// + /// Args: + /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio. + /// * t: Counter time. Counter value at the first update() call. Typically 0. + /// + /// Returns: + /// Initialized RPLL instance. + pub fn new(dt2: u8, t: i32) -> RPLL { let mut pll = RPLL::default(); pll.dt2 = dt2; + pll.t = t; pll } + /// Advance the RPLL and optionally supply a new timestamp. + /// + /// Args: + /// * input: Optional new timestamp (wrapping around at the i32 boundary). + /// * shift_frequency: Frequency lock settling time. 1 << shift_frequency is + /// frequency lock settling time in counter periods. The settling time must be larger + /// than the signal period to lock to. + /// * shift_phase: Phase lock settling time. Usually the same as + /// `shift_frequency` (see there). + /// + /// Returns: + /// A tuple containing the current phase (wrapping at the i32 boundary, pi) and + /// frequency (wrapping at the i32 boundary, Nyquist) estimate. pub fn update( &mut self, input: Option, @@ -24,19 +51,21 @@ impl RPLL { debug_assert!(shift_frequency > 0); debug_assert!(shift_phase > 0); debug_assert!(32 + self.dt2 >= shift_frequency); - self.y1 = self.y1.wrapping_add(self.f1); - if let Some(xj) = input { - self.f2 = self.f2.wrapping_add((1i64 << 32 + self.dt2 - shift_frequency) - - (self.f2.wrapping_mul(xj.wrapping_sub(self.xj1) as i64) - + (1i64 << shift_frequency - 1) - >> shift_frequency)); - self.f1 = (self.f2 as i32) - .wrapping_add((self.f2.wrapping_mul(self.t.wrapping_sub(xj) as i64) - - ((self.y1 as i64) << self.dt2) - + (1i64 << shift_phase - 1) - >> shift_phase) as i32); + self.y = self.y.wrapping_add(self.f as i32); + if let Some(x) = input { + self.ff = self.ff.wrapping_add((1i32 << 32 + self.dt2 - shift_frequency) + - ((self.ff as i64).wrapping_mul(x.wrapping_sub(self.x) as i64) + + (1i64 << shift_frequency - 1) // half-up rounding bias + >> shift_frequency) as i32); + self.f = self.ff.wrapping_add( + ((self.f as i64).wrapping_mul(self.t.wrapping_sub(x) as i64) + .wrapping_sub((self.y as i64) << self.dt2) + // + (1i64 << shift_phase - 1) + >> shift_phase) as i32, + ); + self.x = x; } self.t = self.t.wrapping_add(1 << self.dt2); - (self.y1, self.f1) + (self.y, self.f) } } diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 5845e0a..e1fbb6d 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -53,8 +53,7 @@ const APP: () = { // Configure the microcontroller let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); - let pll = - RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2); + let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0); let lockin = Lockin::new( &iir_int::IIRState::default(), // TODO: lowpass, expose @@ -128,9 +127,10 @@ const APP: () = { let harmonic: i32 = -1; // Demodulation LO phase offset let phase_offset: i32 = 0; - let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic); - let mut sample_phase = phase_offset - .wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); + let sample_frequency = + (pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2).wrapping_mul(harmonic); + let mut sample_phase = + phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); for i in 0..adc_samples[0].len() { // Convert to signed, MSB align the ADC sample.