diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 6e68bb4..4910025 100644 --- a/dsp/src/lockin.rs +++ b/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( - pll_shift_frequency, - pll_shift_phase, - adc_sample_ticks_log2, - sample_buffer_size_log2, - ); + let mut timestamp_handler = + RPLL::new(adc_sample_ticks_log2 + sample_buffer_size_log2); let mut timestamp_start: u64 = 0; let time_constant: f64 = 1. / (2. * PI * corner_frequency); diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index d06abbb..36a9646 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -1,100 +1,39 @@ -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, +#[derive(Copy, Clone, Default)] +pub struct RPLL { + dt2: u8, + t: i32, + f2: i64, + y1: i32, + xj1: i32, + f1: i32, } -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, - } +impl RPLL { + pub fn new(dt2: u8) -> RPLL { + let mut pll = RPLL::default(); + pll.dt2 = dt2; + pll } - /// 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) { - 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; + pub fn update( + &mut self, + input: Option, + shift_frequency: u8, + shift_phase: u8, + ) -> (i32, i32) { + self.y1 += self.f1; + if let Some(xj) = input { + self.f2 += (1i64 << 32 + self.dt2 - shift_frequency) + - (self.f2 * (xj - self.xj1) as i64 + + (1i64 << shift_frequency - 1) + >> shift_frequency); + self.f1 = self.f2 as i32 + + (self.f2 * (self.t - xj) as i64 + - ((self.y1 as i64) << self.dt2) + + (1i64 << shift_phase - 1) + >> shift_phase) as i32; } - - let demodulation_frequency: u32; - 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) + self.t += 1 << self.dt2; + (self.y1, self.f1) } } diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 5460461..5845e0a 100644 --- a/src/bin/lockin.rs +++ b/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( - 4, // relative PLL frequency bandwidth: 2**-4, TODO: expose - 3, // relative PLL phase bandwidth: 2**-3, TODO: expose - ADC_SAMPLE_TICKS_LOG2 as usize, - SAMPLE_BUFFER_SIZE_LOG2, - ); + let pll = + RPLL::new(ADC_SAMPLE_TICKS_LOG2 + 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 - .resources - .pll - .update(c.resources.timestamper.latest_timestamp()); + let (pll_phase, pll_frequency) = c.resources.pll.update( + c.resources.timestamper.latest_timestamp().map(|t| t as i32), + 21, // relative PLL frequency bandwidth: 2**-21, TODO: expose + 21, // relative PLL phase bandwidth: 2**-21, TODO: expose + ); // Harmonic index of the LO: -1 to _de_modulate the fundamental let harmonic: i32 = -1; diff --git a/src/lib.rs b/src/lib.rs index 254e626..218e3cf 100644 --- a/src/lib.rs +++ b/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;