move timestamp handling into new TimestampHandler struct

src/main.rs

@@ -68,6 +68,7 @@ const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
 const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;
 
 // The number of ADC batches in one timer overflow period.
+// TODO almost the same as `calculate_timestamp_timer_period`.
 pub const ADC_BATCHES_LOG2: usize =
     32 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_SAMPLE_TICKS_LOG2 as usize;
 pub const ADC_BATCHES: usize = 1 << ADC_BATCHES_LOG2;
@@ -76,18 +77,11 @@ pub const ADC_BATCHES: usize = 1 << ADC_BATCHES_LOG2;
 const IIR_CASCADE_LENGTH: usize = 1;
 
 // TODO should these be global consts?
-
 // Frequency scaling factor for lock-in harmonic demodulation.
 const HARMONIC: u32 = 1;
-
 // Phase offset applied to the lock-in demodulation signal.
 const PHASE_OFFSET: u32 = 0;
 
-// The PLL locks to an external reference signal. See `PLL::update()`
-// for a description of shift_frequency and shift_phase.
-const PLL_SHIFT_FREQUENCY: u8 = 4;
-const PLL_SHIFT_PHASE: u8 = 3;
-
 #[link_section = ".sram3.eth"]
 static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new();
 
@@ -169,6 +163,79 @@ type AFE1 = afe::ProgrammableGainAmplifier<
     hal::gpio::gpiod::PD15<hal::gpio::Output<hal::gpio::PushPull>>,
 >;
 
