From db3a42a7b925e8f2caf5b5e60f6fa86bf3a328ad Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Tue, 12 Jan 2021 06:54:16 -0800 Subject: [PATCH 01/16] Update src/adc.rs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Robert Jördens --- src/adc.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/adc.rs b/src/adc.rs index b4d4359..6c1bf34 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -1,6 +1,6 @@ ///! Stabilizer ADC management interface ///! -///! The Stabilizer ADCs utilize three DMA channels: one to trigger sampling, one to collect +///! The Stabilizer ADCs utilize three DMA channels each: one to trigger sampling, one to collect ///! samples, and one to clear the EOT flag betwen samples. The SPI interfaces are configured ///! for receiver-only operation. A timer channel is ///! configured to generate a DMA write into the SPI CR1 register, which initiates a SPI transfer and From 028f4a1bb2cc97f586084f35bfcfb36116e80062 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 7 Jan 2021 09:49:23 -0800 Subject: [PATCH 02/16] fix small typos --- src/digital_input_stamper.rs | 4 ++-- src/main.rs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/digital_input_stamper.rs b/src/digital_input_stamper.rs index d63849c..648105f 100644 --- a/src/digital_input_stamper.rs +++ b/src/digital_input_stamper.rs @@ -26,13 +26,13 @@ ///! timestamping is desired in DI1, a separate timer + capture channel will be necessary. use super::{hal, timers, ADC_SAMPLE_TICKS, SAMPLE_BUFFER_SIZE}; -/// Calculate the period of the digital input timestampe timer. +/// Calculate the period of the digital input timestamp timer. /// /// # Note /// The period returned will be 1 less than the required period in timer ticks. The value returned /// can be immediately programmed into a hardware timer period register. /// -/// The period is calcualted to be some power-of-two multiple of the batch size, such that N batches +/// The period is calculated to be some power-of-two multiple of the batch size, such that N batches /// will occur between each timestamp timer overflow. /// /// # Returns diff --git a/src/main.rs b/src/main.rs index e2de7d5..c5a9698 100644 --- a/src/main.rs +++ b/src/main.rs @@ -353,7 +353,7 @@ const APP: () = { timer5.pause(); timer5.set_tick_freq(design_parameters::TIMER_FREQUENCY); - // The time stamp timer must run at exactly a multiple of the sample timer based on the + // The timestamp timer must run at exactly a multiple of the sample timer based on the // batch size. To accomodate this, we manually set the prescaler identical to the sample // timer, but use a period that is longer. let mut timer = timers::TimestampTimer::new(timer5); From bae295140ddd6fe2a97ee703128f8b6d1de91b48 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Fri, 8 Jan 2021 11:53:08 -0800 Subject: [PATCH 03/16] update lock-in for integer math and PLL --- dsp/src/lib.rs | 21 +- dsp/src/lockin.rs | 563 +++++++---------- dsp/tests/lockin_low_pass.rs | 1137 ---------------------------------- 3 files changed, 238 insertions(+), 1483 deletions(-) delete mode 100644 dsp/tests/lockin_low_pass.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index fb189fa..aeec742 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -5,7 +5,7 @@ use core::ops::{Add, Mul, Neg}; pub type Complex = (T, T); -/// Round up half. +/// Bit shift, round up half. /// /// # Arguments /// @@ -20,6 +20,25 @@ pub fn shift_round(x: i32, shift: usize) -> i32 { (x + (1 << (shift - 1))) >> shift } +/// Integer division, round up half. +/// +/// # Arguments +/// +/// `dividend` - Value to divide. +/// `divisor` - Value that divides the dividend. +/// +/// # Returns +/// +/// Divided and rounded value. +#[inline(always)] +pub fn divide_round(dividend: i64, divisor: i64) -> i64 { + debug_assert!( + dividend as i128 + (divisor as i128 - 1) < i64::MAX as i128 + && dividend as i128 + (divisor as i128 - 1) > i64::MIN as i128 + ); + (dividend + (divisor - 1)) / divisor +} + fn abs(x: T) -> T where T: PartialOrd + Default + Neg, diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index bb649ad..5b82313 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -52,47 +52,36 @@ //! the demodulation frequency. This does not require any state //! information and is therefore a normal function. -use super::iir::{IIRState, IIR}; -use super::Complex; -use core::f32::consts::PI; +use super::iir_int::{IIRState, IIR}; +use super::pll::PLL; +use super::trig::{atan2, cossin}; +use super::{divide_round, Complex}; + +/// TODO these constants are copied from main.rs and should be +/// shared. Additionally, we should probably store the log2 values and +/// compute the actual values from these in main, as is done here. +pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 0; +pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2; + +pub const ADC_SAMPLE_TICKS_LOG2: usize = 8; +pub const ADC_SAMPLE_TICKS: usize = 1 << ADC_SAMPLE_TICKS_LOG2; + +pub const ADC_BATCHES_LOG2: usize = + 32 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_SAMPLE_TICKS_LOG2; +pub const ADC_BATCHES: usize = 1 << ADC_BATCHES_LOG2; -/// The number of ADC samples in one batch. -pub const ADC_SAMPLE_BUFFER_SIZE: usize = 16; -/// The number of outputs sent to the DAC for each ADC batch. pub const DECIMATED_BUFFER_SIZE: usize = 1; -/// Treat the 2-element array as a FIFO. This allows new elements to -/// be pushed into the array, existing elements to shift back in the -/// array, and the last element to fall off the array. -trait Fifo2 { - fn push(&mut self, new_element: Option); -} - -impl Fifo2 for [Option; 2] { - /// Push a new element into the array. The existing elements move - /// backward in the array by one location, and the current last - /// element is discarded. - /// - /// # Arguments - /// - /// * `new_element` - New element pushed into the front of the - /// array. - fn push(&mut self, new_element: Option) { - // For array sizes greater than 2 it would be preferable to - // use a rotating index to avoid unnecessary data - // copying. However, this would somewhat complicate the use of - // iterators and for 2 elements, shifting is inexpensive. - self[1] = self[0]; - self[0] = new_element; - } -} - /// Performs lock-in amplifier processing of a signal. pub struct Lockin { - phase_offset: f32, - sample_period: u32, harmonic: u32, - timestamps: [Option; 2], + phase_offset: u32, + batch_index: u32, + last_phase: Option, + last_frequency: Option, + pll: PLL, + pll_shift_frequency: u8, + pll_shift_phase: u8, iir: IIR, iirstate: [IIRState; 2], } @@ -102,31 +91,36 @@ impl Lockin { /// /// # Arguments /// - /// * `phase_offset` - Phase offset (in radians) applied to the - /// demodulation signal. - /// * `sample_period` - ADC sampling period in terms of the - /// internal clock period. /// * `harmonic` - Integer scaling factor used to adjust the /// demodulation frequency. E.g., 2 would demodulate with the /// first harmonic. + /// * `phase_offset` - Phase offset applied to the demodulation + /// signal. /// * `iir` - IIR biquad filter. + /// * `pll_shift_frequency` - See PLL::update(). + /// * `pll_shift_phase` - See PLL::update(). /// /// # Returns /// /// New `Lockin` instance. pub fn new( - phase_offset: f32, - sample_period: u32, harmonic: u32, + phase_offset: u32, iir: IIR, + pll_shift_frequency: u8, + pll_shift_phase: u8, ) -> Self { Lockin { - phase_offset: phase_offset, - sample_period: sample_period, - harmonic: harmonic, - timestamps: [None, None], - iir: iir, - iirstate: [[0.; 5]; 2], + harmonic, + phase_offset, + batch_index: 0, + last_phase: None, + last_frequency: None, + pll: PLL::default(), + pll_shift_frequency, + pll_shift_phase, + iir, + iirstate: [[0; 5]; 2], } } @@ -135,120 +129,88 @@ impl Lockin { /// # Arguments /// /// * `adc_samples` - One batch of ADC samples. - /// * `timestamps` - Counter values corresponding to the edges of - /// an external reference signal. The counter is incremented by a - /// fast internal clock. + /// * `timestamp` - Counter value corresponding to the edges of an + /// external reference signal. The counter is incremented by a + /// fast internal clock. Each ADC sample batch can contain 0 or 1 + /// timestamps. /// /// # Returns /// /// The demodulated complex signal as a `Result`. When there are /// an insufficient number of timestamps to perform processing, /// `Err` is returned. - /// - /// # Assumptions - /// - /// `demodulate` expects that the timestamp counter value is equal - /// to 0 when the ADC samples its first input in a batch. This can - /// be achieved by configuring the timestamp counter to overflow - /// at the end of the ADC batch sampling period. pub fn demodulate( &mut self, adc_samples: &[i16], - timestamps: &[u16], - ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { - let sample_period = self.sample_period as i32; - // update old timestamps for new ADC batch - self.timestamps.iter_mut().for_each(|t| match *t { - Some(timestamp) => { - // Existing timestamps have aged by one ADC batch - // period since the last ADC batch. - *t = Some( - timestamp - ADC_SAMPLE_BUFFER_SIZE as i32 * sample_period, - ); - } - None => (), - }); + timestamp: Option, + ) -> Result<[Complex; SAMPLE_BUFFER_SIZE], &str> { + let frequency: i64; + let phase: i64; - // return prematurely if there aren't enough timestamps for - // processing - let old_timestamp_count = - self.timestamps.iter().filter(|t| t.is_some()).count(); - if old_timestamp_count + timestamps.len() < 2 { - return Err("insufficient timestamps"); + match timestamp { + Some(t) => { + let res = self.pll.update( + t as i32, + self.pll_shift_frequency, + self.pll_shift_phase, + ); + phase = res.0 as u32 as i64; + frequency = res.1 as u32 as i64; + self.last_phase = Some(phase); + self.last_frequency = Some(frequency); + } + None => match self.last_phase { + Some(t) => { + phase = t; + frequency = self.last_frequency.unwrap(); + } + None => { + self.batch_index += 1; + if self.batch_index == ADC_BATCHES as u32 { + self.batch_index = 0; + } + return Err("insufficient timestamps"); + } + }, } - let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; - // if we have not yet recorded any timestamps, the first - // reference period must be computed from the first and - // second timestamps in the array - let mut timestamp_index: usize = - if old_timestamp_count == 0 { 1 } else { 0 }; + let demodulation_frequency = divide_round( + 1 << (64 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_BATCHES_LOG2), + frequency, + ) as u32; + let demodulation_initial_phase = divide_round( + (((self.batch_index as i64) << (32 - ADC_BATCHES_LOG2)) - phase) + << 32, + frequency, + ) as u32; - // compute ADC sample phases, sines/cosines and demodulate - signal + let mut demodulation_signal = [(0_i32, 0_i32); SAMPLE_BUFFER_SIZE]; + + demodulation_signal .iter_mut() .zip(adc_samples.iter()) .enumerate() .for_each(|(i, (s, sample))| { - let adc_sample_count = i as i32 * sample_period; - // index of the closest timestamp that occurred after - // the current ADC sample - let closest_timestamp_after_index: i32 = if timestamps.len() > 0 - { - // Linear search is fast because both the timestamps - // and ADC sample counts are sorted. Because of this, - // we only need to check timestamps that were also - // greater than the last ADC sample count. - while timestamp_index < timestamps.len() - 1 - && (timestamps[timestamp_index] as i32) - < adc_sample_count - { - timestamp_index += 1; - } - timestamp_index as i32 - } else { - -1 - }; - - // closest timestamp that occurred before the current - // ADC sample - let closest_timestamp_before: i32; - let reference_period = if closest_timestamp_after_index < 0 { - closest_timestamp_before = self.timestamps[0].unwrap(); - closest_timestamp_before - self.timestamps[1].unwrap() - } else if closest_timestamp_after_index == 0 { - closest_timestamp_before = self.timestamps[0].unwrap(); - timestamps[0] as i32 - closest_timestamp_before - } else { - closest_timestamp_before = timestamps - [(closest_timestamp_after_index - 1) as usize] - as i32; - timestamps[closest_timestamp_after_index as usize] as i32 - - closest_timestamp_before - }; - - let integer_phase: i32 = (adc_sample_count - - closest_timestamp_before) - * self.harmonic as i32; - let phase = self.phase_offset - + 2. * PI * integer_phase as f32 / reference_period as f32; - let (sine, cosine) = libm::sincosf(phase); - let sample = *sample as f32; - s.0 = sine * sample; - s.1 = cosine * sample; + let sample_phase = (self.harmonic.wrapping_mul( + (demodulation_frequency.wrapping_mul(i as u32)) + .wrapping_add(demodulation_initial_phase), + )) + .wrapping_add(self.phase_offset); + let (cos, sin) = cossin(sample_phase as i32); + // cos/sin take up 32 bits and sample takes up 16 + // bits. Make this fit into a 32 bit result. + s.0 = ((*sample as i64 * cos as i64) >> 16) as i32; + s.1 = ((*sample as i64 * sin as i64) >> 16) as i32; }); - // record new timestamps - let start_index: usize = if timestamps.len() < 2 { - 0 + if self.batch_index < ADC_BATCHES as u32 - 1 { + self.batch_index += 1; } else { - timestamps.len() - 2 - }; - timestamps[start_index..] - .iter() - .for_each(|t| self.timestamps.push(Some(*t as i32))); + self.batch_index = 0; + self.last_phase = Some(self.last_phase.unwrap() - (1 << 32)); + } - Ok(signal) + Ok(demodulation_signal) } /// Filter the complex signal using the supplied biquad IIR. The @@ -257,7 +219,7 @@ impl Lockin { /// # Arguments /// /// * `signal` - Complex signal to filter. - pub fn filter(&mut self, signal: &mut [Complex]) { + pub fn filter(&mut self, signal: &mut [Complex]) { signal.iter_mut().for_each(|s| { s.0 = self.iir.update(&mut self.iirstate[0], s.0); s.1 = self.iir.update(&mut self.iirstate[1], s.1); @@ -266,8 +228,8 @@ impl Lockin { } /// Decimate the complex signal to `DECIMATED_BUFFER_SIZE`. The ratio -/// of `ADC_SAMPLE_BUFFER_SIZE` to `DECIMATED_BUFFER_SIZE` must be a -/// power of 2. +/// of `SAMPLE_BUFFER_SIZE` to `DECIMATED_BUFFER_SIZE` must be a power +/// of 2. /// /// # Arguments /// @@ -277,14 +239,12 @@ impl Lockin { /// /// The decimated signal. pub fn decimate( - signal: [Complex; ADC_SAMPLE_BUFFER_SIZE], -) -> [Complex; DECIMATED_BUFFER_SIZE] { - let n_k = ADC_SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE; - debug_assert!( - ADC_SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0 - ); + signal: [Complex; SAMPLE_BUFFER_SIZE], +) -> [Complex; DECIMATED_BUFFER_SIZE] { + let n_k = SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE; + debug_assert!(SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0); - let mut signal_decimated = [(0_f32, 0_f32); DECIMATED_BUFFER_SIZE]; + let mut signal_decimated = [(0_i32, 0_i32); DECIMATED_BUFFER_SIZE]; signal_decimated .iter_mut() @@ -302,11 +262,13 @@ pub fn decimate( /// /// # Arguments /// -/// * `signal` - Complex signal to decimate. -pub fn magnitude_phase(signal: &mut [Complex]) { +/// * `signal` - Complex signal for which the magnitude and phase +/// should be computed. TODO currently, we compute the square of the +/// magnitude. This should be changed to be the actual magnitude. +pub fn magnitude_phase(signal: &mut [Complex]) { signal.iter_mut().for_each(|s| { - let new_i = libm::sqrtf([s.0, s.1].iter().map(|i| i * i).sum()); - let new_q = libm::atan2f(s.1, s.0); + let new_i = [s.0, s.1].iter().map(|i| i * i).sum(); + let new_q = atan2(s.1, s.0); s.0 = new_i; s.1 = new_q; }); @@ -315,204 +277,115 @@ pub fn magnitude_phase(signal: &mut [Complex]) { #[cfg(test)] mod tests { use super::*; - use crate::testing::complex_allclose; #[test] - fn array_push() { - let mut arr: [Option; 2] = [None, None]; - arr.push(Some(1)); - assert_eq!(arr, [Some(1), None]); - arr.push(Some(2)); - assert_eq!(arr, [Some(2), Some(1)]); - arr.push(Some(10)); - assert_eq!(arr, [Some(10), Some(2)]); - } - - #[test] - fn magnitude_phase_length_1_quadrant_1() { - let mut signal: [Complex; 1] = [(1., 1.)]; - magnitude_phase(&mut signal); - assert!(complex_allclose( - &signal, - &[(2_f32.sqrt(), PI / 4.)], - f32::EPSILON, - 0. - )); - - signal = [(3_f32.sqrt() / 2., 1. / 2.)]; - magnitude_phase(&mut signal); - assert!(complex_allclose( - &signal, - &[(1., PI / 6.)], - f32::EPSILON, - 0. - )); - } - - #[test] - fn magnitude_phase_length_1_quadrant_2() { - let mut signal = [(-1., 1.)]; - magnitude_phase(&mut signal); - assert!(complex_allclose( - &signal, - &[(2_f32.sqrt(), 3. * PI / 4.)], - f32::EPSILON, - 0. - )); - - signal = [(-1. / 2., 3_f32.sqrt() / 2.)]; - magnitude_phase(&mut signal); - assert!(complex_allclose( - &signal, - &[(1_f32, 2. * PI / 3.)], - f32::EPSILON, - 0. - )); - } - - #[test] - fn magnitude_phase_length_1_quadrant_3() { - let mut signal = [(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; - magnitude_phase(&mut signal); - assert!(complex_allclose( - &signal, - &[(1_f32.sqrt(), -3. * PI / 4.)], - f32::EPSILON, - 0. - )); - - signal = [(-1. / 2., -2_f32.sqrt())]; - magnitude_phase(&mut signal); - assert!(complex_allclose( - &signal, - &[((3. / 2.) as f32, -1.91063323625 as f32)], - f32::EPSILON, - 0. - )); - } - - #[test] - fn magnitude_phase_length_1_quadrant_4() { - let mut signal = [(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; - magnitude_phase(&mut signal); - assert!(complex_allclose( - &signal, - &[(1_f32.sqrt(), -1. * PI / 4.)], - f32::EPSILON, - 0. - )); - - signal = [(3_f32.sqrt() / 2., -1. / 2.)]; - magnitude_phase(&mut signal); - assert!(complex_allclose( - &signal, - &[(1_f32, -PI / 6.)], - f32::EPSILON, - 0. - )); - } - - #[test] - fn decimate_sample_16_decimated_1() { - let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [ - (0.0, 1.6), - (0.1, 1.7), - (0.2, 1.8), - (0.3, 1.9), - (0.4, 2.0), - (0.5, 2.1), - (0.6, 2.2), - (0.7, 2.3), - (0.8, 2.4), - (0.9, 2.5), - (1.0, 2.6), - (1.1, 2.7), - (1.2, 2.8), - (1.3, 2.9), - (1.4, 3.0), - (1.5, 3.1), - ]; - assert_eq!(decimate(signal), [(0.0, 1.6)]); - } - - #[test] - fn lockin_demodulate_valid_0() { + /// Ensure that the demodulation signals are within some tolerance + /// band of the target value given the phase and frequency values + /// provided by the PLL. + fn demodulate() { + const PLL_SHIFT_FREQUENCY: u8 = 4; + const PLL_SHIFT_PHASE: u8 = 3; + const HARMONIC: u32 = 1; + const PHASE_OFFSET: u32 = 0; let mut lockin = Lockin::new( - 0., - 200, - 1, - IIR { - ba: [0_f32; 5], - y_offset: 0., - y_min: -(1 << 15) as f32, - y_max: (1 << 15) as f32 - 1., - }, + HARMONIC, + PHASE_OFFSET, + IIR { ba: [0; 5] }, + PLL_SHIFT_FREQUENCY, + PLL_SHIFT_PHASE, ); - assert_eq!( - lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[]), - Err("insufficient timestamps") - ); - } - #[test] - fn lockin_demodulate_valid_1() { - let mut lockin = Lockin::new( - 0., - 200, - 1, - IIR { - ba: [0_f32; 5], - y_offset: 0., - y_min: -(1 << 15) as f32, - y_max: (1 << 15) as f32 - 1., - }, - ); - assert_eq!( - lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[0],), - Err("insufficient timestamps") - ); - } + // Duplicate the PLL outside demodulate so that we don't test + // its behavior. + let mut tracking_pll = PLL::default(); + let mut tracking_phase: i32 = 0; + let mut tracking_frequency: i32 = 0; - #[test] - fn lockin_demodulate_valid_2() { - let adc_period: u32 = 200; - let mut lockin = Lockin::new( - 0., - adc_period, - 1, - IIR { - ba: [0_f32; 5], - y_offset: 0., - y_min: -(1 << 15) as f32, - y_max: (1 << 15) as f32 - 1., - }, + const REFERENCE_FREQUENCY: usize = 10_000; + let mut reference_edge: usize = REFERENCE_FREQUENCY; + + // Ensure that we receive at most 1 timestamp per batch. + debug_assert!( + REFERENCE_FREQUENCY >= SAMPLE_BUFFER_SIZE * ADC_SAMPLE_TICKS ); - let adc_samples: [i16; ADC_SAMPLE_BUFFER_SIZE] = - [-8, 7, -7, 6, -6, 5, -5, 4, -4, 3, -3, 2, -2, -1, 1, 0]; - let reference_period: u16 = 2800; - let initial_phase_integer: u16 = 200; - let timestamps: &[u16] = &[ - initial_phase_integer, - initial_phase_integer + reference_period, - ]; - let initial_phase: f32 = - -(initial_phase_integer as f32) / reference_period as f32 * 2. * PI; - let phase_increment: f32 = - adc_period as f32 / reference_period as f32 * 2. * PI; - let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; - for (n, s) in signal.iter_mut().enumerate() { - let adc_phase = initial_phase + n as f32 * phase_increment; - let sine = adc_phase.sin(); - let cosine = adc_phase.cos(); - s.0 = sine * adc_samples[n] as f32; - s.1 = cosine * adc_samples[n] as f32; + + for batch in 0..100_000 { + let tick: usize = batch * ADC_SAMPLE_TICKS * SAMPLE_BUFFER_SIZE; + let timestamp: Option; + + // When the reference edge occurred during the current + // batch acquisition, register the timestamp and update + // the tracking PLL. + if reference_edge >= tick + && reference_edge < tick + ADC_SAMPLE_TICKS * SAMPLE_BUFFER_SIZE + { + timestamp = Some(reference_edge as u32); + + let tracking_update = tracking_pll.update( + reference_edge as i32, + PLL_SHIFT_FREQUENCY, + PLL_SHIFT_PHASE, + ); + tracking_phase = tracking_update.0; + tracking_frequency = tracking_update.1; + + reference_edge += REFERENCE_FREQUENCY; + } else { + timestamp = None; + } + + let timestamp_before_batch = if tracking_phase > tick as i32 { + // There can be at most 1 reference edge per batch, so + // this will necessarily place the timestamp prior to + // the current batch. + tracking_phase - tracking_frequency + } else { + tracking_phase + }; + + let initial_phase = (((tick as f64 + - timestamp_before_batch as f64) + / tracking_frequency as f64 + * (1_i64 << 32) as f64) + .round() + % u32::MAX as f64) as u32; + let frequency = ((ADC_SAMPLE_TICKS as f64 + / tracking_frequency as f64 + * (1_i64 << 32) as f64) + .round() + % u32::MAX as f64) as u32; + + match lockin.demodulate(&[i16::MAX; SAMPLE_BUFFER_SIZE], timestamp) + { + Ok(v) => { + println!("batch : {}", batch); + for sample in 0..SAMPLE_BUFFER_SIZE { + const TOL: i32 = 50_000; + let cos = v[sample].0; + let sin = v[sample].1; + + let (mut target_cos, mut target_sin) = cossin( + HARMONIC + .wrapping_mul( + (frequency.wrapping_mul(sample as u32)) + .wrapping_add(initial_phase), + ) + .wrapping_add(PHASE_OFFSET) + as i32, + ); + target_cos /= 2; + target_sin /= 2; + + println!("sample : {}", sample); + println!("tol : {}", TOL); + println!("cos, target: {}, {}", cos, target_cos); + println!("sin, target: {}, {}", sin, target_sin); + assert!((cos - target_cos).abs() < TOL); + assert!((sin - target_sin).abs() < TOL); + } + } + Err(_) => {} + } } - let result = lockin.demodulate(&adc_samples, timestamps).unwrap(); - assert!( - complex_allclose(&result, &signal, 0., 1e-5), - "\nsignal computed: {:?},\nsignal expected: {:?}", - result, - signal - ); } } diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs deleted file mode 100644 index 37ab2b0..0000000 --- a/dsp/tests/lockin_low_pass.rs +++ /dev/null @@ -1,1137 +0,0 @@ -use dsp::iir::IIR; -use dsp::lockin::{ - decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, - DECIMATED_BUFFER_SIZE, -}; -use dsp::Complex; - -use std::f64::consts::PI; -use std::vec::Vec; - -const ADC_MAX: f64 = 1.; -const ADC_MAX_COUNT: f64 = (1 << 15) as f64; - -/// Single-frequency sinusoid. -#[derive(Copy, Clone)] -struct PureSine { - // Frequency (in Hz). - frequency: f64, - // Amplitude in dBFS (decibels relative to full-scale). A 16-bit - // ADC has a minimum dBFS for each sample of -90. - amplitude_dbfs: f64, - // Phase offset (in radians). - phase_offset: f64, -} - -/// Convert a dBFS voltage ratio to a linear ratio. -/// -/// # Arguments -/// -/// * `dbfs` - dB ratio relative to full scale. -/// -/// # Returns -/// -/// Linear value. -fn linear(dbfs: f64) -> f64 { - let base = 10_f64; - ADC_MAX * base.powf(dbfs / 20.) -} - -/// Convert a linear voltage ratio to a dBFS ratio. -/// -/// # Arguments -/// -/// * `linear` - Linear voltage ratio. -/// -/// # Returns -/// -/// dBFS value. -fn dbfs(linear: f64) -> f64 { - 20. * (linear / ADC_MAX).log10() -} - -/// Convert a real ADC input value in the range `-ADC_MAX` to -/// `+ADC_MAX` to an equivalent 16-bit ADC sampled value. This models -/// the ideal ADC transfer function. -/// -/// # Arguments -/// -/// * `x` - Real ADC input value. -/// -/// # Returns -/// -/// Sampled ADC value. -fn real_to_adc_sample(x: f64) -> i16 { - let max: i32 = i16::MAX as i32; - let min: i32 = i16::MIN as i32; - - let xi: i32 = (x / ADC_MAX * ADC_MAX_COUNT) as i32; - - // It's difficult to characterize the correct output result when - // the inputs are clipped, so panic instead. - if xi > max { - panic!("Input clipped to maximum, result is unlikely to be correct."); - } else if xi < min { - panic!("Input clipped to minimum, result is unlikely to be correct."); - } - - xi as i16 -} - -/// Generate `ADC_SAMPLE_BUFFER_SIZE` values of an ADC-sampled signal -/// starting at `timestamp_start`. -/// -/// # Arguments -/// -/// * `pure_signals` - Pure sinusoidal components of the ADC-sampled -/// signal. -/// * `timestamp_start` - Starting time of ADC-sampled signal in terms -/// of the internal clock count. -/// * `internal_frequency` - Internal clock frequency (in Hz). -/// * `adc_frequency` - ADC sampling frequency (in Hz). -/// -/// # Returns -/// -/// The sampled signal at the ADC input. -fn adc_sampled_signal( - pure_signals: &Vec, - timestamp_start: u64, - internal_frequency: f64, - adc_frequency: f64, -) -> [i16; ADC_SAMPLE_BUFFER_SIZE] { - // amplitude of each pure signal - let mut amplitude: Vec = Vec::::new(); - // initial phase value for each pure signal - let mut initial_phase: Vec = Vec::::new(); - // phase increment at each ADC sample for each pure signal - let mut phase_increment: Vec = Vec::::new(); - let adc_period = internal_frequency / adc_frequency; - - // For each pure sinusoid, compute the amplitude, phase - // corresponding to the first ADC sample, and phase increment for - // each subsequent ADC sample. - for pure_signal in pure_signals.iter() { - let signal_period = internal_frequency / pure_signal.frequency; - let phase_offset_count = - pure_signal.phase_offset / (2. * PI) * signal_period; - let initial_phase_count = - (phase_offset_count + timestamp_start as f64) % signal_period; - - amplitude.push(linear(pure_signal.amplitude_dbfs)); - initial_phase.push(2. * PI * initial_phase_count / signal_period); - phase_increment.push(2. * PI * adc_period / signal_period); - } - - // Compute the input signal corresponding to each ADC sample by - // summing the contributions from each pure sinusoid. - let mut signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = [0; ADC_SAMPLE_BUFFER_SIZE]; - signal.iter_mut().enumerate().for_each(|(n, s)| { - *s = real_to_adc_sample( - amplitude - .iter() - .zip(initial_phase.iter()) - .zip(phase_increment.iter()) - .fold(0., |acc, ((a, phi), theta)| { - acc + a * (phi + theta * n as f64).sin() - }), - ); - }); - - signal -} - -/// Reference clock timestamp values in one ADC batch period starting -/// at `timestamp_start`. Also returns the number of valid timestamps. -/// -/// # Arguments -/// -/// * `reference_frequency` - External reference signal frequency (in -/// Hz). -/// * `timestamp_start` - Start time in terms of the internal clock -/// count. This is the start time of the current processing sequence -/// (i.e., for the current `ADC_SAMPLE_BUFFER_SIZE` ADC samples). -/// * `timestamp_stop` - Stop time in terms of the internal clock -/// count. -/// * `internal_frequency` - Internal clock frequency (in Hz). -/// -/// # Returns -/// -/// Tuple consisting of the number of valid timestamps in the ADC -/// batch period, followed by an array of the timestamp values. -fn adc_batch_timestamps( - reference_frequency: f64, - timestamps: &mut [u16], - timestamp_start: u64, - timestamp_stop: u64, - internal_frequency: f64, -) -> &[u16] { - let reference_period = internal_frequency / reference_frequency; - let start_count = timestamp_start as f64 % reference_period; - let mut valid_timestamps: usize = 0; - - let mut timestamp = (reference_period - start_count) % reference_period; - while timestamp < (timestamp_stop - timestamp_start) as f64 { - timestamps[valid_timestamps] = timestamp as u16; - timestamp += reference_period; - valid_timestamps += 1; - } - - ×tamps[..valid_timestamps] -} - -/// Lowpass biquad filter using cutoff and sampling frequencies. -/// Taken from: -/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html -/// -/// # Arguments -/// -/// * `corner_frequency` - Corner frequency, or 3dB cutoff frequency -/// (in Hz). -/// * `sampling_frequency` - Sampling frequency (in Hz). -/// -/// # Returns -/// -/// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 -/// is set to -1. -fn lowpass_iir_coefficients( - corner_frequency: f64, - sampling_frequency: f64, -) -> [f32; 5] { - let normalized_angular_frequency: f64 = - 2. * PI * corner_frequency / sampling_frequency; - let quality_factor: f64 = 1. / 2f64.sqrt(); - let alpha: f64 = normalized_angular_frequency.sin() / (2. * quality_factor); - // All b coefficients have been multiplied by a factor of 2 in - // comparison with the link above in order to set the passband - // gain to 2. - let mut b0: f64 = 1. - normalized_angular_frequency.cos(); - let mut b1: f64 = 2. * (1. - normalized_angular_frequency.cos()); - let mut b2: f64 = b0; - let a0: f64 = 1. + alpha; - let mut a1: f64 = -2. * normalized_angular_frequency.cos(); - let mut a2: f64 = 1. - alpha; - b0 /= a0; - b1 /= a0; - b2 /= a0; - a1 /= -a0; - a2 /= -a0; - - [b0 as f32, b1 as f32, b2 as f32, a1 as f32, a2 as f32] -} - -/// Check that a measured value is within some tolerance of the actual -/// value. This allows setting both fixed and relative tolerances. -/// -/// # Arguments -/// -/// * `actual` - Actual value with respect to which the magnitude of -/// the relative tolerance is computed. -/// * `computed` - Computed value. This is compared with the actual -/// value, `actual`. -/// * `fixed_tolerance` - Fixed tolerance. -/// * `relative_tolerance` - Relative tolerance. -/// `relative_tolerance`*`actual` gives the total contribution of the -/// relative tolerance. -/// -/// # Returns -/// -/// `true` if the `actual` and `computed` values are within the -/// specified tolerance of one another, and `false` otherwise. -fn tolerance_check( - actual: f32, - computed: f32, - fixed_tolerance: f32, - relative_tolerance: f32, -) -> bool { - (actual - computed).abs() - < max_error(actual, fixed_tolerance, relative_tolerance) -} - -/// Maximum acceptable error from an actual value given fixed and -/// relative tolerances. -/// -/// # Arguments -/// -/// * `actual` - Actual value with respect to which the magnitude of the -/// relative tolerance is computed. -/// * `fixed_tolerance` - Fixed tolerance. -/// * `relative_tolerance` - Relative tolerance. -/// `relative_tolerance`*`actual` gives the total contribution of the -/// relative tolerance. -/// -/// # Returns -/// -/// Maximum acceptable error. -fn max_error( - actual: f32, - fixed_tolerance: f32, - relative_tolerance: f32, -) -> f32 { - relative_tolerance * actual.abs() + fixed_tolerance -} - -/// Total noise amplitude of the input signal after sampling by the -/// ADC. This computes an upper bound of the total noise amplitude, -/// rather than its actual value. -/// -/// # Arguments -/// -/// * `noise_inputs` - Noise sources at the ADC input. -/// * `demodulation_frequency` - Frequency of the demodulation signal -/// (in Hz). -/// * `corner_frequency` - Low-pass filter 3dB corner (cutoff) -/// frequency. -/// -/// # Returns -/// -/// Upper bound of the total amplitude of all noise sources. -fn sampled_noise_amplitude( - noise_inputs: &Vec, - demodulation_frequency: f64, - corner_frequency: f64, -) -> f64 { - // There is not a simple way to compute the amplitude of a - // superpostition of sinusoids with different frequencies and - // phases. Although we can compute the amplitude in special cases - // (e.g., two signals whose periods have a common multiple), these - // do not help us in the general case. However, we can say that - // the total amplitude will not be greater than the sum of the - // amplitudes of the individual noise sources. We treat this as an - // upper bound, and use it as an approximation of the actual - // amplitude. - - let mut noise: f64 = noise_inputs - .iter() - .map(|n| { - // Noise inputs create an oscillation at the output, where the - // oscillation magnitude is determined by the strength of the - // noise and its attenuation (attenuation is determined by its - // proximity to the demodulation frequency and filter - // rolloff). - let octaves = ((n.frequency - demodulation_frequency).abs() - / corner_frequency) - .log2(); - // 2nd-order filter. Approximately 12dB/octave rolloff. - let attenuation = -2. * 20. * 2_f64.log10() * octaves; - linear(n.amplitude_dbfs + attenuation) - }) - .sum(); - - // Add in 1/2 LSB for the maximum amplitude deviation resulting - // from quantization. - noise += 1. / ADC_MAX_COUNT / 2.; - - noise -} - -/// Compute the maximum effect of input noise on the lock-in magnitude -/// computation. -/// -/// The maximum effect of noise on the magnitude computation is given -/// by: -/// -/// | sqrt((I+n*sin(x))**2 + (Q+n*cos(x))**2) - sqrt(I**2 + Q**2) | -/// -/// * I is the in-phase component of the portion of the input signal -/// with the same frequency as the demodulation signal. -/// * Q is the quadrature component. -/// * n is the total noise amplitude (from all contributions, after -/// attenuation from filtering). -/// * x is the phase of the demodulation signal. -/// -/// We need to find the demodulation phase (x) that maximizes this -/// expression. We can ignore the absolute value operation by also -/// considering the expression minimum. The locations of the minimum -/// and maximum can be computed analytically by finding the value of x -/// when the derivative of this expression with respect to x is -/// 0. When we solve this equation, we find: -/// -/// x = atan(I/Q) -/// -/// It's worth noting that this solution is technically only valid -/// when cos(x)!=0 (i.e., x!=pi/2,-pi/2). However, this is not a -/// problem because we only get these values when Q=0. Rust correctly -/// computes atan(inf)=pi/2, which is precisely what we want because -/// x=pi/2 maximizes sin(x) and therefore also the noise effect. -/// -/// The other maximum or minimum is pi radians away from this -/// value. -/// -/// # Arguments -/// -/// * `total_noise_amplitude` - Combined amplitude of all noise -/// sources sampled by the ADC. -/// * `in_phase_actual` - Value of the in-phase component if no noise -/// were present at the ADC input. -/// * `quadrature_actual` - Value of the quadrature component if no -/// noise were present at the ADC input. -/// * `desired_input_amplitude` - Amplitude of the desired input -/// signal. That is, the input signal component with the same -/// frequency as the demodulation signal. -/// -/// # Returns -/// -/// Approximation of the maximum effect on the magnitude computation -/// due to noise sources at the ADC input. -fn magnitude_noise( - total_noise_amplitude: f64, - in_phase_actual: f64, - quadrature_actual: f64, - desired_input_amplitude: f64, -) -> f64 { - // See function documentation for explanation. - let noise = |in_phase_delta: f64, quadrature_delta: f64| -> f64 { - (((in_phase_actual + in_phase_delta).powf(2.) - + (quadrature_actual + quadrature_delta).powf(2.)) - .sqrt() - - desired_input_amplitude) - .abs() - }; - - let phase = (in_phase_actual / quadrature_actual).atan(); - let max_noise_1 = noise( - total_noise_amplitude * phase.sin(), - total_noise_amplitude * phase.cos(), - ); - let max_noise_2 = noise( - total_noise_amplitude * (phase + PI).sin(), - total_noise_amplitude * (phase + PI).cos(), - ); - - max_noise_1.max(max_noise_2) -} - -/// Compute the maximum phase deviation from the correct value due to -/// the input noise sources. -/// -/// The maximum effect of noise on the phase computation is given by: -/// -/// | atan2(Q+n*cos(x), I+n*sin(x)) - atan2(Q, I) | -/// -/// See `magnitude_noise` for an explanation of the terms in this -/// mathematical expression. -/// -/// This expression is harder to compute analytically than the -/// expression in `magnitude_noise`. We could compute it numerically, -/// but that's expensive. However, we can use heuristics to try to -/// guess the values of x that will maximize the noise -/// effect. Intuitively, the difference will be largest when the -/// Y-argument of the atan2 function (Q+n*cos(x)) is pushed in the -/// opposite direction of the noise effect on the X-argument (i.e., -/// cos(x) and sin(x) have different signs). We can use: -/// -/// * sin(x)=+-1 (+- denotes plus or minus), cos(x)=0, -/// * sin(x)=0, cos(x)=+-1, and -/// * the value of x that maximizes |sin(x)-cos(x)| (when -/// sin(x)=1/sqrt(2) and cos(x)=-1/sqrt(2), or when the signs are -/// flipped) -/// -/// The first choice addresses cases in which |I|>>|Q|, the second -/// choice addresses cases in which |Q|>>|I|, and the third choice -/// addresses cases in which |I|~|Q|. We can test all of these cases -/// as an approximation for the real maximum. -/// -/// # Arguments -/// -/// * `total_noise_amplitude` - Total amplitude of all input noise -/// sources. -/// * `in_phase_actual` - Value of the in-phase component if no noise -/// were present at the input. -/// * `quadrature_actual` - Value of the quadrature component if no -/// noise were present at the input. -/// -/// # Returns -/// -/// Approximation of the maximum effect on the phase computation due -/// to noise sources at the ADC input. -fn phase_noise( - total_noise_amplitude: f64, - in_phase_actual: f64, - quadrature_actual: f64, -) -> f64 { - // See function documentation for explanation. - let noise = |in_phase_delta: f64, quadrature_delta: f64| -> f64 { - ((quadrature_actual + quadrature_delta) - .atan2(in_phase_actual + in_phase_delta) - - quadrature_actual.atan2(in_phase_actual)) - .abs() - }; - - let mut max_noise: f64 = 0.; - for (in_phase_delta, quadrature_delta) in [ - ( - total_noise_amplitude / 2_f64.sqrt(), - total_noise_amplitude / -2_f64.sqrt(), - ), - ( - total_noise_amplitude / -2_f64.sqrt(), - total_noise_amplitude / 2_f64.sqrt(), - ), - (total_noise_amplitude, 0.), - (-total_noise_amplitude, 0.), - (0., total_noise_amplitude), - (0., -total_noise_amplitude), - ] - .iter() - { - max_noise = max_noise.max(noise(*in_phase_delta, *quadrature_delta)); - } - - max_noise -} - -/// Lowpass filter test for in-phase/quadrature and magnitude/phase -/// computations. -/// -/// This attempts to "intelligently" model acceptable tolerance ranges -/// for the measured in-phase, quadrature, magnitude and phase results -/// of lock-in processing for a typical low-pass filter -/// application. So, instead of testing whether the lock-in processing -/// extracts the true magnitude and phase (or in-phase and quadrature -/// components) of the input signal, it attempts to calculate what the -/// lock-in processing should compute given any set of input noise -/// sources. For example, if a noise source of sufficient strength -/// differs in frequency by 1kHz from the reference frequency and the -/// filter cutoff frequency is also 1kHz, testing if the lock-in -/// amplifier extracts the amplitude and phase of the input signal -/// whose frequency is equal to the demodulation frequency is doomed -/// to failure. Instead, this function tests whether the lock-in -/// correctly adheres to its actual transfer function, whether or not -/// it was given reasonable inputs. The logic for computing acceptable -/// tolerance ranges is performed in `sampled_noise_amplitude`, -/// `magnitude_noise`, and `phase_noise`. -/// -/// # Arguments -/// -/// * `internal_frequency` - Internal clock frequency (Hz). The -/// internal clock increments timestamp counter values used to -/// record the edges of the external reference. -/// * `adc_frequency` - ADC sampling frequency (in Hz). -/// * `reference_frequency` - External reference frequency (in Hz). -/// * `demodulation_phase_offset` - Phase offset applied to the -/// in-phase and quadrature demodulation signals. -/// * `harmonic` - Scaling factor for the demodulation -/// frequency. E.g., 2 would demodulate with the first harmonic of the -/// reference frequency. -/// * `corner_frequency` - Lowpass filter 3dB cutoff frequency. -/// * `desired_input` - `PureSine` giving the frequency, amplitude and -/// phase of the desired result. -/// * `noise_inputs` - Vector of `PureSine` for any noise inputs on top -/// of `desired_input`. -/// * `time_constant_factor` - Number of time constants after which -/// the output is considered valid. -/// * `tolerance` - Acceptable relative tolerance for the magnitude -/// and angle outputs. The outputs must remain within this tolerance -/// between `time_constant_factor` and `time_constant_factor+1` time -/// constants. -fn lowpass_test( - internal_frequency: f64, - adc_frequency: f64, - reference_frequency: f64, - demodulation_phase_offset: f64, - harmonic: u32, - corner_frequency: f64, - desired_input: PureSine, - noise_inputs: &mut Vec, - time_constant_factor: f64, - tolerance: f32, -) { - let mut lockin = Lockin::new( - demodulation_phase_offset as f32, - (internal_frequency / adc_frequency) as u32, - harmonic, - IIR { - ba: lowpass_iir_coefficients(corner_frequency, adc_frequency), - y_offset: 0., - y_min: -ADC_MAX_COUNT as f32, - y_max: (ADC_MAX_COUNT - 1.) as f32, - }, - ); - - let mut timestamp_start: u64 = 0; - let time_constant: f64 = 1. / (2. * PI * corner_frequency); - let samples = - (time_constant_factor * time_constant * adc_frequency) as usize; - // Ensure the result remains within tolerance for 1 time constant - // after `time_constant_factor` time constants. - let extra_samples = (time_constant * adc_frequency) as usize; - let sample_count: u64 = (internal_frequency / adc_frequency) as u64 - * ADC_SAMPLE_BUFFER_SIZE as u64; - - let effective_phase_offset = - desired_input.phase_offset - demodulation_phase_offset; - let in_phase_actual = - linear(desired_input.amplitude_dbfs) * effective_phase_offset.cos(); - let quadrature_actual = - linear(desired_input.amplitude_dbfs) * effective_phase_offset.sin(); - - let total_noise_amplitude = sampled_noise_amplitude( - noise_inputs, - reference_frequency * harmonic as f64, - corner_frequency, - ); - let total_magnitude_noise = magnitude_noise( - total_noise_amplitude, - in_phase_actual, - quadrature_actual, - linear(desired_input.amplitude_dbfs), - ); - let total_phase_noise = - phase_noise(total_noise_amplitude, in_phase_actual, quadrature_actual); - - let pure_signals = noise_inputs; - pure_signals.push(desired_input); - - for n in 0..(samples + extra_samples) { - let adc_signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( - &pure_signals, - timestamp_start, - internal_frequency, - adc_frequency, - ); - let mut timestamps_array = [0_u16; ADC_SAMPLE_BUFFER_SIZE / 2]; - let timestamps = adc_batch_timestamps( - reference_frequency, - &mut timestamps_array, - timestamp_start, - timestamp_start + sample_count - 1, - internal_frequency, - ); - - let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE]; - match lockin.demodulate(&adc_signal, timestamps) { - Ok(s) => { - signal = s; - } - Err(_) => { - continue; - } - } - - lockin.filter(&mut signal); - let signal_decimated = decimate(signal); - - let mut magnitude_phase_decimated = signal.clone(); - // let mut magnitude_decimated = in_phase_decimated.clone(); - // let mut phase_decimated = quadrature_decimated.clone(); - - magnitude_phase(&mut magnitude_phase_decimated); - - // Ensure stable within tolerance for 1 time constant after - // `time_constant_factor`. - if n >= samples { - for k in 0..DECIMATED_BUFFER_SIZE { - let amplitude_normalized: f32 = - magnitude_phase_decimated[k].0 / ADC_MAX_COUNT as f32; - assert!( - tolerance_check(linear(desired_input.amplitude_dbfs) as f32, amplitude_normalized, total_magnitude_noise as f32, tolerance), - "magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}", - linear(desired_input.amplitude_dbfs), - desired_input.amplitude_dbfs, - amplitude_normalized, - dbfs(amplitude_normalized as f64), - max_error(linear(desired_input.amplitude_dbfs) as f32, total_magnitude_noise as f32, tolerance) - ); - assert!( - tolerance_check( - effective_phase_offset as f32, - magnitude_phase_decimated[k].1, - total_phase_noise as f32, - tolerance - ), - "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", - effective_phase_offset as f32, - magnitude_phase_decimated[k].1, - max_error( - effective_phase_offset as f32, - total_phase_noise as f32, - tolerance - ) - ); - - let in_phase_normalized: f32 = - signal_decimated[k].0 / ADC_MAX_COUNT as f32; - let quadrature_normalized: f32 = - signal_decimated[k].1 / ADC_MAX_COUNT as f32; - assert!( - tolerance_check( - in_phase_actual as f32, - in_phase_normalized, - total_noise_amplitude as f32, - tolerance - ), - "in-phase actual: {:.4}, in-phase computed: {:.3}, tolerance: {:.4}", - in_phase_actual, - in_phase_normalized, - max_error( - in_phase_actual as f32, - total_noise_amplitude as f32, - tolerance - ) - ); - assert!( - tolerance_check( - quadrature_actual as f32, - quadrature_normalized, - total_noise_amplitude as f32, - tolerance - ), - "quadrature actual: {:.4}, quadrature computed: {:.4}, tolerance: {:.4}", - quadrature_actual, - quadrature_normalized, - max_error( - quadrature_actual as f32, - total_noise_amplitude as f32, - tolerance - ) - ); - } - } - - timestamp_start += sample_count; - } -} - -#[test] -fn lowpass() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 100e3; - let harmonic: u32 = 1; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: 0., - }, - &mut vec![ - PureSine { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.9 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_demodulation_phase_offset_pi_2() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 100e3; - let harmonic: u32 = 1; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = PI / 2.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: 0., - }, - &mut vec![ - PureSine { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.9 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_phase_offset_pi_2() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 100e3; - let harmonic: u32 = 1; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: PI / 2., - }, - &mut vec![ - PureSine { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.9 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_fundamental_111e3_phase_offset_pi_4() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 111e3; - let harmonic: u32 = 1; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: PI / 4., - }, - &mut vec![ - PureSine { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.9 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_first_harmonic() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 50e3; - let harmonic: u32 = 2; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: 0., - }, - &mut vec![ - PureSine { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_second_harmonic() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 50e3; - let harmonic: u32 = 3; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: 0., - }, - &mut vec![ - PureSine { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_third_harmonic() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 50e3; - let harmonic: u32 = 4; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: 0., - }, - &mut vec![ - PureSine { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_first_harmonic_phase_shift() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 50e3; - let harmonic: u32 = 2; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: PI / 4., - }, - &mut vec![ - PureSine { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_adc_frequency_1e6() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 1e6; - let signal_frequency: f64 = 100e3; - let harmonic: u32 = 1; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: 0., - }, - &mut vec![ - PureSine { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_internal_frequency_125e6() { - let internal_frequency: f64 = 125e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 100e3; - let harmonic: u32 = 1; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: 0., - }, - &mut vec![ - PureSine { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - PureSine { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }, - ], - time_constant_factor, - tolerance, - ); -} - -#[test] -fn lowpass_low_signal_frequency() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = 500e3; - let signal_frequency: f64 = 10e3; - let harmonic: u32 = 1; - let corner_frequency: f64 = 1e3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f32 = 1e-2; - - lowpass_test( - internal_frequency, - adc_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - corner_frequency, - PureSine { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase_offset: 0., - }, - &mut vec![PureSine { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase_offset: 0., - }], - time_constant_factor, - tolerance, - ); -} From 31d23a3e0c784452343841d13d29e8d90d34f3ad Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 9 Jan 2021 12:57:13 -0800 Subject: [PATCH 04/16] lock-in: use same method for batch_index branching in both instances --- dsp/src/lockin.rs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 5b82313..4ab6f02 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -165,8 +165,9 @@ impl Lockin { frequency = self.last_frequency.unwrap(); } None => { - self.batch_index += 1; - if self.batch_index == ADC_BATCHES as u32 { + if self.batch_index < ADC_BATCHES as u32 - 1 { + self.batch_index += 1; + } else { self.batch_index = 0; } return Err("insufficient timestamps"); From 891aad3f1751ab70fa2ad50c8ca16abe63f5227f Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 07:43:28 -0800 Subject: [PATCH 05/16] remove debug_assert in divide_round --- dsp/src/lib.rs | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index aeec742..1161943 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -25,17 +25,15 @@ pub fn shift_round(x: i32, shift: usize) -> i32 { /// # Arguments /// /// `dividend` - Value to divide. -/// `divisor` - Value that divides the dividend. +/// `divisor` - Value that divides the +/// dividend. `dividend`+`divisor`-1 must be inside [i64::MIN, +/// i64::MAX]. /// /// # Returns /// /// Divided and rounded value. #[inline(always)] pub fn divide_round(dividend: i64, divisor: i64) -> i64 { - debug_assert!( - dividend as i128 + (divisor as i128 - 1) < i64::MAX as i128 - && dividend as i128 + (divisor as i128 - 1) > i64::MIN as i128 - ); (dividend + (divisor - 1)) / divisor } From e14aa8b613f4cc6dd3eeebc394235dabd6e8c354 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 10:45:34 -0800 Subject: [PATCH 06/16] move lock-in code to main.rs --- dsp/src/lib.rs | 1 - dsp/src/lockin.rs | 392 ---------------------------------------------- src/main.rs | 130 ++++++++++++--- 3 files changed, 110 insertions(+), 413 deletions(-) delete mode 100644 dsp/src/lockin.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 1161943..3f090b5 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -116,7 +116,6 @@ where pub mod iir; pub mod iir_int; -pub mod lockin; pub mod pll; pub mod trig; pub mod unwrap; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs deleted file mode 100644 index 4ab6f02..0000000 --- a/dsp/src/lockin.rs +++ /dev/null @@ -1,392 +0,0 @@ -//! Lock-in amplifier. -//! -//! Lock-in processing is performed through a combination of the -//! following modular processing blocks: demodulation, filtering, -//! decimation and computing the magnitude and phase from a complex -//! signal. These processing blocks are mutually independent. -//! -//! # Terminology -//! -//! * _demodulation signal_ - A copy of the reference signal that is -//! optionally frequency scaled and phase shifted. This is a complex -//! signal. The demodulation signals are used to demodulate the ADC -//! sampled signal. -//! * _internal clock_ - A fast internal clock used to increment a -//! counter for determining the 0-phase points of a reference signal. -//! * _reference signal_ - A constant-frequency signal used to derive -//! the demodulation signal. -//! * _timestamp_ - Timestamps record the timing of the reference -//! signal's 0-phase points. For instance, if a reference signal is -//! provided externally, a fast internal clock increments a -//! counter. When the external reference reaches the 0-phase point -//! (e.g., a positive edge), the value of the counter is recorded as a -//! timestamp. These timestamps are used to determine the frequency -//! and phase of the reference signal. -//! -//! # Usage -//! -//! The first step is to initialize a `Lockin` instance with -//! `Lockin::new()`. This provides the lock-in algorithms with -//! necessary information about the demodulation and filtering steps, -//! such as whether to demodulate with a harmonic of the reference -//! signal and the IIR biquad filter to use. There are then 4 -//! different processing steps that can be used: -//! -//! * `demodulate` - Computes the phase of the demodulation signal -//! corresponding to each ADC sample, uses this phase to compute the -//! demodulation signal, and multiplies this demodulation signal by -//! the ADC-sampled signal. This is a method of `Lockin` since it -//! requires information about how to modify the reference signal for -//! demodulation. -//! * `filter` - Performs IIR biquad filtering of a complex -//! signals. This is commonly performed on the signal provided by the -//! demodulation step, but can be performed at any other point in the -//! processing chain or omitted entirely. `filter` is a method of -//! `Lockin` since it must hold onto the filter configuration and -//! state. -//! * `decimate` - This decimates a signal to reduce the load on the -//! DAC output. It does not require any state information and is -//! therefore a normal function. -//! * `magnitude_phase` - Computes the magnitude and phase of the -//! component of the ADC-sampled signal whose frequency is equal to -//! the demodulation frequency. This does not require any state -//! information and is therefore a normal function. - -use super::iir_int::{IIRState, IIR}; -use super::pll::PLL; -use super::trig::{atan2, cossin}; -use super::{divide_round, Complex}; - -/// TODO these constants are copied from main.rs and should be -/// shared. Additionally, we should probably store the log2 values and -/// compute the actual values from these in main, as is done here. -pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 0; -pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2; - -pub const ADC_SAMPLE_TICKS_LOG2: usize = 8; -pub const ADC_SAMPLE_TICKS: usize = 1 << ADC_SAMPLE_TICKS_LOG2; - -pub const ADC_BATCHES_LOG2: usize = - 32 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_SAMPLE_TICKS_LOG2; -pub const ADC_BATCHES: usize = 1 << ADC_BATCHES_LOG2; - -pub const DECIMATED_BUFFER_SIZE: usize = 1; - -/// Performs lock-in amplifier processing of a signal. -pub struct Lockin { - harmonic: u32, - phase_offset: u32, - batch_index: u32, - last_phase: Option, - last_frequency: Option, - pll: PLL, - pll_shift_frequency: u8, - pll_shift_phase: u8, - iir: IIR, - iirstate: [IIRState; 2], -} - -impl Lockin { - /// Initialize a new `Lockin` instance. - /// - /// # Arguments - /// - /// * `harmonic` - Integer scaling factor used to adjust the - /// demodulation frequency. E.g., 2 would demodulate with the - /// first harmonic. - /// * `phase_offset` - Phase offset applied to the demodulation - /// signal. - /// * `iir` - IIR biquad filter. - /// * `pll_shift_frequency` - See PLL::update(). - /// * `pll_shift_phase` - See PLL::update(). - /// - /// # Returns - /// - /// New `Lockin` instance. - pub fn new( - harmonic: u32, - phase_offset: u32, - iir: IIR, - pll_shift_frequency: u8, - pll_shift_phase: u8, - ) -> Self { - Lockin { - harmonic, - phase_offset, - batch_index: 0, - last_phase: None, - last_frequency: None, - pll: PLL::default(), - pll_shift_frequency, - pll_shift_phase, - iir, - iirstate: [[0; 5]; 2], - } - } - - /// Demodulate an input signal with the complex reference signal. - /// - /// # Arguments - /// - /// * `adc_samples` - One batch of ADC samples. - /// * `timestamp` - Counter value corresponding to the edges of an - /// external reference signal. The counter is incremented by a - /// fast internal clock. Each ADC sample batch can contain 0 or 1 - /// timestamps. - /// - /// # Returns - /// - /// The demodulated complex signal as a `Result`. When there are - /// an insufficient number of timestamps to perform processing, - /// `Err` is returned. - pub fn demodulate( - &mut self, - adc_samples: &[i16], - timestamp: Option, - ) -> Result<[Complex; SAMPLE_BUFFER_SIZE], &str> { - let frequency: i64; - let phase: i64; - - match timestamp { - Some(t) => { - let res = self.pll.update( - t as i32, - self.pll_shift_frequency, - self.pll_shift_phase, - ); - phase = res.0 as u32 as i64; - frequency = res.1 as u32 as i64; - self.last_phase = Some(phase); - self.last_frequency = Some(frequency); - } - None => match self.last_phase { - Some(t) => { - phase = t; - frequency = self.last_frequency.unwrap(); - } - None => { - if self.batch_index < ADC_BATCHES as u32 - 1 { - self.batch_index += 1; - } else { - self.batch_index = 0; - } - return Err("insufficient timestamps"); - } - }, - } - - let demodulation_frequency = divide_round( - 1 << (64 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_BATCHES_LOG2), - frequency, - ) as u32; - let demodulation_initial_phase = divide_round( - (((self.batch_index as i64) << (32 - ADC_BATCHES_LOG2)) - phase) - << 32, - frequency, - ) as u32; - - let mut demodulation_signal = [(0_i32, 0_i32); SAMPLE_BUFFER_SIZE]; - - demodulation_signal - .iter_mut() - .zip(adc_samples.iter()) - .enumerate() - .for_each(|(i, (s, sample))| { - let sample_phase = (self.harmonic.wrapping_mul( - (demodulation_frequency.wrapping_mul(i as u32)) - .wrapping_add(demodulation_initial_phase), - )) - .wrapping_add(self.phase_offset); - let (cos, sin) = cossin(sample_phase as i32); - // cos/sin take up 32 bits and sample takes up 16 - // bits. Make this fit into a 32 bit result. - s.0 = ((*sample as i64 * cos as i64) >> 16) as i32; - s.1 = ((*sample as i64 * sin as i64) >> 16) as i32; - }); - - if self.batch_index < ADC_BATCHES as u32 - 1 { - self.batch_index += 1; - } else { - self.batch_index = 0; - self.last_phase = Some(self.last_phase.unwrap() - (1 << 32)); - } - - Ok(demodulation_signal) - } - - /// Filter the complex signal using the supplied biquad IIR. The - /// signal array is modified in place. - /// - /// # Arguments - /// - /// * `signal` - Complex signal to filter. - pub fn filter(&mut self, signal: &mut [Complex]) { - signal.iter_mut().for_each(|s| { - s.0 = self.iir.update(&mut self.iirstate[0], s.0); - s.1 = self.iir.update(&mut self.iirstate[1], s.1); - }); - } -} - -/// Decimate the complex signal to `DECIMATED_BUFFER_SIZE`. The ratio -/// of `SAMPLE_BUFFER_SIZE` to `DECIMATED_BUFFER_SIZE` must be a power -/// of 2. -/// -/// # Arguments -/// -/// * `signal` - Complex signal to decimate. -/// -/// # Returns -/// -/// The decimated signal. -pub fn decimate( - signal: [Complex; SAMPLE_BUFFER_SIZE], -) -> [Complex; DECIMATED_BUFFER_SIZE] { - let n_k = SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE; - debug_assert!(SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0); - - let mut signal_decimated = [(0_i32, 0_i32); DECIMATED_BUFFER_SIZE]; - - signal_decimated - .iter_mut() - .zip(signal.iter().step_by(n_k)) - .for_each(|(s_d, s)| { - s_d.0 = s.0; - s_d.1 = s.1; - }); - - signal_decimated -} - -/// Compute the magnitude and phase from the complex signal. The -/// signal array is modified in place. -/// -/// # Arguments -/// -/// * `signal` - Complex signal for which the magnitude and phase -/// should be computed. TODO currently, we compute the square of the -/// magnitude. This should be changed to be the actual magnitude. -pub fn magnitude_phase(signal: &mut [Complex]) { - signal.iter_mut().for_each(|s| { - let new_i = [s.0, s.1].iter().map(|i| i * i).sum(); - let new_q = atan2(s.1, s.0); - s.0 = new_i; - s.1 = new_q; - }); -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - /// Ensure that the demodulation signals are within some tolerance - /// band of the target value given the phase and frequency values - /// provided by the PLL. - fn demodulate() { - const PLL_SHIFT_FREQUENCY: u8 = 4; - const PLL_SHIFT_PHASE: u8 = 3; - const HARMONIC: u32 = 1; - const PHASE_OFFSET: u32 = 0; - let mut lockin = Lockin::new( - HARMONIC, - PHASE_OFFSET, - IIR { ba: [0; 5] }, - PLL_SHIFT_FREQUENCY, - PLL_SHIFT_PHASE, - ); - - // Duplicate the PLL outside demodulate so that we don't test - // its behavior. - let mut tracking_pll = PLL::default(); - let mut tracking_phase: i32 = 0; - let mut tracking_frequency: i32 = 0; - - const REFERENCE_FREQUENCY: usize = 10_000; - let mut reference_edge: usize = REFERENCE_FREQUENCY; - - // Ensure that we receive at most 1 timestamp per batch. - debug_assert!( - REFERENCE_FREQUENCY >= SAMPLE_BUFFER_SIZE * ADC_SAMPLE_TICKS - ); - - for batch in 0..100_000 { - let tick: usize = batch * ADC_SAMPLE_TICKS * SAMPLE_BUFFER_SIZE; - let timestamp: Option; - - // When the reference edge occurred during the current - // batch acquisition, register the timestamp and update - // the tracking PLL. - if reference_edge >= tick - && reference_edge < tick + ADC_SAMPLE_TICKS * SAMPLE_BUFFER_SIZE - { - timestamp = Some(reference_edge as u32); - - let tracking_update = tracking_pll.update( - reference_edge as i32, - PLL_SHIFT_FREQUENCY, - PLL_SHIFT_PHASE, - ); - tracking_phase = tracking_update.0; - tracking_frequency = tracking_update.1; - - reference_edge += REFERENCE_FREQUENCY; - } else { - timestamp = None; - } - - let timestamp_before_batch = if tracking_phase > tick as i32 { - // There can be at most 1 reference edge per batch, so - // this will necessarily place the timestamp prior to - // the current batch. - tracking_phase - tracking_frequency - } else { - tracking_phase - }; - - let initial_phase = (((tick as f64 - - timestamp_before_batch as f64) - / tracking_frequency as f64 - * (1_i64 << 32) as f64) - .round() - % u32::MAX as f64) as u32; - let frequency = ((ADC_SAMPLE_TICKS as f64 - / tracking_frequency as f64 - * (1_i64 << 32) as f64) - .round() - % u32::MAX as f64) as u32; - - match lockin.demodulate(&[i16::MAX; SAMPLE_BUFFER_SIZE], timestamp) - { - Ok(v) => { - println!("batch : {}", batch); - for sample in 0..SAMPLE_BUFFER_SIZE { - const TOL: i32 = 50_000; - let cos = v[sample].0; - let sin = v[sample].1; - - let (mut target_cos, mut target_sin) = cossin( - HARMONIC - .wrapping_mul( - (frequency.wrapping_mul(sample as u32)) - .wrapping_add(initial_phase), - ) - .wrapping_add(PHASE_OFFSET) - as i32, - ); - target_cos /= 2; - target_sin /= 2; - - println!("sample : {}", sample); - println!("tol : {}", TOL); - println!("cos, target: {}, {}", cos, target_cos); - println!("sin, target: {}, {}", sin, target_sin); - assert!((cos - target_cos).abs() < TOL); - assert!((sin - target_sin).abs() < TOL); - } - } - Err(_) => {} - } - } - } -} diff --git a/src/main.rs b/src/main.rs index c5a9698..538fb2a 100644 --- a/src/main.rs +++ b/src/main.rs @@ -60,14 +60,34 @@ use heapless::{consts::*, String}; // 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 -const ADC_SAMPLE_TICKS: u16 = 256; +const ADC_SAMPLE_TICKS_LOG2: u16 = 8; +const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2; // The desired ADC sample processing buffer size. -const SAMPLE_BUFFER_SIZE: usize = 8; +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. +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; // The number of cascaded IIR biquads per channel. Select 1 or 2! 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(); @@ -84,7 +104,11 @@ mod timers; use adc::{Adc0Input, Adc1Input}; use dac::{Dac0Output, Dac1Output}; -use dsp::iir; +use dsp::{ + divide_round, iir, iir_int, + pll::PLL, + trig::{atan2, cossin}, +}; use pounder::DdsOutput; #[cfg(not(feature = "semihosting"))] @@ -205,6 +229,12 @@ 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, + iir_lockin: iir_int::IIR, + iir_state_lockin: [iir_int::IIRState; 2], eeprom_i2c: hal::i2c::I2c, @@ -927,6 +957,13 @@ 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 iir_lockin = iir_int::IIR { ba: [0; 5] }; + let iir_state_lockin = [[0; 5]; 2]; + // Start sampling ADCs. sampling_timer.start(); timestamp_timer.start(); @@ -942,6 +979,13 @@ const APP: () = { pounder: pounder_devices, pounder_stamper, + pll, + batch_index, + reference_phase, + reference_frequency, + iir_lockin, + iir_state_lockin, + eeprom_i2c, net_interface: network_interface, eth_mac, @@ -949,7 +993,7 @@ const APP: () = { } } - #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper], priority=2)] + #[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)] fn process(c: process::Context) { if let Some(stamper) = c.resources.pounder_stamper { let pounder_timestamps = stamper.acquire_buffer(); @@ -965,24 +1009,71 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; - let _timestamp = c.resources.input_stamper.latest_timestamp(); + let reference_phase = c.resources.reference_phase; + let reference_frequency = c.resources.reference_frequency; - for channel in 0..adc_samples.len() { - for sample in 0..adc_samples[0].len() { - let x = f32::from(adc_samples[channel][sample] as i16); - let mut y = x; - for i in 0..c.resources.iir_state[channel].len() { - y = c.resources.iir_ch[channel][i] - .update(&mut c.resources.iir_state[channel][i], y); - } - // Note(unsafe): The filter limits ensure that the value is in range. - // The truncation introduces 1/2 LSB distortion. - let y = unsafe { y.to_int_unchecked::() }; - // Convert to DAC code - dac_samples[channel][sample] = y as u16 ^ 0x8000; - } + 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 [dac0, dac1] = dac_samples; + let iir_lockin = c.resources.iir_lockin; + let iir_state_lockin = c.resources.iir_state_lockin; + + dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( + |(i, (d0, d1))| { + let sample_phase = (HARMONIC.wrapping_mul( + (demodulation_frequency.wrapping_mul(i as u32)) + .wrapping_add(demodulation_initial_phase), + )) + .wrapping_add(PHASE_OFFSET); + let (cos, sin) = cossin(sample_phase as i32); + + let mut signal = (0_i32, 0_i32); + + signal.0 = ((adc_samples[0][i] as i16 as i64 * cos as i64) + >> 16) as i32; + signal.1 = ((adc_samples[0][i] as i16 as i64 * sin as i64) + >> 16) as i32; + + signal.0 = + iir_lockin.update(&mut iir_state_lockin[0], signal.0); + signal.1 = + iir_lockin.update(&mut iir_state_lockin[1], signal.0); + + let magnitude = signal.0 * signal.0 + signal.1 * signal.1; + let phase = atan2(signal.0, signal.0); + + *d0 = (magnitude >> 16) as i16 as u16; + *d1 = (phase >> 16) as i16 as u16; + }, + ); + if let Some(dds_output) = c.resources.dds_output { let builder = dds_output.builder().update_channels( &[pounder::Channel::Out0.into()], @@ -994,7 +1085,6 @@ const APP: () = { builder.write_profile(); } - let [dac0, dac1] = dac_samples; c.resources.dacs.0.release_buffer(dac0); c.resources.dacs.1.release_buffer(dac1); } From 4c033c0f3e3068fbfe03fd0e5f794cc02ea61c4e Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 13:04:40 -0800 Subject: [PATCH 07/16] 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; From 41ea2ebed4dfe917dcf845b452143cc9c90139c4 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 15:50:26 -0800 Subject: [PATCH 08/16] use round up half integer rounding --- dsp/src/lib.rs | 6 ++++++ src/main.rs | 20 +++++++++++++------- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 3f090b5..2f02601 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -20,6 +20,12 @@ pub fn shift_round(x: i32, shift: usize) -> i32 { (x + (1 << (shift - 1))) >> shift } +/// TODO consolidate with `shift_round`. +#[inline(always)] +pub fn shift_round64(x: i64, shift: usize) -> i64 { + (x + (1 << (shift - 1))) >> shift +} + /// Integer division, round up half. /// /// # Arguments diff --git a/src/main.rs b/src/main.rs index 1bc6a11..3d484d8 100644 --- a/src/main.rs +++ b/src/main.rs @@ -101,6 +101,7 @@ use dac::{Dac0Output, Dac1Output}; use dsp::{ divide_round, iir, iir_int, pll::PLL, + shift_round, shift_round64, trig::{atan2, cossin}, }; use pounder::DdsOutput; @@ -196,7 +197,7 @@ impl TimestampHandler { } } - /// Compute the initial phase value and frequency of the demodulation signal. + /// Compute the initial phase and frequency of the demodulation signal. /// /// # Args /// * `timestamp` - Counter value corresponding to an external reference edge. @@ -1087,10 +1088,15 @@ const APP: () = { let mut signal = (0_i32, 0_i32); - signal.0 = ((adc_samples[0][i] as i16 as i64 * cos as i64) - >> 16) as i32; - signal.1 = ((adc_samples[0][i] as i16 as i64 * sin as i64) - >> 16) as i32; + // TODO should we shift cos/sin first to avoid i64? + signal.0 = shift_round64( + adc_samples[0][i] as i16 as i64 * cos as i64, + 16, + ) as i32; + signal.1 = shift_round64( + adc_samples[0][i] as i16 as i64 * sin as i64, + 16, + ) as i32; signal.0 = iir_lockin.update(&mut iir_state_lockin[0], signal.0); @@ -1100,8 +1106,8 @@ const APP: () = { let magnitude = signal.0 * signal.0 + signal.1 * signal.1; let phase = atan2(signal.1, signal.0); - *d0 = (magnitude >> 16) as i16 as u16; - *d1 = (phase >> 16) as i16 as u16; + *d0 = shift_round(magnitude, 16) as i16 as u16; + *d1 = shift_round(phase, 16) as i16 as u16; }, ); From 80ed715f5a11411a3396e98fab7906c89884938b Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 16:07:04 -0800 Subject: [PATCH 09/16] shift sin/cos before demodulation product to avoid i64 --- dsp/src/lib.rs | 6 ------ src/main.rs | 16 ++++++---------- 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 2f02601..3f090b5 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -20,12 +20,6 @@ pub fn shift_round(x: i32, shift: usize) -> i32 { (x + (1 << (shift - 1))) >> shift } -/// TODO consolidate with `shift_round`. -#[inline(always)] -pub fn shift_round64(x: i64, shift: usize) -> i64 { - (x + (1 << (shift - 1))) >> shift -} - /// Integer division, round up half. /// /// # Arguments diff --git a/src/main.rs b/src/main.rs index 3d484d8..22fe534 100644 --- a/src/main.rs +++ b/src/main.rs @@ -101,7 +101,7 @@ use dac::{Dac0Output, Dac1Output}; use dsp::{ divide_round, iir, iir_int, pll::PLL, - shift_round, shift_round64, + shift_round, trig::{atan2, cossin}, }; use pounder::DdsOutput; @@ -1088,15 +1088,11 @@ const APP: () = { let mut signal = (0_i32, 0_i32); - // TODO should we shift cos/sin first to avoid i64? - signal.0 = shift_round64( - adc_samples[0][i] as i16 as i64 * cos as i64, - 16, - ) as i32; - signal.1 = shift_round64( - adc_samples[0][i] as i16 as i64 * sin as i64, - 16, - ) as i32; + // shift cos/sin before multiplying to avoid i64 multiplication + signal.0 = + adc_samples[0][i] as i16 as i32 * shift_round(cos, 16); + signal.0 = + adc_samples[0][i] as i16 as i32 * shift_round(sin, 16); signal.0 = iir_lockin.update(&mut iir_state_lockin[0], signal.0); From f974f4099ca22eddb32269857dea88120bfbf101 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 16:17:58 -0800 Subject: [PATCH 10/16] remove TODO note relating ADC_BATCHES and calculate_timestamp_timer_period Having both is not really redundant. --- src/main.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main.rs b/src/main.rs index 22fe534..be947f4 100644 --- a/src/main.rs +++ b/src/main.rs @@ -68,7 +68,6 @@ 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; From a0d472b398f2cae1e3b7d5a15407ee6916999ceb Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 16:35:44 -0800 Subject: [PATCH 11/16] use only integer iir --- src/main.rs | 36 +++++++++++++++++++++--------------- src/server.rs | 12 ++++++------ 2 files changed, 27 insertions(+), 21 deletions(-) diff --git a/src/main.rs b/src/main.rs index be947f4..bc439c9 100644 --- a/src/main.rs +++ b/src/main.rs @@ -75,7 +75,6 @@ pub const ADC_BATCHES: usize = 1 << ADC_BATCHES_LOG2; // The number of cascaded IIR biquads per channel. Select 1 or 2! 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. @@ -98,7 +97,8 @@ mod timers; use adc::{Adc0Input, Adc1Input}; use dac::{Dac0Output, Dac1Output}; use dsp::{ - divide_round, iir, iir_int, + divide_round, + iir_int::{IIRState, IIR}, pll::PLL, shift_round, trig::{atan2, cossin}, @@ -146,8 +146,6 @@ static mut NET_STORE: NetStorage = NetStorage { routes_storage: [None; 1], }; -const SCALE: f32 = ((1 << 15) - 1) as f32; - // static ETHERNET_PENDING: AtomicBool = AtomicBool::new(true); const TCP_RX_BUFFER_SIZE: usize = 8192; @@ -163,8 +161,8 @@ 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. +/// Processes external timestamps to produce the frequency and initial phase of the demodulation +/// signal. pub struct TimestampHandler { pll: PLL, pll_shift_frequency: u8, @@ -297,8 +295,8 @@ const APP: () = { dacs: (Dac0Output, Dac1Output), input_stamper: digital_input_stamper::InputStamper, timestamp_handler: TimestampHandler, - iir_lockin: iir_int::IIR, - iir_state_lockin: [iir_int::IIRState; 2], + iir_lockin: IIR, + iir_state_lockin: [IIRState; 2], eeprom_i2c: hal::i2c::I2c, @@ -320,10 +318,10 @@ const APP: () = { pounder_stamper: Option, // Format: iir_state[ch][cascade-no][coeff] - #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] - iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], - #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] - iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], + #[init([[[0; 5]; IIR_CASCADE_LENGTH]; 2])] + iir_state: [[IIRState; IIR_CASCADE_LENGTH]; 2], + #[init([[IIR { ba: [1, 0, 0, 0, 0] }; IIR_CASCADE_LENGTH]; 2])] + iir_ch: [[IIR; IIR_CASCADE_LENGTH]; 2], } #[init] @@ -1022,7 +1020,7 @@ const APP: () = { let pounder_stamper = None; let timestamp_handler = TimestampHandler::new(4, 3); - let iir_lockin = iir_int::IIR { ba: [0; 5] }; + let iir_lockin = IIR { ba: [1, 0, 0, 0, 0] }; let iir_state_lockin = [[0; 5]; 2]; // Start sampling ADCs. @@ -1075,6 +1073,8 @@ const APP: () = { let [dac0, dac1] = dac_samples; let iir_lockin = c.resources.iir_lockin; let iir_state_lockin = c.resources.iir_state_lockin; + let iir_ch = c.resources.iir_ch; + let iir_state = c.resources.iir_state; dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( |(i, (d0, d1))| { @@ -1098,8 +1098,14 @@ const APP: () = { signal.1 = iir_lockin.update(&mut iir_state_lockin[1], signal.1); - let magnitude = signal.0 * signal.0 + signal.1 * signal.1; - let phase = atan2(signal.1, signal.0); + let mut magnitude = signal.0 * signal.0 + signal.1 * signal.1; + let mut phase = atan2(signal.1, signal.0); + + for j in 0..iir_state[0].len() { + magnitude = + iir_ch[0][j].update(&mut iir_state[0][j], magnitude); + phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); + } *d0 = shift_round(magnitude, 16) as i16 as u16; *d1 = shift_round(phase, 16) as i16 as u16; diff --git a/src/server.rs b/src/server.rs index 2803805..72e9102 100644 --- a/src/server.rs +++ b/src/server.rs @@ -6,8 +6,8 @@ use serde::{Deserialize, Serialize}; use serde_json_core::{de::from_slice, ser::to_string}; -use super::iir; use super::net; +use super::IIR; #[derive(Deserialize, Serialize, Debug)] pub enum AccessRequest { @@ -25,7 +25,7 @@ pub struct Request<'a> { #[derive(Serialize, Deserialize)] pub struct IirRequest { pub channel: u8, - pub iir: iir::IIR, + pub iir: IIR, } #[derive(Serialize)] @@ -137,10 +137,10 @@ impl Response { #[derive(Serialize)] pub struct Status { pub t: u32, - pub x0: f32, - pub y0: f32, - pub x1: f32, - pub y1: f32, + pub x0: i32, + pub y0: i32, + pub x1: i32, + pub y1: i32, } pub fn json_reply(socket: &mut net::socket::TcpSocket, msg: &T) { From 07b7201b49c79fd16f76328c80696ea45b7e7904 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 17:26:42 -0800 Subject: [PATCH 12/16] fix cargo fmt style --- src/main.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main.rs b/src/main.rs index bc439c9..19e0b3d 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1020,7 +1020,9 @@ const APP: () = { let pounder_stamper = None; let timestamp_handler = TimestampHandler::new(4, 3); - let iir_lockin = IIR { ba: [1, 0, 0, 0, 0] }; + let iir_lockin = IIR { + ba: [1, 0, 0, 0, 0], + }; let iir_state_lockin = [[0; 5]; 2]; // Start sampling ADCs. From 6aad92af433e7a3da44ef2e74930c71cc6540b49 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 12 Jan 2021 18:36:18 -0800 Subject: [PATCH 13/16] fix bug in which real signal component is assigned twice --- src/main.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.rs b/src/main.rs index 19e0b3d..347eebe 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1092,7 +1092,7 @@ const APP: () = { // shift cos/sin before multiplying to avoid i64 multiplication signal.0 = adc_samples[0][i] as i16 as i32 * shift_round(cos, 16); - signal.0 = + signal.1 = adc_samples[0][i] as i16 as i32 * shift_round(sin, 16); signal.0 = From 76088efda52bfdcb65c3f7514a0fd261eac1447f Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 13 Jan 2021 08:37:33 -0800 Subject: [PATCH 14/16] dsp: add reciprocal_pll --- dsp/src/lib.rs | 1 + dsp/src/reciprocal_pll.rs | 92 +++++++++++++++++++++++++++++++++++++++ src/main.rs | 73 ------------------------------- 3 files changed, 93 insertions(+), 73 deletions(-) create mode 100644 dsp/src/reciprocal_pll.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 3f090b5..77d8bc1 100644 --- a/dsp/src/lib.rs +++ b/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; diff --git a/dsp/src/reciprocal_pll.rs b/dsp/src/reciprocal_pll.rs new file mode 100644 index 0000000..49a135b --- /dev/null +++ b/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) { + 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) + } +} diff --git a/src/main.rs b/src/main.rs index 347eebe..6de262c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -161,79 +161,6 @@ type AFE1 = afe::ProgrammableGainAmplifier< hal::gpio::gpiod::PD15>, >; -/// 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) { - 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),*], From e599977983dd097f9854c4f553a67ba3b55c207a Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 13 Jan 2021 08:53:06 -0800 Subject: [PATCH 15/16] revert changes in main.rs and server.rs --- src/main.rs | 109 ++++++++++++-------------------------------------- src/server.rs | 12 +++--- 2 files changed, 32 insertions(+), 89 deletions(-) diff --git a/src/main.rs b/src/main.rs index 6de262c..c5a9698 100644 --- a/src/main.rs +++ b/src/main.rs @@ -60,26 +60,14 @@ use heapless::{consts::*, String}; // 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 -const ADC_SAMPLE_TICKS_LOG2: u16 = 8; -const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2; +const ADC_SAMPLE_TICKS: u16 = 256; // The desired ADC sample processing buffer size. -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. -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; +const SAMPLE_BUFFER_SIZE: usize = 8; // The number of cascaded IIR biquads per channel. Select 1 or 2! const IIR_CASCADE_LENGTH: usize = 1; -// 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; - #[link_section = ".sram3.eth"] static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); @@ -96,13 +84,7 @@ mod timers; use adc::{Adc0Input, Adc1Input}; use dac::{Dac0Output, Dac1Output}; -use dsp::{ - divide_round, - iir_int::{IIRState, IIR}, - pll::PLL, - shift_round, - trig::{atan2, cossin}, -}; +use dsp::iir; use pounder::DdsOutput; #[cfg(not(feature = "semihosting"))] @@ -146,6 +128,8 @@ static mut NET_STORE: NetStorage = NetStorage { routes_storage: [None; 1], }; +const SCALE: f32 = ((1 << 15) - 1) as f32; + // static ETHERNET_PENDING: AtomicBool = AtomicBool::new(true); const TCP_RX_BUFFER_SIZE: usize = 8192; @@ -221,9 +205,6 @@ const APP: () = { adcs: (Adc0Input, Adc1Input), dacs: (Dac0Output, Dac1Output), input_stamper: digital_input_stamper::InputStamper, - timestamp_handler: TimestampHandler, - iir_lockin: IIR, - iir_state_lockin: [IIRState; 2], eeprom_i2c: hal::i2c::I2c, @@ -245,10 +226,10 @@ const APP: () = { pounder_stamper: Option, // Format: iir_state[ch][cascade-no][coeff] - #[init([[[0; 5]; IIR_CASCADE_LENGTH]; 2])] - iir_state: [[IIRState; IIR_CASCADE_LENGTH]; 2], - #[init([[IIR { ba: [1, 0, 0, 0, 0] }; IIR_CASCADE_LENGTH]; 2])] - iir_ch: [[IIR; IIR_CASCADE_LENGTH]; 2], + #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] + iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] + iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], } #[init] @@ -946,12 +927,6 @@ const APP: () = { #[cfg(not(feature = "pounder_v1_1"))] let pounder_stamper = None; - let timestamp_handler = TimestampHandler::new(4, 3); - let iir_lockin = IIR { - ba: [1, 0, 0, 0, 0], - }; - let iir_state_lockin = [[0; 5]; 2]; - // Start sampling ADCs. sampling_timer.start(); timestamp_timer.start(); @@ -967,10 +942,6 @@ const APP: () = { pounder: pounder_devices, pounder_stamper, - timestamp_handler, - iir_lockin, - iir_state_lockin, - eeprom_i2c, net_interface: network_interface, eth_mac, @@ -978,7 +949,7 @@ const APP: () = { } } - #[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)] + #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper], priority=2)] fn process(c: process::Context) { if let Some(stamper) = c.resources.pounder_stamper { let pounder_timestamps = stamper.acquire_buffer(); @@ -994,52 +965,23 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; - let (demodulation_initial_phase, demodulation_frequency) = c - .resources - .timestamp_handler - .update(c.resources.input_stamper.latest_timestamp()); + let _timestamp = c.resources.input_stamper.latest_timestamp(); - let [dac0, dac1] = dac_samples; - let iir_lockin = c.resources.iir_lockin; - let iir_state_lockin = c.resources.iir_state_lockin; - let iir_ch = c.resources.iir_ch; - let iir_state = c.resources.iir_state; - - dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( - |(i, (d0, d1))| { - let sample_phase = (HARMONIC.wrapping_mul( - (demodulation_frequency.wrapping_mul(i as u32)) - .wrapping_add(demodulation_initial_phase), - )) - .wrapping_add(PHASE_OFFSET); - let (cos, sin) = cossin(sample_phase as i32); - - let mut signal = (0_i32, 0_i32); - - // shift cos/sin before multiplying to avoid i64 multiplication - signal.0 = - adc_samples[0][i] as i16 as i32 * shift_round(cos, 16); - signal.1 = - adc_samples[0][i] as i16 as i32 * shift_round(sin, 16); - - signal.0 = - iir_lockin.update(&mut iir_state_lockin[0], signal.0); - signal.1 = - iir_lockin.update(&mut iir_state_lockin[1], signal.1); - - let mut magnitude = signal.0 * signal.0 + signal.1 * signal.1; - let mut phase = atan2(signal.1, signal.0); - - for j in 0..iir_state[0].len() { - magnitude = - iir_ch[0][j].update(&mut iir_state[0][j], magnitude); - phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); + for channel in 0..adc_samples.len() { + for sample in 0..adc_samples[0].len() { + let x = f32::from(adc_samples[channel][sample] as i16); + let mut y = x; + for i in 0..c.resources.iir_state[channel].len() { + y = c.resources.iir_ch[channel][i] + .update(&mut c.resources.iir_state[channel][i], y); } - - *d0 = shift_round(magnitude, 16) as i16 as u16; - *d1 = shift_round(phase, 16) as i16 as u16; - }, - ); + // Note(unsafe): The filter limits ensure that the value is in range. + // The truncation introduces 1/2 LSB distortion. + let y = unsafe { y.to_int_unchecked::() }; + // Convert to DAC code + dac_samples[channel][sample] = y as u16 ^ 0x8000; + } + } if let Some(dds_output) = c.resources.dds_output { let builder = dds_output.builder().update_channels( @@ -1052,6 +994,7 @@ const APP: () = { builder.write_profile(); } + let [dac0, dac1] = dac_samples; c.resources.dacs.0.release_buffer(dac0); c.resources.dacs.1.release_buffer(dac1); } diff --git a/src/server.rs b/src/server.rs index 72e9102..2803805 100644 --- a/src/server.rs +++ b/src/server.rs @@ -6,8 +6,8 @@ use serde::{Deserialize, Serialize}; use serde_json_core::{de::from_slice, ser::to_string}; +use super::iir; use super::net; -use super::IIR; #[derive(Deserialize, Serialize, Debug)] pub enum AccessRequest { @@ -25,7 +25,7 @@ pub struct Request<'a> { #[derive(Serialize, Deserialize)] pub struct IirRequest { pub channel: u8, - pub iir: IIR, + pub iir: iir::IIR, } #[derive(Serialize)] @@ -137,10 +137,10 @@ impl Response { #[derive(Serialize)] pub struct Status { pub t: u32, - pub x0: i32, - pub y0: i32, - pub x1: i32, - pub y1: i32, + pub x0: f32, + pub y0: f32, + pub x1: f32, + pub y1: f32, } pub fn json_reply(socket: &mut net::socket::TcpSocket, msg: &T) { From 9697560404ad32a3cffdda22db75d2313afd702c Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 13 Jan 2021 09:08:16 -0800 Subject: [PATCH 16/16] reciprocal_pll: remove unneeded type cast --- dsp/src/reciprocal_pll.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dsp/src/reciprocal_pll.rs b/dsp/src/reciprocal_pll.rs index 49a135b..eaeaac7 100644 --- a/dsp/src/reciprocal_pll.rs +++ b/dsp/src/reciprocal_pll.rs @@ -78,7 +78,7 @@ impl TimestampHandler { if self.batch_index < (1 << (32 - self.adc_sample_ticks_log2 - - self.sample_buffer_size_log2)) as u32 + - self.sample_buffer_size_log2)) - 1 { self.batch_index += 1;