rpll: implement

dsp/src/lockin.rs

@@ -43,7 +43,7 @@ mod test {
         atan2,
         iir_int::{IIRState, IIR},
         lockin::Lockin,
-        rpll::TimestampHandler,
+        rpll::RPLL,
         testing::{isclose, max_error},
         Complex,
     };
@@ -422,12 +422,8 @@ mod test {
                 2.,
             ), // DC gain to get to full scale with the image filtered out
         );
-        let mut timestamp_handler = TimestampHandler::new(
+        let mut timestamp_handler =
-            pll_shift_frequency,
+            RPLL::new(adc_sample_ticks_log2 + sample_buffer_size_log2);
-            pll_shift_phase,
-            adc_sample_ticks_log2,
-            sample_buffer_size_log2,
-        );
 
         let mut timestamp_start: u64 = 0;
         let time_constant: f64 = 1. / (2. * PI * corner_frequency);

dsp/src/rpll.rs

@@ -1,100 +1,39 @@
-use super::{divide_round, pll::PLL};
+#[derive(Copy, Clone, Default)]
-
+pub struct RPLL {
-/// Processes external timestamps to produce the frequency and initial phase of the demodulation
+    dt2: u8,
-/// signal.
+    t: i32,
-pub struct TimestampHandler {
+    f2: i64,
-    pll: PLL,
+    y1: i32,
-    pll_shift_frequency: u8,
+    xj1: i32,
-    pll_shift_phase: u8,
+    f1: i32,
-    // Index of the current ADC batch.
-    batch_index: u32,
-    // Most recent phase and frequency values of the external reference.
-    reference_phase: i64,
-    reference_frequency: i64,
-    adc_sample_ticks_log2: usize,
-    sample_buffer_size_log2: usize,
 }
 
-impl TimestampHandler {
+impl RPLL {
-    /// Construct a new `TimestampHandler` instance.
+    pub fn new(dt2: u8) -> RPLL {
-    ///
+        let mut pll = RPLL::default();
-    /// # Args
+        pll.dt2 = dt2;
-    /// * `pll_shift_frequency` - See `PLL::update()`.
+        pll
-    /// * `pll_shift_phase` - See `PLL::update()`.
-    /// * `adc_sample_ticks_log2` - Number of ticks in one ADC sampling timer period.
-    /// * `sample_buffer_size_log2` - Number of ADC samples in one processing batch.
-    ///
-    /// # Returns
-    /// New `TimestampHandler` instance.
-    pub fn new(
-        pll_shift_frequency: u8,
-        pll_shift_phase: u8,
-        adc_sample_ticks_log2: usize,
-        sample_buffer_size_log2: usize,
-    ) -> Self {
-        TimestampHandler {
-            pll: PLL::default(),
-            pll_shift_frequency,
-            pll_shift_phase,
-            batch_index: 0,
-            reference_phase: 0,
-            reference_frequency: 0,
-            adc_sample_ticks_log2,
-            sample_buffer_size_log2,
-        }
     }
 
-    /// Compute the initial phase and frequency of the demodulation signal.
+    pub fn update(
-    ///
+        &mut self,
-    /// # Args
+        input: Option<i32>,
-    /// * `timestamp` - Counter value corresponding to an external reference edge.
+        shift_frequency: u8,
-    ///
+        shift_phase: u8,
-    /// # Returns
+    ) -> (i32, i32) {
-    /// Tuple consisting of the initial phase value and frequency of the demodulation signal.
+        self.y1 += self.f1;
-    pub fn update(&mut self, timestamp: Option<u32>) -> (u32, u32) {
+        if let Some(xj) = input {
-        if let Some(t) = timestamp {
+            self.f2 += (1i64 << 32 + self.dt2 - shift_frequency)
-            let (phase, frequency) = self.pll.update(
+                - (self.f2 * (xj - self.xj1) as i64
-                t as i32,
+                    + (1i64 << shift_frequency - 1)
-                self.pll_shift_frequency,
+                    >> shift_frequency);
-                self.pll_shift_phase,
+            self.f1 = self.f2 as i32
-            );
+                + (self.f2 * (self.t - xj) as i64
-            self.reference_phase = phase as u32 as i64;
+                    - ((self.y1 as i64) << self.dt2)
-            self.reference_frequency = frequency as u32 as i64;
+                    + (1i64 << shift_phase - 1)
+                    >> shift_phase) as i32;
         }
-
+        self.t += 1 << self.dt2;
-        let demodulation_frequency: u32;
+        (self.y1, self.f1)
-        let demodulation_initial_phase: u32;
-
-        if self.reference_frequency == 0 {
-            demodulation_frequency = u32::MAX;
-            demodulation_initial_phase = u32::MAX;
-        } else {
-            demodulation_frequency = divide_round(
-                1 << (32 + self.adc_sample_ticks_log2),
-                self.reference_frequency,
-            ) as u32;
-            demodulation_initial_phase = divide_round(
-                (((self.batch_index as i64)
-                    << (self.adc_sample_ticks_log2
-                        + self.sample_buffer_size_log2))
-                    - self.reference_phase)
-                    << 32,
-                self.reference_frequency,
-            ) as u32;
-        }
-
-        if self.batch_index
-            < (1 << (32
-                - self.adc_sample_ticks_log2
-                - self.sample_buffer_size_log2))
-                - 1
-        {
-            self.batch_index += 1;
-        } else {
-            self.batch_index = 0;
-            self.reference_phase -= 1 << 32;
-        }
-
-        (demodulation_initial_phase, demodulation_frequency)
     }
 }

src/bin/lockin.rs

@@ -16,7 +16,7 @@ use stabilizer::{
     hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
 };
 
-use dsp::{iir, iir_int, lockin::Lockin, rpll::TimestampHandler};
+use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL};
 use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
@@ -44,7 +44,7 @@ const APP: () = {
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
 
         timestamper: InputStamper,
-        pll: TimestampHandler,
+        pll: RPLL,
         lockin: Lockin,
     }
 
@@ -53,12 +53,8 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let pll = TimestampHandler::new(
+        let pll =
-            4, // relative PLL frequency bandwidth: 2**-4, TODO: expose
+            RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2);
-            3, // relative PLL phase bandwidth: 2**-3, TODO: expose
-            ADC_SAMPLE_TICKS_LOG2 as usize,
-            SAMPLE_BUFFER_SIZE_LOG2,
-        );
 
         let lockin = Lockin::new(
             &iir_int::IIRState::default(), // TODO: lowpass, expose
@@ -122,10 +118,11 @@ const APP: () = {
         let iir_state = c.resources.iir_state;
         let lockin = c.resources.lockin;
 
-        let (pll_phase, pll_frequency) = c
+        let (pll_phase, pll_frequency) = c.resources.pll.update(
-            .resources
+            c.resources.timestamper.latest_timestamp().map(|t| t as i32),
-            .pll
+            21, // relative PLL frequency bandwidth: 2**-21, TODO: expose
-            .update(c.resources.timestamper.latest_timestamp());
+            21, // relative PLL phase bandwidth: 2**-21, TODO: expose
+        );
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
         let harmonic: i32 = -1;

src/lib.rs

@@ -9,9 +9,9 @@ pub mod server;
 // The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is
 // equal to 10ns per tick.
 // Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz
-pub const ADC_SAMPLE_TICKS_LOG2: u16 = 8;
+pub const ADC_SAMPLE_TICKS_LOG2: u8 = 8;
 pub const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2;
 
 // The desired ADC sample processing buffer size.
-pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
+pub const SAMPLE_BUFFER_SIZE_LOG2: u8 = 3;
 pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;