+/// Locks a PLL to an external reference and computes the initial phase value and frequency of the
+/// demodulation signal.
+pub struct TimestampHandler {
+    pll: PLL,
+    pll_shift_frequency: u8,
+    pll_shift_phase: u8,
+    // 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,
+}
+
+impl TimestampHandler {
+    /// Construct a new `TimestampHandler` instance.
+    ///
+    /// # Args
+    /// * `pll_shift_frequency` - See `PLL::update()`.
+    /// * `pll_shift_phase` - See `PLL::update()`.
+    ///
+    /// # Returns
+    /// New `TimestampHandler` instance.
+    pub fn new(pll_shift_frequency: u8, pll_shift_phase: u8) -> Self {
+        TimestampHandler {
+            pll: PLL::default(),
+            pll_shift_frequency,
+            pll_shift_phase,
+            batch_index: 0,
+            reference_phase: 0,
+            reference_frequency: 0,
+        }
+    }
+
+    /// Compute the initial phase value and frequency of the demodulation signal.
+    ///
+    /// # Args
+    /// * `timestamp` - Counter value corresponding to an external reference edge.
+    ///
+    /// # Returns
+    /// Tuple consisting of the initial phase value and frequency of the demodulation signal.
+    pub fn update(&mut self, timestamp: Option<u32>) -> (u32, u32) {
+        if let Some(t) = timestamp {
+            let (phase, frequency) = self.pll.update(
+                t as i32,
+                self.pll_shift_frequency,
+                self.pll_shift_phase,
+            );
+            self.reference_phase = phase as u32 as i64;
+            self.reference_frequency = frequency as u32 as i64;
+        }
+
+        let demodulation_frequency = divide_round(
+            1 << (64 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_BATCHES_LOG2),
+            self.reference_frequency,
+        ) as u32;
+        let demodulation_initial_phase = divide_round(
+            (((self.batch_index as i64) << (32 - ADC_BATCHES_LOG2))
+                - self.reference_phase)
+                << 32,
+            self.reference_frequency,
+        ) as u32;
+
+        if self.batch_index < ADC_BATCHES as u32 - 1 {
+            self.batch_index += 1;
+        } else {
+            self.batch_index = 0;
+            self.reference_phase -= 1 << 32;
+        }
+
+        (demodulation_initial_phase, demodulation_frequency)
+    }
+}
+
 macro_rules! route_request {
     ($request:ident,
             readable_attributes: [$($read_attribute:tt: $getter:tt),*],
@@ -229,10 +296,7 @@ const APP: () = {
         adcs: (Adc0Input, Adc1Input),
         dacs: (Dac0Output, Dac1Output),
         input_stamper: digital_input_stamper::InputStamper,
-        pll: PLL,
+        timestamp_handler: TimestampHandler,
-        batch_index: u32,
-        reference_phase: i64,
-        reference_frequency: i64,
         iir_lockin: iir_int::IIR,
         iir_state_lockin: [iir_int::IIRState; 2],
 
@@ -957,10 +1021,7 @@ const APP: () = {
         #[cfg(not(feature = "pounder_v1_1"))]
         let pounder_stamper = None;
 
-        let pll = PLL::default();
+        let timestamp_handler = TimestampHandler::new(4, 3);
-        let batch_index = 0;
-        let reference_phase = 0;
-        let reference_frequency = 0;
         let iir_lockin = iir_int::IIR { ba: [0; 5] };
         let iir_state_lockin = [[0; 5]; 2];
 
@@ -979,10 +1040,7 @@ const APP: () = {
             pounder: pounder_devices,
             pounder_stamper,
 
-            pll,
+            timestamp_handler,
-            batch_index,
-            reference_phase,
-            reference_frequency,
             iir_lockin,
             iir_state_lockin,
 
@@ -993,7 +1051,7 @@ const APP: () = {
         }
     }
 
-    #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper, pll, iir_lockin, iir_state_lockin, batch_index, reference_phase, reference_frequency], priority=2)]
+    #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper, timestamp_handler, iir_lockin, iir_state_lockin], priority=2)]
     fn process(c: process::Context) {
         if let Some(stamper) = c.resources.pounder_stamper {
             let pounder_timestamps = stamper.acquire_buffer();
@@ -1009,37 +1067,10 @@ const APP: () = {
             c.resources.dacs.1.acquire_buffer(),
         ];
 
-        let reference_phase = c.resources.reference_phase;
+        let (demodulation_initial_phase, demodulation_frequency) = c
-        let reference_frequency = c.resources.reference_frequency;
+            .resources
-
+            .timestamp_handler
-        if let Some(t) = c.resources.input_stamper.latest_timestamp() {
+            .update(c.resources.input_stamper.latest_timestamp());
-            let res = c.resources.pll.update(
-                t as i32,
-                PLL_SHIFT_FREQUENCY,
-                PLL_SHIFT_PHASE,
-            );
-            *reference_phase = res.0 as u32 as i64;
-            *reference_frequency = res.1 as u32 as i64;
-        }
-
-        let demodulation_frequency = divide_round(
-            1 << (64 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_BATCHES_LOG2),
-            *reference_frequency,
-        ) as u32;
-        let batch_index = c.resources.batch_index;
-        let demodulation_initial_phase = divide_round(
-            (((*batch_index as i64) << (32 - ADC_BATCHES_LOG2))
-                - *reference_phase)
-                << 32,
-            *reference_frequency,
-        ) as u32;
-
-        if *batch_index < ADC_BATCHES as u32 - 1 {
-            *batch_index += 1;
-        } else {
-            *batch_index = 0;
-            *reference_phase -= 1 << 32;
-        }
 
         let [dac0, dac1] = dac_samples;
         let iir_lockin = c.resources.iir_lockin;
@@ -1064,10 +1095,10 @@ const APP: () = {
                 signal.0 =
                     iir_lockin.update(&mut iir_state_lockin[0], signal.0);
                 signal.1 =
-                    iir_lockin.update(&mut iir_state_lockin[1], signal.0);
+                    iir_lockin.update(&mut iir_state_lockin[1], signal.1);
 
                 let magnitude = signal.0 * signal.0 + signal.1 * signal.1;
-                let phase = atan2(signal.0, signal.0);
+                let phase = atan2(signal.1, signal.0);
 
                 *d0 = (magnitude >> 16) as i16 as u16;
                 *d1 = (phase >> 16) as i16 as u16;