diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index fb189fa..77d8bc1 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,23 @@ 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. `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 { + (dividend + (divisor - 1)) / divisor +} + fn abs(x: T) -> T where T: PartialOrd + Default + Neg, @@ -99,8 +116,8 @@ where pub mod iir; pub mod iir_int; -pub mod lockin; pub mod pll; +pub mod reciprocal_pll; pub mod trig; pub mod unwrap; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs deleted file mode 100644 index bb649ad..0000000 --- a/dsp/src/lockin.rs +++ /dev/null @@ -1,518 +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::{IIRState, IIR}; -use super::Complex; -use core::f32::consts::PI; - -/// 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], - iir: IIR, - iirstate: [IIRState; 2], -} - -impl Lockin { - /// Initialize a new `Lockin` instance. - /// - /// # 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. - /// * `iir` - IIR biquad filter. - /// - /// # Returns - /// - /// New `Lockin` instance. - pub fn new( - phase_offset: f32, - sample_period: u32, - harmonic: u32, - iir: IIR, - ) -> Self { - Lockin { - phase_offset: phase_offset, - sample_period: sample_period, - harmonic: harmonic, - timestamps: [None, None], - iir: iir, - iirstate: [[0.; 5]; 2], - } - } - - /// Demodulate an input signal with the complex reference signal. - /// - /// # 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. - /// - /// # 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 => (), - }); - - // 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"); - } - - 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 }; - - // compute ADC sample phases, sines/cosines and demodulate - 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; - }); - - // record new timestamps - let start_index: usize = if timestamps.len() < 2 { - 0 - } else { - timestamps.len() - 2 - }; - timestamps[start_index..] - .iter() - .for_each(|t| self.timestamps.push(Some(*t as i32))); - - Ok(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 `ADC_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; 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 - ); - - let mut signal_decimated = [(0_f32, 0_f32); 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 to decimate. -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); - s.0 = new_i; - s.1 = new_q; - }); -} - -#[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() { - 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], &[]), - 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") - ); - } - - #[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., - }, - ); - 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; - } - 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/src/reciprocal_pll.rs b/dsp/src/reciprocal_pll.rs new file mode 100644 index 0000000..eaeaac7 --- /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)) + - 1 + { + self.batch_index += 1; + } else { + self.batch_index = 0; + self.reference_phase -= 1 << 32; + } + + (demodulation_initial_phase, demodulation_frequency) + } +} 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, - ); -} 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);