rpll: refine, simplify, document and comment

dsp/src/rpll.rs

@@ -8,7 +8,7 @@ pub struct RPLL {
     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
+    ff: i32, // current frequency estimate from frequency loop
     f: i32,  // current frequency estimate from both frequency and phase loop
     y: i32,  // current phase estimate
 }
@@ -23,20 +23,22 @@ impl RPLL {
     /// Returns:
     /// Initialized RPLL instance.
     pub fn new(dt2: u8, t: i32) -> RPLL {
-        let mut pll = RPLL::default();
-        pll.dt2 = dt2;
-        pll.t = t;
-        pll
+        RPLL {
+            dt2,
+            t,
+            ..Default::default()
+        }
     }
 
     /// Advance the RPLL and optionally supply a new timestamp.
     ///
     /// Args:
     /// * input: Optional new timestamp (wrapping around at the i32 boundary).
+    ///   There can be at most one timestamp per `update()` cycle (1 << dt2 counter cycles).
     /// * 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_phase: Phase lock settling time. Usually one less than
     ///   `shift_frequency` (see there).
     ///
     /// Returns:
@@ -48,23 +50,34 @@ impl RPLL {
         shift_frequency: u8,
         shift_phase: u8,
     ) -> (i32, i32) {
-        debug_assert!(shift_frequency > 0);
-        debug_assert!(shift_phase > 0);
-        debug_assert!(32 + self.dt2 >= shift_frequency);
-        self.y = self.y.wrapping_add(self.f as i32);
+        debug_assert!(shift_frequency > self.dt2);
+        debug_assert!(shift_phase > self.dt2);
+        // Advance phase
+        self.y = self.y.wrapping_add(self.f);
         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,
-            );
+            // Reference period in counter cycles
+            let dx = x.wrapping_sub(self.x);
+            // Store timestamp for next time.
             self.x = x;
+            // Phase using the current frequency estimate
+            let p_sig_long = (self.ff as i64).wrapping_mul(dx as i64);
+            // Add half-up rounding bias and apply gain/attenuation
+            let p_sig = (p_sig_long.wrapping_add(1i64 << (shift_frequency - 1))
+                >> shift_frequency) as i32;
+            // Reference phase (1 << dt2 full turns) with gain/attenuation applied
+            let p_ref = 1i32 << (32 + self.dt2 - shift_frequency);
+            // Update frequency lock
+            self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
+            // Time in counter cycles between timestamp and "now"
+            let dt = self.t.wrapping_sub(x);
+            // Reference phase estimate "now"
+            let y_ref = (self.f >> self.dt2).wrapping_mul(dt);
+            // Phase error
+            let dy = y_ref.wrapping_sub(self.y);
+            // Current frequency estimate from frequency lock and phase error
+            self.f = self.ff.wrapping_add(dy >> (shift_phase - self.dt2));
         }
+        // Advance time
         self.t = self.t.wrapping_add(1 << self.dt2);
         (self.y, self.f)
     }