dsp: add reciprocal_pll

dsp/src/lib.rs

@@ -117,6 +117,7 @@ where
 pub mod iir;
 pub mod iir_int;
 pub mod pll;
+pub mod reciprocal_pll;
 pub mod trig;
 pub mod unwrap;
 

dsp/src/reciprocal_pll.rs

@@ -0,0 +1,92 @@
+use super::{divide_round, pll::PLL};
+
+/// Processes external timestamps to produce the frequency and initial phase 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,
+    adc_sample_ticks_log2: usize,
+    sample_buffer_size_log2: usize,
+}
+
+impl TimestampHandler {
+    /// Construct a new `TimestampHandler` instance.
+    ///
+    /// # Args
+    /// * `pll_shift_frequency` - See `PLL::update()`.
+    /// * `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.
+    ///
+    /// # 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 << (32 + self.adc_sample_ticks_log2),
+            self.reference_frequency,
+        ) as u32;
+        let 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)) as u32
+                - 1
+        {
+            self.batch_index += 1;
+        } else {
+            self.batch_index = 0;
+            self.reference_phase -= 1 << 32;
+        }
+
+        (demodulation_initial_phase, demodulation_frequency)
+    }
+}

src/main.rs

@@ -161,79 +161,6 @@ type AFE1 = afe::ProgrammableGainAmplifier<
     hal::gpio::gpiod::PD15<hal::gpio::Output<hal::gpio::PushPull>>,
 >;
 
-/// Processes external timestamps to produce the frequency and initial phase 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 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),*],