From b34c8bb8a1287c7c037a80fb070c3523ccb08557 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 18 Nov 2020 14:55:55 -0800 Subject: [PATCH 01/20] add github CI test workflow --- .github/workflows/ci.yml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1f131c8..253a307 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -77,6 +77,25 @@ jobs: command: build args: --release --features semihosting + test: + runs-on: ubuntu-latest + strategy: + matrix: + toolchain: + - stable + - beta + steps: + - uses: actions/checkout@v2 + - name: Install Rust ${{ matrix.toolchain }} + uses: actions-rs/toolchain@v1 + with: + toolchain: ${{ matrix.toolchain }} + - name: cargo test + uses: actions-rs/cargo@v1 + with: + command: test + args: --manifest-path dsp/Cargo.toml --target=x86_64-unknown-linux-gnu + # Tell bors about it # https://github.com/rtic-rs/cortex-m-rtic/blob/8a4f9c6b8ae91bebeea0791680f89375a78bffc6/.github/workflows/build.yml#L566-L603 ci-success: From 9a83d565ae44697a2e3a452e5946298d794c623b Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 14:32:43 -0800 Subject: [PATCH 02/20] add dsp/target to gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 265b7f5..68b644a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ /target +/dsp/target .gdb_history From 85adc8b1e1e348c6c3fd8c3d2377f5d9e4fdc58e Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 14:34:38 -0800 Subject: [PATCH 03/20] add lockin module --- Cargo.lock | 7 + dsp/Cargo.lock | 7 + dsp/Cargo.toml | 1 + dsp/src/lib.rs | 1 + dsp/src/lockin.rs | 534 ++++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 550 insertions(+) create mode 100644 dsp/src/lockin.rs diff --git a/Cargo.lock b/Cargo.lock index c804d71..080623c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -189,6 +189,7 @@ dependencies = [ name = "dsp" version = "0.1.0" dependencies = [ + "libm", "serde", ] @@ -297,6 +298,12 @@ dependencies = [ "hashbrown", ] +[[package]] +name = "libm" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a" + [[package]] name = "log" version = "0.4.11" diff --git a/dsp/Cargo.lock b/dsp/Cargo.lock index afad0c4..cda08c3 100644 --- a/dsp/Cargo.lock +++ b/dsp/Cargo.lock @@ -4,9 +4,16 @@ name = "dsp" version = "0.1.0" dependencies = [ + "libm", "serde", ] +[[package]] +name = "libm" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a" + [[package]] name = "proc-macro2" version = "1.0.24" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index c8ef52b..2d0c15c 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -5,6 +5,7 @@ authors = ["Robert Jördens "] edition = "2018" [dependencies] +libm = "0.2.1" serde = { version = "1.0", features = ["derive"], default-features = false } [features] diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 9b9e966..ffb021c 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -2,4 +2,5 @@ #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] pub mod iir; +pub mod lockin; pub mod pll; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs new file mode 100644 index 0000000..3645754 --- /dev/null +++ b/dsp/src/lockin.rs @@ -0,0 +1,534 @@ +//! 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 the in-phase +//! and quadrature signals. These processing blocks are mutually +//! independent. +//! +//! # Terminology +//! +//! * _demodulation signal_ - A copy of the reference signal that is +//! optionally frequency scaled and phase shifted. There are two +//! copies of this signal. The first copy is in-phase with the +//! reference signal (before any optional phase shifting). The second +//! is 90 degrees out of phase (in quadrature) with the first +//! copy. The demodulation signals are used to demodulate the ADC +//! sampled signal. +//! * _in-phase_ and _quadrature_ - These terms are used to delineate +//! between the two components of the demodulation signal and the +//! resulting two signals at any step downstream of the demodulation +//! step. The in-phase signal is in-phase with the reference signal +//! prior to any phase shifts. The quadrature signal is 90 degrees out +//! of phase with the in-phase 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 +//! in-phase and quadrature demodulation signals, and multiplies these +//! demodulation signals 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 in-phase and +//! quadrature signals. This is commonly performed on the in-phase and +//! quadrature components 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 the in-phase and quadrature signals +//! 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 core::f32::consts::PI; + +/// The number of ADC samples in one batch. +pub const ADC_SAMPLE_BUFFER_SIZE: usize = 16; +/// The maximum number of timestamps in the period for one ADC +/// batch. Each timestamp corresponds to the time of an external +/// reference clock edge. +pub const TIMESTAMP_BUFFER_SIZE: usize = ADC_SAMPLE_BUFFER_SIZE / 2; +/// The number of outputs sent to the DAC for each ADC batch. +pub const DECIMATED_BUFFER_SIZE: usize = 1; + +/// Performs lock-in amplifier processing of a signal. +pub struct Lockin { + phase_offset: f32, + sample_period: u32, + harmonic: u32, + timestamps: [Option; 2], + iir: [IIR; 2], + 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. Two identical copies of this IIR + /// filter are used: one for the in-phase signal and the other for + /// the quadrature signal. + /// + /// # 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, iir], + iirstate: [[0.; 5]; 2], + } + } + + /// Demodulate an input signal with in-phase and quadrature + /// reference signals. + /// + /// # 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. + /// * `valid_timestamps` - The number of valid timestamps in + /// `timestamps`. Only `×tamps[..valid_timestamps]` are used; + /// every other value in the `timestamps` array is ignored. + /// + /// # Returns + /// + /// The demodulated in-phase and quadrature signals as an + /// `Option`. When there are an insufficient number of timestamps + /// to perform processing, `None` 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; ADC_SAMPLE_BUFFER_SIZE], + timestamps: [u16; TIMESTAMP_BUFFER_SIZE], + valid_timestamps: u16, + ) -> Option<([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE])> + { + // update old timestamps for new ADC batch + let sample_period = self.sample_period as i32; + self.timestamps.iter_mut().for_each(|t| match *t { + Some(i) => { + *t = Some(i - ADC_SAMPLE_BUFFER_SIZE as i32 * sample_period); + } + None => (), + }); + + // record new timestamps + timestamps + .iter() + .take(valid_timestamps as usize) + .rev() + .take(2) + .rev() + .for_each(|t| self.timestamps.push(Some(*t as i32))); + + // return prematurely if there aren't enough timestamps for + // processing + if self.timestamps.iter().filter(|t| t.is_some()).count() < 2 { + return None; + } + + // compute ADC sample phases, sines/cosines and demodulate + let reference_period = + self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); + let mut in_phase = [0f32; ADC_SAMPLE_BUFFER_SIZE]; + let mut quadrature = [0f32; ADC_SAMPLE_BUFFER_SIZE]; + in_phase + .iter_mut() + .zip(quadrature.iter_mut()) + .zip(adc_samples.iter()) + .enumerate() + .for_each(|(n, ((i, q), sample))| { + let integer_phase: i32 = (n as i32 * self.sample_period as i32 + - self.timestamps[0].unwrap()) + * 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; + *i = sine * sample; + *q = cosine * sample; + }); + + Some((in_phase, quadrature)) + } + + /// Filter the in-phase and quadrature signals using the supplied + /// biquad IIR. The signal arrays are modified in place. + /// + /// # Arguments + /// + /// * `in_phase` - In-phase signal. + /// * `quadrature` - Quadrature signal. + pub fn filter(&mut self, in_phase: &mut [f32], quadrature: &mut [f32]) { + in_phase + .iter_mut() + .zip(quadrature.iter_mut()) + .for_each(|(i, q)| { + *i = self.iir[0].update(&mut self.iirstate[0], *i); + *q = self.iir[1].update(&mut self.iirstate[1], *q); + }); + } +} + +/// Decimate the in-phase and quadrature signals to +/// `DECIMATED_BUFFER_SIZE`. The ratio of `ADC_SAMPLE_BUFFER_SIZE` to +/// `DECIMATED_BUFFER_SIZE` must be a power of 2. +/// +/// # Arguments +/// +/// * `in_phase` - In-phase signal. +/// * `quadrature` - Quadrature signal. +/// +/// # Returns +/// +/// The decimated in-phase and quadrature signals. +pub fn decimate( + in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE], + quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE], +) -> ([f32; DECIMATED_BUFFER_SIZE], [f32; 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 in_phase_decimated = [0f32; DECIMATED_BUFFER_SIZE]; + let mut quadrature_decimated = [0f32; DECIMATED_BUFFER_SIZE]; + + in_phase_decimated + .iter_mut() + .zip(quadrature_decimated.iter_mut()) + .zip(in_phase.iter().step_by(n_k)) + .zip(quadrature.iter().step_by(n_k)) + .for_each(|(((i_decimated, q_decimated), i_original), q_original)| { + *i_decimated = *i_original; + *q_decimated = *q_original; + }); + + (in_phase_decimated, quadrature_decimated) +} + +/// Compute the magnitude and phase from the in-phase and quadrature +/// signals. The in-phase and quadrature arrays are modified in place. +/// +/// # Arguments +/// +/// * `in_phase` - In-phase signal. +/// * `quadrature` - Quadrature signal. +pub fn magnitude_phase(in_phase: &mut [f32], quadrature: &mut [f32]) { + in_phase + .iter_mut() + .zip(quadrature.iter_mut()) + .for_each(|(i, q)| { + let new_i = libm::sqrtf([*i, *q].iter().map(|i| i * i).sum()); + let new_q = libm::atan2f(*q, *i); + *i = new_i; + *q = new_q; + }); +} + +/// 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; + } +} + +#[cfg(test)] +mod tests { + use super::*; + extern crate std; + + fn f32_is_close(a: f32, b: f32) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON + } + + fn f32_array_is_close(a: &[f32], b: &[f32]) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= f32_is_close(*i, *j); + }); + result + } + + fn within_tolerance( + a: f32, + b: f32, + relative_tolerance: f32, + fixed_tolerance: f32, + ) -> bool { + (a - b).abs() + <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance + } + + fn array_within_tolerance( + a: &[f32], + b: &[f32], + relative_tolerance: f32, + fixed_tolerance: f32, + ) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= + within_tolerance(*i, *j, relative_tolerance, fixed_tolerance); + }); + result + } + + #[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 in_phase: [f32; 1] = [1.]; + let mut quadrature: [f32; 1] = [1.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); + assert!(f32_array_is_close(&quadrature, &[PI / 4.])); + + in_phase = [3_f32.sqrt() / 2.]; + quadrature = [1. / 2.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32])); + assert!(f32_array_is_close(&quadrature, &[PI / 6.])); + } + + #[test] + fn magnitude_phase_length_1_quadrant_2() { + let mut in_phase: [f32; 1] = [-1.]; + let mut quadrature: [f32; 1] = [1.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); + assert!(f32_array_is_close(&quadrature, &[3. * PI / 4.])); + + in_phase = [-1. / 2.]; + quadrature = [3_f32.sqrt() / 2.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32])); + assert!(f32_array_is_close(&quadrature, &[2. * PI / 3.])); + } + + #[test] + fn magnitude_phase_length_1_quadrant_3() { + let mut in_phase: [f32; 1] = [-1. / 2_f32.sqrt()]; + let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); + assert!(f32_array_is_close(&quadrature, &[-3. * PI / 4.])); + + in_phase = [-1. / 2.]; + quadrature = [-2_f32.sqrt()]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[(3. / 2.) as f32])); + assert!(f32_array_is_close(&quadrature, &[-1.91063323625 as f32])); + } + + #[test] + fn magnitude_phase_length_1_quadrant_4() { + let mut in_phase: [f32; 1] = [1. / 2_f32.sqrt()]; + let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); + assert!(f32_array_is_close(&quadrature, &[-1. * PI / 4.])); + + in_phase = [3_f32.sqrt() / 2.]; + quadrature = [-1. / 2.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32])); + assert!(f32_array_is_close(&quadrature, &[-PI / 6.])); + } + + #[test] + fn decimate_sample_16_decimated_1() { + let in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ + 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, + 1.3, 1.4, 1.5, + ]; + let quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ + 1.6, 1.7, 1.8, 1.9, 2.0, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8, + 2.9, 3.0, 3.1, + ]; + assert_eq!(decimate(in_phase, quadrature), ([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], + [0; TIMESTAMP_BUFFER_SIZE], + 0 + ), + None + ); + } + + #[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; TIMESTAMP_BUFFER_SIZE], + 1 + ), + None + ); + } + + #[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; TIMESTAMP_BUFFER_SIZE] = [ + initial_phase_integer, + initial_phase_integer + reference_period, + 0, + 0, + 0, + 0, + 0, + 0, + ]; + 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 in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = + [0.; ADC_SAMPLE_BUFFER_SIZE]; + let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = + [0.; ADC_SAMPLE_BUFFER_SIZE]; + for (n, (i, q)) in + in_phase.iter_mut().zip(quadrature.iter_mut()).enumerate() + { + let adc_phase = initial_phase + n as f32 * phase_increment; + let sine = adc_phase.sin(); + let cosine = adc_phase.cos(); + *i = sine * adc_samples[n] as f32; + *q = cosine * adc_samples[n] as f32; + } + let (result_in_phase, result_quadrature) = + lockin.demodulate(adc_samples, timestamps, 2).unwrap(); + assert!( + array_within_tolerance(&result_in_phase, &in_phase, 0., 1e-5), + "\nin_phase computed: {:?},\nin_phase expected: {:?}", + result_in_phase, + in_phase + ); + assert!( + array_within_tolerance(&result_quadrature, &quadrature, 0., 1e-5), + "\nquadrature computed: {:?},\nquadrature expected: {:?}", + result_quadrature, + quadrature + ); + } +} From 8ae20009d70f455d50b77c8ddcacc5edda5775f3 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 14:35:23 -0800 Subject: [PATCH 04/20] add lock-in low-pass integration tests --- dsp/tests/lockin_low_pass.rs | 1130 ++++++++++++++++++++++++++++++++++ 1 file changed, 1130 insertions(+) create mode 100644 dsp/tests/lockin_low_pass.rs diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs new file mode 100644 index 0000000..e138c6f --- /dev/null +++ b/dsp/tests/lockin_low_pass.rs @@ -0,0 +1,1130 @@ +use dsp::iir::IIR; +use dsp::lockin::{ + decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, + DECIMATED_BUFFER_SIZE, TIMESTAMP_BUFFER_SIZE, +}; + +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, + timestamp_start: u64, + timestamp_stop: u64, + internal_frequency: f64, +) -> (usize, [u16; TIMESTAMP_BUFFER_SIZE]) { + let reference_period = internal_frequency / reference_frequency; + let start_count = timestamp_start as f64 % reference_period; + let mut valid_timestamps: usize = 0; + let mut timestamps: [u16; TIMESTAMP_BUFFER_SIZE] = + [0; TIMESTAMP_BUFFER_SIZE]; + + 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; + } + + (valid_timestamps, 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 part of the input signal we +/// care about (component 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 and can be chosen to +/// be anywhere in the range [0, 2pi) to maximize this expression. +/// +/// We need to find the demodulation phase (x) that maximizes this +/// expression. We could compute this, because we know I, Q, and n, +/// but that's a fairly expensive computation and probably +/// overkill. Instead, we can employ the heuristic that when |I|>>|Q|, +/// sin(x)=+-1 (+- denotes plus or minus) will maximize the error, +/// when |Q|>>|I|, cos(x)=+-1 will maximize the error and when +/// |I|~|Q|, max,min(sin(x)+cos(x)) will maximize the error (this +/// occurs when sin(x)=cos(x)=+-1/sqrt(2)). Whether a positive or +/// negative noise term maximizes the error depends on the values and +/// signs of I and Q (for instance, when I,Q>0, negative noise terms +/// will maximize the error since the sqrt function is concave down), +/// but the difference should be modest in each case so we should be +/// able to get a reasonably good approximation by using the positive +/// noise case. We can use the maximum of all 3 cases as a rough +/// approximation of the real maximum. +/// +/// # 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 mut max_noise: f64 = 0.; + for (in_phase_delta, quadrature_delta) in [ + (total_noise_amplitude, 0.), + (0., total_noise_amplitude), + ( + total_noise_amplitude / 2_f64.sqrt(), + total_noise_amplitude / 2_f64.sqrt(), + ), + ] + .iter() + { + max_noise = max_noise.max(noise(*in_phase_delta, *quadrature_delta)); + } + + max_noise +} + +/// 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. +/// +/// Similar to the heuristic used when computing the error in +/// `magnitude_noise`, we can use (sin(x)=+-1,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) as cases to test as an approximation +/// for the actual maximum value of this expression. +/// +/// # 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 signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( + &pure_signals, + timestamp_start, + internal_frequency, + adc_frequency, + ); + let (valid_timestamps, timestamps) = adc_batch_timestamps( + reference_frequency, + timestamp_start, + timestamp_start + sample_count - 1, + internal_frequency, + ); + + let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE]; + let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE]; + let lockin_demodulate = + lockin.demodulate(signal, timestamps, valid_timestamps as u16); + match lockin_demodulate { + Some(i) => { + in_phase = i.0; + quadrature = i.1; + } + None => { + continue; + } + } + + lockin.filter(&mut in_phase, &mut quadrature); + let (in_phase_decimated, quadrature_decimated) = + decimate(in_phase, quadrature); + + let mut magnitude_decimated = in_phase_decimated.clone(); + let mut phase_decimated = quadrature_decimated.clone(); + + magnitude_phase(&mut magnitude_decimated, &mut 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_decimated[k] / 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, + phase_decimated[k], + total_phase_noise as f32, + tolerance + ), + "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", + effective_phase_offset as f32, + phase_decimated[k], + max_error( + effective_phase_offset as f32, + total_phase_noise as f32, + tolerance + ) + ); + + let in_phase_normalized: f32 = + in_phase_decimated[k] / ADC_MAX_COUNT as f32; + let quadrature_normalized: f32 = + quadrature_decimated[k] / ADC_MAX_COUNT as f32; + assert!( + tolerance_check( + in_phase_actual as f32, + in_phase_normalized, + total_noise_amplitude as f32, + tolerance + ), + "in-phase actual: {:.4}, in-phase computed: {:.3}, tolerance: {:.4}", + in_phase_actual, + in_phase_normalized, + max_error( + in_phase_actual as f32, + total_noise_amplitude as f32, + tolerance + ) + ); + assert!( + tolerance_check( + quadrature_actual as f32, + quadrature_normalized, + total_noise_amplitude as f32, + tolerance + ), + "quadrature actual: {:.4}, quadrature computed: {:.4}, tolerance: {:.4}", + quadrature_actual, + quadrature_normalized, + max_error( + quadrature_actual as f32, + total_noise_amplitude as f32, + tolerance + ) + ); + } + } + + timestamp_start += sample_count; + } +} + +#[test] +fn lowpass() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_demodulation_phase_offset_pi_2() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = PI / 2.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_phase_offset_pi_2() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 2., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_fundamental_111e3_phase_offset_pi_4() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 111e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 4., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_first_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 2; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_second_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 3; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_third_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 4; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_first_harmonic_phase_shift() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 2; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 4., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_adc_frequency_1e6() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 1e6; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_internal_frequency_125e6() { + let internal_frequency: f64 = 125e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_low_signal_frequency() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 10e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }], + time_constant_factor, + tolerance, + ); +} From 8806feb4237464c7733db6d67bb699e1129cddbc Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 23 Nov 2020 16:16:21 -0800 Subject: [PATCH 05/20] lockin_low_pass: compute magnitude noise analytically --- dsp/tests/lockin_low_pass.rs | 87 ++++++++++++++++++++---------------- 1 file changed, 48 insertions(+), 39 deletions(-) diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index e138c6f..0cc32fd 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -332,30 +332,30 @@ fn sampled_noise_amplitude( /// /// | sqrt((I+n*sin(x))**2 + (Q+n*cos(x))**2) - sqrt(I**2 + Q**2) | /// -/// * I is the in-phase component of the part of the input signal we -/// care about (component of the input signal with the same frequency -/// as the demodulation signal). +/// * 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 and can be chosen to -/// be anywhere in the range [0, 2pi) to maximize this expression. +/// * x is the phase of the demodulation signal. /// /// We need to find the demodulation phase (x) that maximizes this -/// expression. We could compute this, because we know I, Q, and n, -/// but that's a fairly expensive computation and probably -/// overkill. Instead, we can employ the heuristic that when |I|>>|Q|, -/// sin(x)=+-1 (+- denotes plus or minus) will maximize the error, -/// when |Q|>>|I|, cos(x)=+-1 will maximize the error and when -/// |I|~|Q|, max,min(sin(x)+cos(x)) will maximize the error (this -/// occurs when sin(x)=cos(x)=+-1/sqrt(2)). Whether a positive or -/// negative noise term maximizes the error depends on the values and -/// signs of I and Q (for instance, when I,Q>0, negative noise terms -/// will maximize the error since the sqrt function is concave down), -/// but the difference should be modest in each case so we should be -/// able to get a reasonably good approximation by using the positive -/// noise case. We can use the maximum of all 3 cases as a rough -/// approximation of the real maximum. +/// 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 /// @@ -388,21 +388,17 @@ fn magnitude_noise( .abs() }; - let mut max_noise: f64 = 0.; - for (in_phase_delta, quadrature_delta) in [ - (total_noise_amplitude, 0.), - (0., total_noise_amplitude), - ( - total_noise_amplitude / 2_f64.sqrt(), - total_noise_amplitude / 2_f64.sqrt(), - ), - ] - .iter() - { - max_noise = max_noise.max(noise(*in_phase_delta, *quadrature_delta)); - } + 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 + max_noise_1.max(max_noise_2) } /// Compute the maximum phase deviation from the correct value due to @@ -415,12 +411,25 @@ fn magnitude_noise( /// See `magnitude_noise` for an explanation of the terms in this /// mathematical expression. /// -/// Similar to the heuristic used when computing the error in -/// `magnitude_noise`, we can use (sin(x)=+-1,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) as cases to test as an approximation -/// for the actual maximum value of this 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 /// From 3c4e83bf0ffecf0ae93dcc21987280f76ab8ff0b Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:30:57 -0800 Subject: [PATCH 06/20] lockin: move fifo trait before use This clarifies what it means to "push" to an array. --- dsp/src/lockin.rs | 52 +++++++++++++++++++++++------------------------ 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 3645754..8b7e51d 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -74,6 +74,32 @@ pub const TIMESTAMP_BUFFER_SIZE: usize = ADC_SAMPLE_BUFFER_SIZE / 2; /// 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, @@ -274,32 +300,6 @@ pub fn magnitude_phase(in_phase: &mut [f32], quadrature: &mut [f32]) { }); } -/// 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; - } -} - #[cfg(test)] mod tests { use super::*; From da4430e912bda5c9fba3973679aa2a48e1d64330 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:43:21 -0800 Subject: [PATCH 07/20] lockin: add documentation explaining timestamp decrement --- dsp/src/lockin.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 8b7e51d..3aabf33 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -180,8 +180,12 @@ impl Lockin { // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; self.timestamps.iter_mut().for_each(|t| match *t { - Some(i) => { - *t = Some(i - ADC_SAMPLE_BUFFER_SIZE as i32 * sample_period); + 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 => (), }); From 4edda09d86a3560a3adaa2676f27abcf4f9e73b3 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:44:39 -0800 Subject: [PATCH 08/20] lockin: change demodulate to return result instead of option --- dsp/src/lockin.rs | 14 ++++++++------ dsp/tests/lockin_low_pass.rs | 8 ++++---- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 3aabf33..e185c31 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -175,8 +175,10 @@ impl Lockin { adc_samples: [i16; ADC_SAMPLE_BUFFER_SIZE], timestamps: [u16; TIMESTAMP_BUFFER_SIZE], valid_timestamps: u16, - ) -> Option<([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE])> - { + ) -> Result< + ([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE]), + &str, + > { // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; self.timestamps.iter_mut().for_each(|t| match *t { @@ -202,7 +204,7 @@ impl Lockin { // return prematurely if there aren't enough timestamps for // processing if self.timestamps.iter().filter(|t| t.is_some()).count() < 2 { - return None; + return Err("insufficient timestamps"); } // compute ADC sample phases, sines/cosines and demodulate @@ -227,7 +229,7 @@ impl Lockin { *q = cosine * sample; }); - Some((in_phase, quadrature)) + Ok((in_phase, quadrature)) } /// Filter the in-phase and quadrature signals using the supplied @@ -448,7 +450,7 @@ mod tests { [0; TIMESTAMP_BUFFER_SIZE], 0 ), - None + Err("insufficient timestamps") ); } @@ -471,7 +473,7 @@ mod tests { [0; TIMESTAMP_BUFFER_SIZE], 1 ), - None + Err("insufficient timestamps") ); } diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index 0cc32fd..1f8bed2 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -601,11 +601,11 @@ fn lowpass_test( let lockin_demodulate = lockin.demodulate(signal, timestamps, valid_timestamps as u16); match lockin_demodulate { - Some(i) => { - in_phase = i.0; - quadrature = i.1; + Ok((i, q)) => { + in_phase = i; + quadrature = q; } - None => { + Err(_) => { continue; } } From 9592bb74a72ed723433a4189b074630bf83a4ffe Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:49:49 -0800 Subject: [PATCH 09/20] lockin: change zip order in decimate for clarity --- dsp/src/lockin.rs | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index e185c31..7f420d2 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -277,9 +277,13 @@ pub fn decimate( in_phase_decimated .iter_mut() .zip(quadrature_decimated.iter_mut()) - .zip(in_phase.iter().step_by(n_k)) - .zip(quadrature.iter().step_by(n_k)) - .for_each(|(((i_decimated, q_decimated), i_original), q_original)| { + .zip( + in_phase + .iter() + .step_by(n_k) + .zip(quadrature.iter().step_by(n_k)), + ) + .for_each(|((i_decimated, q_decimated), (i_original, q_original))| { *i_decimated = *i_original; *q_decimated = *q_original; }); From f259d6cf65ba8e0a3dbcc7e7e18a1390b7418624 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:56:36 -0800 Subject: [PATCH 10/20] lockin: minor variable name changes --- dsp/src/lockin.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 7f420d2..858cd77 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -283,9 +283,9 @@ pub fn decimate( .step_by(n_k) .zip(quadrature.iter().step_by(n_k)), ) - .for_each(|((i_decimated, q_decimated), (i_original, q_original))| { - *i_decimated = *i_original; - *q_decimated = *q_original; + .for_each(|((i_d, q_d), (i, q))| { + *i_d = *i; + *q_d = *q; }); (in_phase_decimated, quadrature_decimated) From 90ef9f1e6a54e5fae2bd222438f12f7dce91d965 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 13:20:42 -0800 Subject: [PATCH 11/20] lockin: borrow adc samples and timestamps as slices --- dsp/src/lockin.rs | 32 +++++++------------------------- dsp/tests/lockin_low_pass.rs | 17 ++++++++--------- 2 files changed, 15 insertions(+), 34 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 858cd77..30d2f8e 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -154,9 +154,6 @@ impl Lockin { /// * `timestamps` - Counter values corresponding to the edges of /// an external reference signal. The counter is incremented by a /// fast internal clock. - /// * `valid_timestamps` - The number of valid timestamps in - /// `timestamps`. Only `×tamps[..valid_timestamps]` are used; - /// every other value in the `timestamps` array is ignored. /// /// # Returns /// @@ -172,9 +169,8 @@ impl Lockin { /// at the end of the ADC batch sampling period. pub fn demodulate( &mut self, - adc_samples: [i16; ADC_SAMPLE_BUFFER_SIZE], - timestamps: [u16; TIMESTAMP_BUFFER_SIZE], - valid_timestamps: u16, + adc_samples: &[i16], + timestamps: &[u16], ) -> Result< ([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE]), &str, @@ -195,7 +191,7 @@ impl Lockin { // record new timestamps timestamps .iter() - .take(valid_timestamps as usize) + .take(timestamps.len()) .rev() .take(2) .rev() @@ -449,11 +445,7 @@ mod tests { }, ); assert_eq!( - lockin.demodulate( - [0; ADC_SAMPLE_BUFFER_SIZE], - [0; TIMESTAMP_BUFFER_SIZE], - 0 - ), + lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[],), Err("insufficient timestamps") ); } @@ -472,11 +464,7 @@ mod tests { }, ); assert_eq!( - lockin.demodulate( - [0; ADC_SAMPLE_BUFFER_SIZE], - [0; TIMESTAMP_BUFFER_SIZE], - 1 - ), + lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[0],), Err("insufficient timestamps") ); } @@ -499,15 +487,9 @@ mod tests { [-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; TIMESTAMP_BUFFER_SIZE] = [ + let timestamps: &[u16] = &[ initial_phase_integer, initial_phase_integer + reference_period, - 0, - 0, - 0, - 0, - 0, - 0, ]; let initial_phase: f32 = -(initial_phase_integer as f32) / reference_period as f32 * 2. * PI; @@ -527,7 +509,7 @@ mod tests { *q = cosine * adc_samples[n] as f32; } let (result_in_phase, result_quadrature) = - lockin.demodulate(adc_samples, timestamps, 2).unwrap(); + lockin.demodulate(&adc_samples, timestamps).unwrap(); assert!( array_within_tolerance(&result_in_phase, &in_phase, 0., 1e-5), "\nin_phase computed: {:?},\nin_phase expected: {:?}", diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index 1f8bed2..af64651 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -1,7 +1,7 @@ use dsp::iir::IIR; use dsp::lockin::{ decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, - DECIMATED_BUFFER_SIZE, TIMESTAMP_BUFFER_SIZE, + DECIMATED_BUFFER_SIZE, }; use std::f64::consts::PI; @@ -159,15 +159,14 @@ fn adc_sampled_signal( /// 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, -) -> (usize, [u16; TIMESTAMP_BUFFER_SIZE]) { +) -> &[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 timestamps: [u16; TIMESTAMP_BUFFER_SIZE] = - [0; TIMESTAMP_BUFFER_SIZE]; let mut timestamp = (reference_period - start_count) % reference_period; while timestamp < (timestamp_stop - timestamp_start) as f64 { @@ -176,7 +175,7 @@ fn adc_batch_timestamps( valid_timestamps += 1; } - (valid_timestamps, timestamps) + ×tamps[..valid_timestamps] } /// Lowpass biquad filter using cutoff and sampling frequencies. @@ -589,8 +588,10 @@ fn lowpass_test( internal_frequency, adc_frequency, ); - let (valid_timestamps, timestamps) = adc_batch_timestamps( + 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, @@ -598,9 +599,7 @@ fn lowpass_test( let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE]; let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE]; - let lockin_demodulate = - lockin.demodulate(signal, timestamps, valid_timestamps as u16); - match lockin_demodulate { + match lockin.demodulate(&signal, timestamps) { Ok((i, q)) => { in_phase = i; quadrature = q; From 785c98f93dd2a964ef74285cde70843d060061e7 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 13:25:49 -0800 Subject: [PATCH 12/20] lockin: remove TIMESTAMP_BUFFER_SIZE constant --- dsp/src/lockin.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 30d2f8e..a8bcce4 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -67,10 +67,6 @@ use core::f32::consts::PI; /// The number of ADC samples in one batch. pub const ADC_SAMPLE_BUFFER_SIZE: usize = 16; -/// The maximum number of timestamps in the period for one ADC -/// batch. Each timestamp corresponds to the time of an external -/// reference clock edge. -pub const TIMESTAMP_BUFFER_SIZE: usize = ADC_SAMPLE_BUFFER_SIZE / 2; /// The number of outputs sent to the DAC for each ADC batch. pub const DECIMATED_BUFFER_SIZE: usize = 1; From fcdfcb0be7e6ac7d9347ddbd235a5e84974e80f4 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 14:21:48 -0800 Subject: [PATCH 13/20] lockin: use single iir instance for both in-phase and quadrature signals --- dsp/src/lockin.rs | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index a8bcce4..321aceb 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -102,7 +102,7 @@ pub struct Lockin { sample_period: u32, harmonic: u32, timestamps: [Option; 2], - iir: [IIR; 2], + iir: IIR, iirstate: [IIRState; 2], } @@ -118,9 +118,7 @@ impl Lockin { /// * `harmonic` - Integer scaling factor used to adjust the /// demodulation frequency. E.g., 2 would demodulate with the /// first harmonic. - /// * `iir` - IIR biquad filter. Two identical copies of this IIR - /// filter are used: one for the in-phase signal and the other for - /// the quadrature signal. + /// * `iir` - IIR biquad filter. /// /// # Returns /// @@ -136,7 +134,7 @@ impl Lockin { sample_period: sample_period, harmonic: harmonic, timestamps: [None, None], - iir: [iir, iir], + iir: iir, iirstate: [[0.; 5]; 2], } } @@ -236,8 +234,8 @@ impl Lockin { .iter_mut() .zip(quadrature.iter_mut()) .for_each(|(i, q)| { - *i = self.iir[0].update(&mut self.iirstate[0], *i); - *q = self.iir[1].update(&mut self.iirstate[1], *q); + *i = self.iir.update(&mut self.iirstate[0], *i); + *q = self.iir.update(&mut self.iirstate[1], *q); }); } } From d1b7efad48e4925b18f3439fe68840b434051413 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 16:04:22 -0800 Subject: [PATCH 14/20] dsp: replace in_phase and quadrature with Complex --- dsp/src/complex.rs | 21 +++ dsp/src/lib.rs | 1 + dsp/src/lockin.rs | 333 +++++++++++++++++------------------ dsp/tests/lockin_low_pass.rs | 35 ++-- 4 files changed, 201 insertions(+), 189 deletions(-) create mode 100644 dsp/src/complex.rs diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs new file mode 100644 index 0000000..3fb998f --- /dev/null +++ b/dsp/src/complex.rs @@ -0,0 +1,21 @@ +use core::cmp::PartialEq; + +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct Complex { + pub re: f32, + pub im: f32, +} + +impl Complex { + pub fn new(re: f32, im: f32) -> Self { + Complex { re: re, im: im } + } + + pub fn arg(&self) -> f32 { + libm::atan2f(self.im, self.re) + } + + pub fn abs(&self) -> f32 { + libm::sqrtf([self.re, self.im].iter().map(|i| i * i).sum()) + } +} diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index ffb021c..41e8a52 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,6 +1,7 @@ #![cfg_attr(not(test), no_std)] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] +pub mod complex; pub mod iir; pub mod lockin; pub mod pll; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 321aceb..4dcfe69 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -2,25 +2,15 @@ //! //! Lock-in processing is performed through a combination of the //! following modular processing blocks: demodulation, filtering, -//! decimation and computing the magnitude and phase from the in-phase -//! and quadrature signals. These processing blocks are mutually -//! independent. +//! 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. There are two -//! copies of this signal. The first copy is in-phase with the -//! reference signal (before any optional phase shifting). The second -//! is 90 degrees out of phase (in quadrature) with the first -//! copy. The demodulation signals are used to demodulate the ADC +//! optionally frequency scaled and phase shifted. This is a complex +//! signal. The demodulation signals are used to demodulate the ADC //! sampled signal. -//! * _in-phase_ and _quadrature_ - These terms are used to delineate -//! between the two components of the demodulation signal and the -//! resulting two signals at any step downstream of the demodulation -//! step. The in-phase signal is in-phase with the reference signal -//! prior to any phase shifts. The quadrature signal is 90 degrees out -//! of phase with the in-phase 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 @@ -44,24 +34,25 @@ //! //! * `demodulate` - Computes the phase of the demodulation signal //! corresponding to each ADC sample, uses this phase to compute the -//! in-phase and quadrature demodulation signals, and multiplies these -//! demodulation signals 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 in-phase and -//! quadrature signals. This is commonly performed on the in-phase and -//! quadrature components 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 the in-phase and quadrature signals -//! to reduce the load on the DAC output. It does not require any -//! state information and is therefore a normal function. +//! 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::complex::Complex; use super::iir::{IIRState, IIR}; use core::f32::consts::PI; @@ -139,8 +130,7 @@ impl Lockin { } } - /// Demodulate an input signal with in-phase and quadrature - /// reference signals. + /// Demodulate an input signal with the complex reference signal. /// /// # Arguments /// @@ -151,9 +141,9 @@ impl Lockin { /// /// # Returns /// - /// The demodulated in-phase and quadrature signals as an - /// `Option`. When there are an insufficient number of timestamps - /// to perform processing, `None` is returned. + /// The demodulated complex signal as a `Result`. When there are + /// an insufficient number of timestamps to perform processing, + /// `Err` is returned. /// /// # Assumptions /// @@ -165,10 +155,7 @@ impl Lockin { &mut self, adc_samples: &[i16], timestamps: &[u16], - ) -> Result< - ([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE]), - &str, - > { + ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; self.timestamps.iter_mut().for_each(|t| match *t { @@ -200,14 +187,12 @@ impl Lockin { // compute ADC sample phases, sines/cosines and demodulate let reference_period = self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); - let mut in_phase = [0f32; ADC_SAMPLE_BUFFER_SIZE]; - let mut quadrature = [0f32; ADC_SAMPLE_BUFFER_SIZE]; - in_phase + let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + signal .iter_mut() - .zip(quadrature.iter_mut()) .zip(adc_samples.iter()) .enumerate() - .for_each(|(n, ((i, q), sample))| { + .for_each(|(n, (s, sample))| { let integer_phase: i32 = (n as i32 * self.sample_period as i32 - self.timestamps[0].unwrap()) * self.harmonic as i32; @@ -215,104 +200,90 @@ impl Lockin { + 2. * PI * integer_phase as f32 / reference_period as f32; let (sine, cosine) = libm::sincosf(phase); let sample = *sample as f32; - *i = sine * sample; - *q = cosine * sample; + s.re = sine * sample; + s.im = cosine * sample; }); - Ok((in_phase, quadrature)) + Ok(signal) } - /// Filter the in-phase and quadrature signals using the supplied - /// biquad IIR. The signal arrays are modified in place. + /// Filter the complex signal using the supplied biquad IIR. The + /// signal array is modified in place. /// /// # Arguments /// - /// * `in_phase` - In-phase signal. - /// * `quadrature` - Quadrature signal. - pub fn filter(&mut self, in_phase: &mut [f32], quadrature: &mut [f32]) { - in_phase - .iter_mut() - .zip(quadrature.iter_mut()) - .for_each(|(i, q)| { - *i = self.iir.update(&mut self.iirstate[0], *i); - *q = self.iir.update(&mut self.iirstate[1], *q); - }); + /// * `signal` - Complex signal to filter. + pub fn filter(&mut self, signal: &mut [Complex]) { + signal.iter_mut().for_each(|s| { + s.re = self.iir.update(&mut self.iirstate[0], s.re); + s.im = self.iir.update(&mut self.iirstate[1], s.im); + }); } } -/// Decimate the in-phase and quadrature signals to -/// `DECIMATED_BUFFER_SIZE`. The ratio of `ADC_SAMPLE_BUFFER_SIZE` to -/// `DECIMATED_BUFFER_SIZE` must be a power of 2. +/// 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 /// -/// * `in_phase` - In-phase signal. -/// * `quadrature` - Quadrature signal. +/// * `signal` - Complex signal to decimate. /// /// # Returns /// -/// The decimated in-phase and quadrature signals. +/// The decimated signal. pub fn decimate( - in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE], - quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE], -) -> ([f32; DECIMATED_BUFFER_SIZE], [f32; DECIMATED_BUFFER_SIZE]) { + 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 in_phase_decimated = [0f32; DECIMATED_BUFFER_SIZE]; - let mut quadrature_decimated = [0f32; DECIMATED_BUFFER_SIZE]; + let mut signal_decimated = [Complex::new(0., 0.); DECIMATED_BUFFER_SIZE]; - in_phase_decimated + signal_decimated .iter_mut() - .zip(quadrature_decimated.iter_mut()) - .zip( - in_phase - .iter() - .step_by(n_k) - .zip(quadrature.iter().step_by(n_k)), - ) - .for_each(|((i_d, q_d), (i, q))| { - *i_d = *i; - *q_d = *q; + .zip(signal.iter().step_by(n_k)) + .for_each(|(s_d, s)| { + s_d.re = s.re; + s_d.im = s.im; }); - (in_phase_decimated, quadrature_decimated) + signal_decimated } -/// Compute the magnitude and phase from the in-phase and quadrature -/// signals. The in-phase and quadrature arrays are modified in place. +/// Compute the magnitude and phase from the complex signal. The +/// signal array is modified in place. /// /// # Arguments /// -/// * `in_phase` - In-phase signal. -/// * `quadrature` - Quadrature signal. -pub fn magnitude_phase(in_phase: &mut [f32], quadrature: &mut [f32]) { - in_phase - .iter_mut() - .zip(quadrature.iter_mut()) - .for_each(|(i, q)| { - let new_i = libm::sqrtf([*i, *q].iter().map(|i| i * i).sum()); - let new_q = libm::atan2f(*q, *i); - *i = new_i; - *q = new_q; - }); +/// * `signal` - Complex signal to decimate. +pub fn magnitude_phase(signal: &mut [Complex]) { + signal.iter_mut().for_each(|s| { + let new_i = s.abs(); + let new_q = s.arg(); + s.re = new_i; + s.im = new_q; + }); } #[cfg(test)] mod tests { use super::*; - extern crate std; fn f32_is_close(a: f32, b: f32) -> bool { (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON } - fn f32_array_is_close(a: &[f32], b: &[f32]) -> bool { + fn complex_is_close(a: Complex, b: Complex) -> bool { + f32_is_close(a.re, b.re) && f32_is_close(a.im, b.im) + } + + fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { let mut result: bool = true; a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= f32_is_close(*i, *j); + result &= complex_is_close(*i, *j); }); result } @@ -327,16 +298,30 @@ mod tests { <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance } - fn array_within_tolerance( - a: &[f32], - b: &[f32], + fn complex_within_tolerance( + a: Complex, + b: Complex, + relative_tolerance: f32, + fixed_tolerance: f32, + ) -> bool { + within_tolerance(a.re, b.re, relative_tolerance, fixed_tolerance) + && within_tolerance(a.im, b.im, relative_tolerance, fixed_tolerance) + } + + fn complex_array_within_tolerance( + a: &[Complex], + b: &[Complex], relative_tolerance: f32, fixed_tolerance: f32, ) -> bool { let mut result: bool = true; a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= - within_tolerance(*i, *j, relative_tolerance, fixed_tolerance); + result &= complex_within_tolerance( + *i, + *j, + relative_tolerance, + fixed_tolerance, + ); }); result } @@ -354,75 +339,93 @@ mod tests { #[test] fn magnitude_phase_length_1_quadrant_1() { - let mut in_phase: [f32; 1] = [1.]; - let mut quadrature: [f32; 1] = [1.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); - assert!(f32_array_is_close(&quadrature, &[PI / 4.])); + let mut signal: [Complex; 1] = [Complex::new(1., 1.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(2_f32.sqrt(), PI / 4.)] + )); - in_phase = [3_f32.sqrt() / 2.]; - quadrature = [1. / 2.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32])); - assert!(f32_array_is_close(&quadrature, &[PI / 6.])); + signal = [Complex::new(3_f32.sqrt() / 2., 1. / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1., PI / 6.)] + )); } #[test] fn magnitude_phase_length_1_quadrant_2() { - let mut in_phase: [f32; 1] = [-1.]; - let mut quadrature: [f32; 1] = [1.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); - assert!(f32_array_is_close(&quadrature, &[3. * PI / 4.])); + let mut signal = [Complex::new(-1., 1.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(2_f32.sqrt(), 3. * PI / 4.)] + )); - in_phase = [-1. / 2.]; - quadrature = [3_f32.sqrt() / 2.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32])); - assert!(f32_array_is_close(&quadrature, &[2. * PI / 3.])); + signal = [Complex::new(-1. / 2., 3_f32.sqrt() / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1_f32, 2. * PI / 3.)] + )); } #[test] fn magnitude_phase_length_1_quadrant_3() { - let mut in_phase: [f32; 1] = [-1. / 2_f32.sqrt()]; - let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); - assert!(f32_array_is_close(&quadrature, &[-3. * PI / 4.])); + let mut signal = [Complex::new(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1_f32.sqrt(), -3. * PI / 4.)] + )); - in_phase = [-1. / 2.]; - quadrature = [-2_f32.sqrt()]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[(3. / 2.) as f32])); - assert!(f32_array_is_close(&quadrature, &[-1.91063323625 as f32])); + signal = [Complex::new(-1. / 2., -2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new((3. / 2.) as f32, -1.91063323625 as f32)] + )); } #[test] fn magnitude_phase_length_1_quadrant_4() { - let mut in_phase: [f32; 1] = [1. / 2_f32.sqrt()]; - let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); - assert!(f32_array_is_close(&quadrature, &[-1. * PI / 4.])); + let mut signal = [Complex::new(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1_f32.sqrt(), -1. * PI / 4.)] + )); - in_phase = [3_f32.sqrt() / 2.]; - quadrature = [-1. / 2.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32])); - assert!(f32_array_is_close(&quadrature, &[-PI / 6.])); + signal = [Complex::new(3_f32.sqrt() / 2., -1. / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1_f32, -PI / 6.)] + )); } #[test] fn decimate_sample_16_decimated_1() { - let in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ - 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, - 1.3, 1.4, 1.5, + let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [ + Complex::new(0.0, 1.6), + Complex::new(0.1, 1.7), + Complex::new(0.2, 1.8), + Complex::new(0.3, 1.9), + Complex::new(0.4, 2.0), + Complex::new(0.5, 2.1), + Complex::new(0.6, 2.2), + Complex::new(0.7, 2.3), + Complex::new(0.8, 2.4), + Complex::new(0.9, 2.5), + Complex::new(1.0, 2.6), + Complex::new(1.1, 2.7), + Complex::new(1.2, 2.8), + Complex::new(1.3, 2.9), + Complex::new(1.4, 3.0), + Complex::new(1.5, 3.1), ]; - let quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ - 1.6, 1.7, 1.8, 1.9, 2.0, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8, - 2.9, 3.0, 3.1, - ]; - assert_eq!(decimate(in_phase, quadrature), ([0.0], [1.6])); + assert_eq!(decimate(signal), [Complex::new(0.0, 1.6)]); } #[test] @@ -439,7 +442,7 @@ mod tests { }, ); assert_eq!( - lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[],), + lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[]), Err("insufficient timestamps") ); } @@ -489,32 +492,20 @@ mod tests { -(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 in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = - [0.; ADC_SAMPLE_BUFFER_SIZE]; - let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = - [0.; ADC_SAMPLE_BUFFER_SIZE]; - for (n, (i, q)) in - in_phase.iter_mut().zip(quadrature.iter_mut()).enumerate() - { + let mut signal = [Complex::new(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(); - *i = sine * adc_samples[n] as f32; - *q = cosine * adc_samples[n] as f32; + s.re = sine * adc_samples[n] as f32; + s.im = cosine * adc_samples[n] as f32; } - let (result_in_phase, result_quadrature) = - lockin.demodulate(&adc_samples, timestamps).unwrap(); + let result = lockin.demodulate(&adc_samples, timestamps).unwrap(); assert!( - array_within_tolerance(&result_in_phase, &in_phase, 0., 1e-5), - "\nin_phase computed: {:?},\nin_phase expected: {:?}", - result_in_phase, - in_phase - ); - assert!( - array_within_tolerance(&result_quadrature, &quadrature, 0., 1e-5), - "\nquadrature computed: {:?},\nquadrature expected: {:?}", - result_quadrature, - quadrature + complex_array_within_tolerance(&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 index af64651..21ace3e 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -1,3 +1,4 @@ +use dsp::complex::Complex; use dsp::iir::IIR; use dsp::lockin::{ decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, @@ -582,7 +583,7 @@ fn lowpass_test( pure_signals.push(desired_input); for n in 0..(samples + extra_samples) { - let signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( + let adc_signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( &pure_signals, timestamp_start, internal_frequency, @@ -597,33 +598,31 @@ fn lowpass_test( internal_frequency, ); - let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE]; - let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE]; - match lockin.demodulate(&signal, timestamps) { - Ok((i, q)) => { - in_phase = i; - quadrature = q; + let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE]; + match lockin.demodulate(&adc_signal, timestamps) { + Ok(s) => { + signal = s; } Err(_) => { continue; } } - lockin.filter(&mut in_phase, &mut quadrature); - let (in_phase_decimated, quadrature_decimated) = - decimate(in_phase, quadrature); + lockin.filter(&mut signal); + let signal_decimated = decimate(signal); - let mut magnitude_decimated = in_phase_decimated.clone(); - let mut phase_decimated = quadrature_decimated.clone(); + 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_decimated, &mut phase_decimated); + 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_decimated[k] / ADC_MAX_COUNT as f32; + magnitude_phase_decimated[k].re / 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}", @@ -636,13 +635,13 @@ fn lowpass_test( assert!( tolerance_check( effective_phase_offset as f32, - phase_decimated[k], + magnitude_phase_decimated[k].im, total_phase_noise as f32, tolerance ), "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", effective_phase_offset as f32, - phase_decimated[k], + magnitude_phase_decimated[k].im, max_error( effective_phase_offset as f32, total_phase_noise as f32, @@ -651,9 +650,9 @@ fn lowpass_test( ); let in_phase_normalized: f32 = - in_phase_decimated[k] / ADC_MAX_COUNT as f32; + signal_decimated[k].re / ADC_MAX_COUNT as f32; let quadrature_normalized: f32 = - quadrature_decimated[k] / ADC_MAX_COUNT as f32; + signal_decimated[k].im / ADC_MAX_COUNT as f32; assert!( tolerance_check( in_phase_actual as f32, From 260206e4f0d2d1be8e5fabe253bc81df3699e999 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 16:21:08 -0800 Subject: [PATCH 15/20] dsp: implement Complex as type alias for tuple --- dsp/src/complex.rs | 21 ------ dsp/src/lib.rs | 2 +- dsp/src/lockin.rs | 140 ++++++++++++++++------------------- dsp/tests/lockin_low_pass.rs | 14 ++-- 4 files changed, 72 insertions(+), 105 deletions(-) delete mode 100644 dsp/src/complex.rs diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs deleted file mode 100644 index 3fb998f..0000000 --- a/dsp/src/complex.rs +++ /dev/null @@ -1,21 +0,0 @@ -use core::cmp::PartialEq; - -#[derive(Copy, Clone, Debug, PartialEq)] -pub struct Complex { - pub re: f32, - pub im: f32, -} - -impl Complex { - pub fn new(re: f32, im: f32) -> Self { - Complex { re: re, im: im } - } - - pub fn arg(&self) -> f32 { - libm::atan2f(self.im, self.re) - } - - pub fn abs(&self) -> f32 { - libm::sqrtf([self.re, self.im].iter().map(|i| i * i).sum()) - } -} diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 41e8a52..ca1daec 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,7 +1,7 @@ #![cfg_attr(not(test), no_std)] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] -pub mod complex; +pub type Complex = (T, T); pub mod iir; pub mod lockin; pub mod pll; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 4dcfe69..66b3a9a 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -52,8 +52,8 @@ //! the demodulation frequency. This does not require any state //! information and is therefore a normal function. -use super::complex::Complex; use super::iir::{IIRState, IIR}; +use super::Complex; use core::f32::consts::PI; /// The number of ADC samples in one batch. @@ -155,7 +155,7 @@ impl Lockin { &mut self, adc_samples: &[i16], timestamps: &[u16], - ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { + ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; self.timestamps.iter_mut().for_each(|t| match *t { @@ -187,7 +187,7 @@ impl Lockin { // compute ADC sample phases, sines/cosines and demodulate let reference_period = self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); - let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; signal .iter_mut() .zip(adc_samples.iter()) @@ -200,8 +200,8 @@ impl Lockin { + 2. * PI * integer_phase as f32 / reference_period as f32; let (sine, cosine) = libm::sincosf(phase); let sample = *sample as f32; - s.re = sine * sample; - s.im = cosine * sample; + s.0 = sine * sample; + s.1 = cosine * sample; }); Ok(signal) @@ -213,10 +213,10 @@ 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.re = self.iir.update(&mut self.iirstate[0], s.re); - s.im = self.iir.update(&mut self.iirstate[1], s.im); + s.0 = self.iir.update(&mut self.iirstate[0], s.0); + s.1 = self.iir.update(&mut self.iirstate[1], s.1); }); } } @@ -233,21 +233,21 @@ impl Lockin { /// /// The decimated signal. pub fn decimate( - signal: [Complex; ADC_SAMPLE_BUFFER_SIZE], -) -> [Complex; DECIMATED_BUFFER_SIZE] { + 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 = [Complex::new(0., 0.); DECIMATED_BUFFER_SIZE]; + 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.re = s.re; - s_d.im = s.im; + s_d.0 = s.0; + s_d.1 = s.1; }); signal_decimated @@ -259,12 +259,12 @@ pub fn decimate( /// # Arguments /// /// * `signal` - Complex signal to decimate. -pub fn magnitude_phase(signal: &mut [Complex]) { +pub fn magnitude_phase(signal: &mut [Complex]) { signal.iter_mut().for_each(|s| { - let new_i = s.abs(); - let new_q = s.arg(); - s.re = new_i; - s.im = new_q; + 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; }); } @@ -276,11 +276,11 @@ mod tests { (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON } - fn complex_is_close(a: Complex, b: Complex) -> bool { - f32_is_close(a.re, b.re) && f32_is_close(a.im, b.im) + fn complex_is_close(a: Complex, b: Complex) -> bool { + f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1) } - fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { + fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { let mut result: bool = true; a.iter().zip(b.iter()).for_each(|(i, j)| { result &= complex_is_close(*i, *j); @@ -299,18 +299,18 @@ mod tests { } fn complex_within_tolerance( - a: Complex, - b: Complex, + a: Complex, + b: Complex, relative_tolerance: f32, fixed_tolerance: f32, ) -> bool { - within_tolerance(a.re, b.re, relative_tolerance, fixed_tolerance) - && within_tolerance(a.im, b.im, relative_tolerance, fixed_tolerance) + within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance) + && within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance) } fn complex_array_within_tolerance( - a: &[Complex], - b: &[Complex], + a: &[Complex], + b: &[Complex], relative_tolerance: f32, fixed_tolerance: f32, ) -> bool { @@ -339,93 +339,81 @@ mod tests { #[test] fn magnitude_phase_length_1_quadrant_1() { - let mut signal: [Complex; 1] = [Complex::new(1., 1.)]; + let mut signal: [Complex; 1] = [(1., 1.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( - &signal, - &[Complex::new(2_f32.sqrt(), PI / 4.)] - )); + assert!(complex_array_is_close(&signal, &[(2_f32.sqrt(), PI / 4.)])); - signal = [Complex::new(3_f32.sqrt() / 2., 1. / 2.)]; + signal = [(3_f32.sqrt() / 2., 1. / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( - &signal, - &[Complex::new(1., PI / 6.)] - )); + assert!(complex_array_is_close(&signal, &[(1., PI / 6.)])); } #[test] fn magnitude_phase_length_1_quadrant_2() { - let mut signal = [Complex::new(-1., 1.)]; + let mut signal = [(-1., 1.)]; magnitude_phase(&mut signal); assert!(complex_array_is_close( &signal, - &[Complex::new(2_f32.sqrt(), 3. * PI / 4.)] + &[(2_f32.sqrt(), 3. * PI / 4.)] )); - signal = [Complex::new(-1. / 2., 3_f32.sqrt() / 2.)]; + signal = [(-1. / 2., 3_f32.sqrt() / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( - &signal, - &[Complex::new(1_f32, 2. * PI / 3.)] - )); + assert!(complex_array_is_close(&signal, &[(1_f32, 2. * PI / 3.)])); } #[test] fn magnitude_phase_length_1_quadrant_3() { - let mut signal = [Complex::new(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + let mut signal = [(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; magnitude_phase(&mut signal); assert!(complex_array_is_close( &signal, - &[Complex::new(1_f32.sqrt(), -3. * PI / 4.)] + &[(1_f32.sqrt(), -3. * PI / 4.)] )); - signal = [Complex::new(-1. / 2., -2_f32.sqrt())]; + signal = [(-1. / 2., -2_f32.sqrt())]; magnitude_phase(&mut signal); assert!(complex_array_is_close( &signal, - &[Complex::new((3. / 2.) as f32, -1.91063323625 as f32)] + &[((3. / 2.) as f32, -1.91063323625 as f32)] )); } #[test] fn magnitude_phase_length_1_quadrant_4() { - let mut signal = [Complex::new(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + let mut signal = [(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; magnitude_phase(&mut signal); assert!(complex_array_is_close( &signal, - &[Complex::new(1_f32.sqrt(), -1. * PI / 4.)] + &[(1_f32.sqrt(), -1. * PI / 4.)] )); - signal = [Complex::new(3_f32.sqrt() / 2., -1. / 2.)]; + signal = [(3_f32.sqrt() / 2., -1. / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( - &signal, - &[Complex::new(1_f32, -PI / 6.)] - )); + assert!(complex_array_is_close(&signal, &[(1_f32, -PI / 6.)])); } #[test] fn decimate_sample_16_decimated_1() { - let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [ - Complex::new(0.0, 1.6), - Complex::new(0.1, 1.7), - Complex::new(0.2, 1.8), - Complex::new(0.3, 1.9), - Complex::new(0.4, 2.0), - Complex::new(0.5, 2.1), - Complex::new(0.6, 2.2), - Complex::new(0.7, 2.3), - Complex::new(0.8, 2.4), - Complex::new(0.9, 2.5), - Complex::new(1.0, 2.6), - Complex::new(1.1, 2.7), - Complex::new(1.2, 2.8), - Complex::new(1.3, 2.9), - Complex::new(1.4, 3.0), - Complex::new(1.5, 3.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), [Complex::new(0.0, 1.6)]); + assert_eq!(decimate(signal), [(0.0, 1.6)]); } #[test] @@ -492,13 +480,13 @@ mod tests { -(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 = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + 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.re = sine * adc_samples[n] as f32; - s.im = cosine * adc_samples[n] as f32; + 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!( diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index 21ace3e..37ab2b0 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -1,9 +1,9 @@ -use dsp::complex::Complex; 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; @@ -598,7 +598,7 @@ fn lowpass_test( internal_frequency, ); - let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE]; + let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE]; match lockin.demodulate(&adc_signal, timestamps) { Ok(s) => { signal = s; @@ -622,7 +622,7 @@ fn lowpass_test( if n >= samples { for k in 0..DECIMATED_BUFFER_SIZE { let amplitude_normalized: f32 = - magnitude_phase_decimated[k].re / ADC_MAX_COUNT as 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}", @@ -635,13 +635,13 @@ fn lowpass_test( assert!( tolerance_check( effective_phase_offset as f32, - magnitude_phase_decimated[k].im, + 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].im, + magnitude_phase_decimated[k].1, max_error( effective_phase_offset as f32, total_phase_noise as f32, @@ -650,9 +650,9 @@ fn lowpass_test( ); let in_phase_normalized: f32 = - signal_decimated[k].re / ADC_MAX_COUNT as f32; + signal_decimated[k].0 / ADC_MAX_COUNT as f32; let quadrature_normalized: f32 = - signal_decimated[k].im / ADC_MAX_COUNT as f32; + signal_decimated[k].1 / ADC_MAX_COUNT as f32; assert!( tolerance_check( in_phase_actual as f32, From 277a5d2d812157e1d71b5cb96afe3a1db978a898 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 16:41:16 -0800 Subject: [PATCH 16/20] dsp: move common test code to testing.rs file --- dsp/src/lib.rs | 3 +++ dsp/src/lockin.rs | 57 +++------------------------------------------- dsp/src/testing.rs | 54 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 60 insertions(+), 54 deletions(-) create mode 100644 dsp/src/testing.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index ca1daec..389bda5 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -5,3 +5,6 @@ pub type Complex = (T, T); pub mod iir; pub mod lockin; pub mod pll; + +#[cfg(test)] +mod testing; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 66b3a9a..6b6017b 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -271,60 +271,9 @@ pub fn magnitude_phase(signal: &mut [Complex]) { #[cfg(test)] mod tests { use super::*; - - fn f32_is_close(a: f32, b: f32) -> bool { - (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON - } - - fn complex_is_close(a: Complex, b: Complex) -> bool { - f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1) - } - - fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { - let mut result: bool = true; - a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_is_close(*i, *j); - }); - result - } - - fn within_tolerance( - a: f32, - b: f32, - relative_tolerance: f32, - fixed_tolerance: f32, - ) -> bool { - (a - b).abs() - <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance - } - - fn complex_within_tolerance( - a: Complex, - b: Complex, - relative_tolerance: f32, - fixed_tolerance: f32, - ) -> bool { - within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance) - && within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance) - } - - fn complex_array_within_tolerance( - a: &[Complex], - b: &[Complex], - relative_tolerance: f32, - fixed_tolerance: f32, - ) -> bool { - let mut result: bool = true; - a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_within_tolerance( - *i, - *j, - relative_tolerance, - fixed_tolerance, - ); - }); - result - } + use crate::testing::{ + complex_array_is_close, complex_array_within_tolerance, + }; #[test] fn array_push() { diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs new file mode 100644 index 0000000..0fe9e4f --- /dev/null +++ b/dsp/src/testing.rs @@ -0,0 +1,54 @@ +use super::Complex; + +pub fn f32_is_close(a: f32, b: f32) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON +} + +pub fn complex_is_close(a: Complex, b: Complex) -> bool { + f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1) +} + +pub fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= complex_is_close(*i, *j); + }); + result +} + +pub fn within_tolerance( + a: f32, + b: f32, + relative_tolerance: f32, + fixed_tolerance: f32, +) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance +} + +pub fn complex_within_tolerance( + a: Complex, + b: Complex, + relative_tolerance: f32, + fixed_tolerance: f32, +) -> bool { + within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance) + && within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance) +} + +pub fn complex_array_within_tolerance( + a: &[Complex], + b: &[Complex], + relative_tolerance: f32, + fixed_tolerance: f32, +) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= complex_within_tolerance( + *i, + *j, + relative_tolerance, + fixed_tolerance, + ); + }); + result +} From 0cca4589fde0bed505113219f44a9c5723ecae90 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 30 Nov 2020 11:37:59 -0800 Subject: [PATCH 17/20] lockin: compute reference period from the closest 2 timestamps to the ADC sample --- dsp/src/lockin.rs | 78 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 61 insertions(+), 17 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 6b6017b..03e7334 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -156,8 +156,8 @@ impl Lockin { adc_samples: &[i16], timestamps: &[u16], ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { - // update old timestamps for new ADC batch 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 @@ -169,32 +169,66 @@ impl Lockin { None => (), }); - // record new timestamps - timestamps - .iter() - .take(timestamps.len()) - .rev() - .take(2) - .rev() - .for_each(|t| self.timestamps.push(Some(*t as i32))); - // return prematurely if there aren't enough timestamps for // processing - if self.timestamps.iter().filter(|t| t.is_some()).count() < 2 { + let old_timestamp_count = + self.timestamps.iter().filter(|t| t.is_some()).count(); + if old_timestamp_count + timestamps.len() < 2 { return Err("insufficient timestamps"); } - // compute ADC sample phases, sines/cosines and demodulate - let reference_period = - self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); 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(|(n, (s, sample))| { - let integer_phase: i32 = (n as i32 * self.sample_period as i32 - - self.timestamps[0].unwrap()) + .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 to 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; @@ -204,6 +238,16 @@ impl Lockin { 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) } From 55e7f1f0db74761626eb6c57501407773dd4a9e7 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 30 Nov 2020 12:34:20 -0800 Subject: [PATCH 18/20] dsp: fix small comment grammar error --- dsp/src/lockin.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 03e7334..d4e635d 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -210,8 +210,8 @@ impl Lockin { -1 }; - // closest timestamp that occurred before to the - // current ADC sample + // 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(); From 1b02f558f660cbe4d9df0ae144b08907e06e9333 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Fri, 4 Dec 2020 08:13:02 -0800 Subject: [PATCH 19/20] CI: specify test location using --package dsp --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 253a307..6c7b0e9 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -94,7 +94,7 @@ jobs: uses: actions-rs/cargo@v1 with: command: test - args: --manifest-path dsp/Cargo.toml --target=x86_64-unknown-linux-gnu + args: --package dsp --target=x86_64-unknown-linux-gnu # Tell bors about it # https://github.com/rtic-rs/cortex-m-rtic/blob/8a4f9c6b8ae91bebeea0791680f89375a78bffc6/.github/workflows/build.yml#L566-L603 From 43a760e57a6b65d615de51513a4d9c92a50b8638 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Fri, 4 Dec 2020 09:09:23 -0800 Subject: [PATCH 20/20] dsp testing: improve api for tolerance checking --- dsp/src/lockin.rs | 58 +++++++++++++++++++++++++++++++++------------- dsp/src/testing.rs | 47 ++++++++----------------------------- 2 files changed, 52 insertions(+), 53 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index d4e635d..bb649ad 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -315,9 +315,7 @@ pub fn magnitude_phase(signal: &mut [Complex]) { #[cfg(test)] mod tests { use super::*; - use crate::testing::{ - complex_array_is_close, complex_array_within_tolerance, - }; + use crate::testing::complex_allclose; #[test] fn array_push() { @@ -334,41 +332,62 @@ mod tests { fn magnitude_phase_length_1_quadrant_1() { let mut signal: [Complex; 1] = [(1., 1.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close(&signal, &[(2_f32.sqrt(), PI / 4.)])); + 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_array_is_close(&signal, &[(1., PI / 6.)])); + 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_array_is_close( + assert!(complex_allclose( &signal, - &[(2_f32.sqrt(), 3. * PI / 4.)] + &[(2_f32.sqrt(), 3. * PI / 4.)], + f32::EPSILON, + 0. )); signal = [(-1. / 2., 3_f32.sqrt() / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close(&signal, &[(1_f32, 2. * PI / 3.)])); + 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_array_is_close( + assert!(complex_allclose( &signal, - &[(1_f32.sqrt(), -3. * PI / 4.)] + &[(1_f32.sqrt(), -3. * PI / 4.)], + f32::EPSILON, + 0. )); signal = [(-1. / 2., -2_f32.sqrt())]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( + assert!(complex_allclose( &signal, - &[((3. / 2.) as f32, -1.91063323625 as f32)] + &[((3. / 2.) as f32, -1.91063323625 as f32)], + f32::EPSILON, + 0. )); } @@ -376,14 +395,21 @@ mod tests { fn magnitude_phase_length_1_quadrant_4() { let mut signal = [(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( + assert!(complex_allclose( &signal, - &[(1_f32.sqrt(), -1. * PI / 4.)] + &[(1_f32.sqrt(), -1. * PI / 4.)], + f32::EPSILON, + 0. )); signal = [(3_f32.sqrt() / 2., -1. / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close(&signal, &[(1_f32, -PI / 6.)])); + assert!(complex_allclose( + &signal, + &[(1_f32, -PI / 6.)], + f32::EPSILON, + 0. + )); } #[test] @@ -483,7 +509,7 @@ mod tests { } let result = lockin.demodulate(&adc_samples, timestamps).unwrap(); assert!( - complex_array_within_tolerance(&result, &signal, 0., 1e-5), + complex_allclose(&result, &signal, 0., 1e-5), "\nsignal computed: {:?},\nsignal expected: {:?}", result, signal diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs index 0fe9e4f..1a8e109 100644 --- a/dsp/src/testing.rs +++ b/dsp/src/testing.rs @@ -1,54 +1,27 @@ use super::Complex; -pub fn f32_is_close(a: f32, b: f32) -> bool { - (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON +pub fn isclose(a: f32, b: f32, rtol: f32, atol: f32) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * rtol + atol } -pub fn complex_is_close(a: Complex, b: Complex) -> bool { - f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1) -} - -pub fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { - let mut result: bool = true; - a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_is_close(*i, *j); - }); - result -} - -pub fn within_tolerance( - a: f32, - b: f32, - relative_tolerance: f32, - fixed_tolerance: f32, -) -> bool { - (a - b).abs() <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance -} - -pub fn complex_within_tolerance( +pub fn complex_isclose( a: Complex, b: Complex, - relative_tolerance: f32, - fixed_tolerance: f32, + rtol: f32, + atol: f32, ) -> bool { - within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance) - && within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance) + isclose(a.0, b.0, rtol, atol) && isclose(a.1, b.1, rtol, atol) } -pub fn complex_array_within_tolerance( +pub fn complex_allclose( a: &[Complex], b: &[Complex], - relative_tolerance: f32, - fixed_tolerance: f32, + rtol: f32, + atol: f32, ) -> bool { let mut result: bool = true; a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_within_tolerance( - *i, - *j, - relative_tolerance, - fixed_tolerance, - ); + result &= complex_isclose(*i, *j, rtol, atol); }); result }