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, - ); -}