rpll: refine

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,

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<i32>,
@@ -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)
     }
 }

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.