From 4c033c0f3e3068fbfe03fd0e5f794cc02ea61c4e Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 13:04:40 -0800 Subject: [PATCH] move timestamp handling into new TimestampHandler struct --- src/main.rs | 137 ++++++++++++++++++++++++++++++++-------------------- 1 file changed, 84 insertions(+), 53 deletions(-) diff --git a/src/main.rs b/src/main.rs index 538fb2a..1bc6a11 100644 --- a/src/main.rs +++ b/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>, >; +/// 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) { + 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, - batch_index: u32, - reference_phase: i64, - reference_frequency: i64, + timestamp_handler: TimestampHandler, 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 batch_index = 0; - let reference_phase = 0; - let reference_frequency = 0; + let timestamp_handler = TimestampHandler::new(4, 3); 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, - batch_index, - reference_phase, - reference_frequency, + timestamp_handler, 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 reference_frequency = c.resources.reference_frequency; - - if let Some(t) = 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 (demodulation_initial_phase, demodulation_frequency) = c + .resources + .timestamp_handler + .update(c.resources.input_stamper.latest_timestamp()); 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;