From 1749d48ca390a4df27dd770a998914fd5fd5fded Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Wed, 27 Jan 2021 09:00:48 +0100
Subject: [PATCH] Revert "rpll: auto-align counter"

This reverts commit dbacc5293e12f712fef7bd85848e1b0bd8fde823.
---
 dsp/src/rpll.rs   | 18 ++++--------------
 src/bin/lockin.rs |  2 +-
 2 files changed, 5 insertions(+), 15 deletions(-)

diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs
index 292c523..2bfd29c 100644
--- a/dsp/src/rpll.rs
+++ b/dsp/src/rpll.rs
@@ -18,12 +18,14 @@ impl RPLL {
     ///
     /// 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) -> RPLL {
+    pub fn new(dt2: u8, t: i32) -> RPLL {
         RPLL {
             dt2,
+            t,
             ..Default::default()
         }
     }
@@ -67,19 +69,7 @@ impl RPLL {
             // Update frequency lock
             self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
             // Time in counter cycles between timestamp and "now"
-            let mut dt = self.t.wrapping_sub(x);
-
-            if dt < 0 {
-                // Timestamp is in the future
-                // Advance phase and time until we are just past the most recent
-                // timestamp.
-                let mut dt_ceil = dt >> self.dt2;
-                self.y = self.y.wrapping_sub(self.f.wrapping_mul(dt_ceil));
-                dt_ceil <<= self.dt2;
-                self.t = self.t.wrapping_sub(dt_ceil);
-                dt = dt.wrapping_sub(dt_ceil);
-            }
-
+            let dt = self.t.wrapping_sub(x);
             // Reference phase estimate "now"
             let y_ref = (self.f >> self.dt2).wrapping_mul(dt);
             // Phase error
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 1c2aa1f..7066ad8 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -53,7 +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::lowpass(1e-3, 0.707, 2.), // TODO: expose