diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1f131c8..6c7b0e9 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: --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 ci-success: diff --git a/.gitignore b/.gitignore index 265b7f5..68b644a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ /target +/dsp/target .gdb_history diff --git a/Cargo.lock b/Cargo.lock index c804d71..9cf3a58 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -178,9 +178,9 @@ dependencies = [ [[package]] name = "cortex-m-semihosting" -version = "0.3.5" +version = "0.3.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "113ef0ecffee2b62b58f9380f4469099b30e9f9cbee2804771b4203ba1762cfa" +checksum = "6bffa6c1454368a6aa4811ae60964c38e6996d397ff8095a8b9211b1c1f749bc" dependencies = [ "cortex-m", ] @@ -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" @@ -383,9 +390,9 @@ checksum = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f" [[package]] name = "rtic-core" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ab51fe832317e805f869b3d859f91aadf855c2c3da51f9b84bc645c201597158" +checksum = "8bd58a6949de8ff797a346a28d9f13f7b8f54fa61bb5e3cb0985a4efb497a5ef" [[package]] name = "rtic-syntax" @@ -424,9 +431,9 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" [[package]] name = "serde" -version = "1.0.117" +version = "1.0.118" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b88fa983de7720629c9387e9f517353ed404164b1e482c970a90c1a4aaf7dc1a" +checksum = "06c64263859d87aa2eb554587e2d23183398d617427327cf2b3d0ed8c69e4800" dependencies = [ "serde_derive", ] @@ -443,9 +450,9 @@ dependencies = [ [[package]] name = "serde_derive" -version = "1.0.117" +version = "1.0.118" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cbd1ae72adb44aab48f325a02444a5fc079349a8d804c1fc922aed3f7454c74e" +checksum = "c84d3526699cd55261af4b941e4e725444df67aa4f9e6a3564f18030d12672df" dependencies = [ "proc-macro2", "quote", @@ -527,9 +534,9 @@ dependencies = [ [[package]] name = "syn" -version = "1.0.48" +version = "1.0.53" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cc371affeffc477f42a221a1e4297aedcea33d47d19b61455588bd9d8f6b19ac" +checksum = "8833e20724c24de12bbaba5ad230ea61c3eafb05b881c7c9d3cfe8638b187e68" dependencies = [ "proc-macro2", "quote", diff --git a/Cargo.toml b/Cargo.toml index 896eecf..7217589 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -18,7 +18,6 @@ exclude = [ ] [badges] -travis-ci = { repository = "quartiq/stabilizer", branch = "master" } maintenance = { status = "experimental" } [package.metadata.docs.rs] @@ -42,6 +41,7 @@ asm-delay = "0.9.0" enum-iterator = "0.6.0" paste = "1" dsp = { path = "dsp" } +ad9959 = { path = "ad9959" } [dependencies.mcp23017] git = "https://github.com/mrd0ll4r/mcp23017.git" @@ -51,9 +51,6 @@ version = "0.6" features = ["ethernet", "proto-ipv4", "socket-tcp", "proto-ipv6"] default-features = false -[dependencies.ad9959] -path = "ad9959" - [dependencies.stm32h7xx-hal] features = ["stm32h743v", "rt", "unproven", "ethernet", "quadspi"] git = "https://github.com/stm32-rs/stm32h7xx-hal" 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 b2acf34..10cfcaa 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,4 +1,11 @@ -#![no_std] +#![cfg_attr(not(test), no_std)] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] +pub type Complex = (T, T); pub mod iir; +pub mod lockin; +pub mod pll; +pub mod unwrap; + +#[cfg(test)] +mod testing; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs new file mode 100644 index 0000000..bb649ad --- /dev/null +++ b/dsp/src/lockin.rs @@ -0,0 +1,518 @@ +//! Lock-in amplifier. +//! +//! Lock-in processing is performed through a combination of the +//! following modular processing blocks: demodulation, filtering, +//! decimation and computing the magnitude and phase from a complex +//! signal. These processing blocks are mutually independent. +//! +//! # Terminology +//! +//! * _demodulation signal_ - A copy of the reference signal that is +//! optionally frequency scaled and phase shifted. This is a complex +//! signal. The demodulation signals are used to demodulate the ADC +//! sampled signal. +//! * _internal clock_ - A fast internal clock used to increment a +//! counter for determining the 0-phase points of a reference signal. +//! * _reference signal_ - A constant-frequency signal used to derive +//! the demodulation signal. +//! * _timestamp_ - Timestamps record the timing of the reference +//! signal's 0-phase points. For instance, if a reference signal is +//! provided externally, a fast internal clock increments a +//! counter. When the external reference reaches the 0-phase point +//! (e.g., a positive edge), the value of the counter is recorded as a +//! timestamp. These timestamps are used to determine the frequency +//! and phase of the reference signal. +//! +//! # Usage +//! +//! The first step is to initialize a `Lockin` instance with +//! `Lockin::new()`. This provides the lock-in algorithms with +//! necessary information about the demodulation and filtering steps, +//! such as whether to demodulate with a harmonic of the reference +//! signal and the IIR biquad filter to use. There are then 4 +//! different processing steps that can be used: +//! +//! * `demodulate` - Computes the phase of the demodulation signal +//! corresponding to each ADC sample, uses this phase to compute the +//! demodulation signal, and multiplies this demodulation signal by +//! the ADC-sampled signal. This is a method of `Lockin` since it +//! requires information about how to modify the reference signal for +//! demodulation. +//! * `filter` - Performs IIR biquad filtering of a complex +//! signals. This is commonly performed on the signal provided by the +//! demodulation step, but can be performed at any other point in the +//! processing chain or omitted entirely. `filter` is a method of +//! `Lockin` since it must hold onto the filter configuration and +//! state. +//! * `decimate` - This decimates a signal to reduce the load on the +//! DAC output. It does not require any state information and is +//! therefore a normal function. +//! * `magnitude_phase` - Computes the magnitude and phase of the +//! component of the ADC-sampled signal whose frequency is equal to +//! the demodulation frequency. This does not require any state +//! information and is therefore a normal function. + +use super::iir::{IIRState, IIR}; +use super::Complex; +use core::f32::consts::PI; + +/// The number of ADC samples in one batch. +pub const ADC_SAMPLE_BUFFER_SIZE: usize = 16; +/// The number of outputs sent to the DAC for each ADC batch. +pub const DECIMATED_BUFFER_SIZE: usize = 1; + +/// Treat the 2-element array as a FIFO. This allows new elements to +/// be pushed into the array, existing elements to shift back in the +/// array, and the last element to fall off the array. +trait Fifo2 { + fn push(&mut self, new_element: Option); +} + +impl Fifo2 for [Option; 2] { + /// Push a new element into the array. The existing elements move + /// backward in the array by one location, and the current last + /// element is discarded. + /// + /// # Arguments + /// + /// * `new_element` - New element pushed into the front of the + /// array. + fn push(&mut self, new_element: Option) { + // For array sizes greater than 2 it would be preferable to + // use a rotating index to avoid unnecessary data + // copying. However, this would somewhat complicate the use of + // iterators and for 2 elements, shifting is inexpensive. + self[1] = self[0]; + self[0] = new_element; + } +} + +/// Performs lock-in amplifier processing of a signal. +pub struct Lockin { + phase_offset: f32, + sample_period: u32, + harmonic: u32, + timestamps: [Option; 2], + iir: IIR, + iirstate: [IIRState; 2], +} + +impl Lockin { + /// Initialize a new `Lockin` instance. + /// + /// # Arguments + /// + /// * `phase_offset` - Phase offset (in radians) applied to the + /// demodulation signal. + /// * `sample_period` - ADC sampling period in terms of the + /// internal clock period. + /// * `harmonic` - Integer scaling factor used to adjust the + /// demodulation frequency. E.g., 2 would demodulate with the + /// first harmonic. + /// * `iir` - IIR biquad filter. + /// + /// # Returns + /// + /// New `Lockin` instance. + pub fn new( + phase_offset: f32, + sample_period: u32, + harmonic: u32, + iir: IIR, + ) -> Self { + Lockin { + phase_offset: phase_offset, + sample_period: sample_period, + harmonic: harmonic, + timestamps: [None, None], + iir: iir, + iirstate: [[0.; 5]; 2], + } + } + + /// Demodulate an input signal with the complex reference signal. + /// + /// # Arguments + /// + /// * `adc_samples` - One batch of ADC samples. + /// * `timestamps` - Counter values corresponding to the edges of + /// an external reference signal. The counter is incremented by a + /// fast internal clock. + /// + /// # Returns + /// + /// The demodulated complex signal as a `Result`. When there are + /// an insufficient number of timestamps to perform processing, + /// `Err` is returned. + /// + /// # Assumptions + /// + /// `demodulate` expects that the timestamp counter value is equal + /// to 0 when the ADC samples its first input in a batch. This can + /// be achieved by configuring the timestamp counter to overflow + /// at the end of the ADC batch sampling period. + pub fn demodulate( + &mut self, + adc_samples: &[i16], + timestamps: &[u16], + ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { + let sample_period = self.sample_period as i32; + // update old timestamps for new ADC batch + self.timestamps.iter_mut().for_each(|t| match *t { + Some(timestamp) => { + // Existing timestamps have aged by one ADC batch + // period since the last ADC batch. + *t = Some( + timestamp - ADC_SAMPLE_BUFFER_SIZE as i32 * sample_period, + ); + } + None => (), + }); + + // return prematurely if there aren't enough timestamps for + // processing + let old_timestamp_count = + self.timestamps.iter().filter(|t| t.is_some()).count(); + if old_timestamp_count + timestamps.len() < 2 { + return Err("insufficient timestamps"); + } + + let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + // if we have not yet recorded any timestamps, the first + // reference period must be computed from the first and + // second timestamps in the array + let mut timestamp_index: usize = + if old_timestamp_count == 0 { 1 } else { 0 }; + + // compute ADC sample phases, sines/cosines and demodulate + signal + .iter_mut() + .zip(adc_samples.iter()) + .enumerate() + .for_each(|(i, (s, sample))| { + let adc_sample_count = i as i32 * sample_period; + // index of the closest timestamp that occurred after + // the current ADC sample + let closest_timestamp_after_index: i32 = if timestamps.len() > 0 + { + // Linear search is fast because both the timestamps + // and ADC sample counts are sorted. Because of this, + // we only need to check timestamps that were also + // greater than the last ADC sample count. + while timestamp_index < timestamps.len() - 1 + && (timestamps[timestamp_index] as i32) + < adc_sample_count + { + timestamp_index += 1; + } + timestamp_index as i32 + } else { + -1 + }; + + // closest timestamp that occurred before the current + // ADC sample + let closest_timestamp_before: i32; + let reference_period = if closest_timestamp_after_index < 0 { + closest_timestamp_before = self.timestamps[0].unwrap(); + closest_timestamp_before - self.timestamps[1].unwrap() + } else if closest_timestamp_after_index == 0 { + closest_timestamp_before = self.timestamps[0].unwrap(); + timestamps[0] as i32 - closest_timestamp_before + } else { + closest_timestamp_before = timestamps + [(closest_timestamp_after_index - 1) as usize] + as i32; + timestamps[closest_timestamp_after_index as usize] as i32 + - closest_timestamp_before + }; + + let integer_phase: i32 = (adc_sample_count + - closest_timestamp_before) + * self.harmonic as i32; + let phase = self.phase_offset + + 2. * PI * integer_phase as f32 / reference_period as f32; + let (sine, cosine) = libm::sincosf(phase); + let sample = *sample as f32; + s.0 = sine * sample; + s.1 = cosine * sample; + }); + + // record new timestamps + let start_index: usize = if timestamps.len() < 2 { + 0 + } else { + timestamps.len() - 2 + }; + timestamps[start_index..] + .iter() + .for_each(|t| self.timestamps.push(Some(*t as i32))); + + Ok(signal) + } + + /// Filter the complex signal using the supplied biquad IIR. The + /// signal array is modified in place. + /// + /// # Arguments + /// + /// * `signal` - Complex signal to filter. + pub fn filter(&mut self, signal: &mut [Complex]) { + signal.iter_mut().for_each(|s| { + s.0 = self.iir.update(&mut self.iirstate[0], s.0); + s.1 = self.iir.update(&mut self.iirstate[1], s.1); + }); + } +} + +/// Decimate the complex signal to `DECIMATED_BUFFER_SIZE`. The ratio +/// of `ADC_SAMPLE_BUFFER_SIZE` to `DECIMATED_BUFFER_SIZE` must be a +/// power of 2. +/// +/// # Arguments +/// +/// * `signal` - Complex signal to decimate. +/// +/// # Returns +/// +/// The decimated signal. +pub fn decimate( + signal: [Complex; ADC_SAMPLE_BUFFER_SIZE], +) -> [Complex; DECIMATED_BUFFER_SIZE] { + let n_k = ADC_SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE; + debug_assert!( + ADC_SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0 + ); + + let mut signal_decimated = [(0_f32, 0_f32); DECIMATED_BUFFER_SIZE]; + + signal_decimated + .iter_mut() + .zip(signal.iter().step_by(n_k)) + .for_each(|(s_d, s)| { + s_d.0 = s.0; + s_d.1 = s.1; + }); + + signal_decimated +} + +/// Compute the magnitude and phase from the complex signal. The +/// signal array is modified in place. +/// +/// # Arguments +/// +/// * `signal` - Complex signal to decimate. +pub fn magnitude_phase(signal: &mut [Complex]) { + signal.iter_mut().for_each(|s| { + let new_i = libm::sqrtf([s.0, s.1].iter().map(|i| i * i).sum()); + let new_q = libm::atan2f(s.1, s.0); + s.0 = new_i; + s.1 = new_q; + }); +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::testing::complex_allclose; + + #[test] + fn array_push() { + let mut arr: [Option; 2] = [None, None]; + arr.push(Some(1)); + assert_eq!(arr, [Some(1), None]); + arr.push(Some(2)); + assert_eq!(arr, [Some(2), Some(1)]); + arr.push(Some(10)); + assert_eq!(arr, [Some(10), Some(2)]); + } + + #[test] + fn magnitude_phase_length_1_quadrant_1() { + let mut signal: [Complex; 1] = [(1., 1.)]; + magnitude_phase(&mut signal); + assert!(complex_allclose( + &signal, + &[(2_f32.sqrt(), PI / 4.)], + f32::EPSILON, + 0. + )); + + signal = [(3_f32.sqrt() / 2., 1. / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_allclose( + &signal, + &[(1., PI / 6.)], + f32::EPSILON, + 0. + )); + } + + #[test] + fn magnitude_phase_length_1_quadrant_2() { + let mut signal = [(-1., 1.)]; + magnitude_phase(&mut signal); + assert!(complex_allclose( + &signal, + &[(2_f32.sqrt(), 3. * PI / 4.)], + f32::EPSILON, + 0. + )); + + signal = [(-1. / 2., 3_f32.sqrt() / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_allclose( + &signal, + &[(1_f32, 2. * PI / 3.)], + f32::EPSILON, + 0. + )); + } + + #[test] + fn magnitude_phase_length_1_quadrant_3() { + let mut signal = [(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_allclose( + &signal, + &[(1_f32.sqrt(), -3. * PI / 4.)], + f32::EPSILON, + 0. + )); + + signal = [(-1. / 2., -2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_allclose( + &signal, + &[((3. / 2.) as f32, -1.91063323625 as f32)], + f32::EPSILON, + 0. + )); + } + + #[test] + fn magnitude_phase_length_1_quadrant_4() { + let mut signal = [(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_allclose( + &signal, + &[(1_f32.sqrt(), -1. * PI / 4.)], + f32::EPSILON, + 0. + )); + + signal = [(3_f32.sqrt() / 2., -1. / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_allclose( + &signal, + &[(1_f32, -PI / 6.)], + f32::EPSILON, + 0. + )); + } + + #[test] + fn decimate_sample_16_decimated_1() { + let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [ + (0.0, 1.6), + (0.1, 1.7), + (0.2, 1.8), + (0.3, 1.9), + (0.4, 2.0), + (0.5, 2.1), + (0.6, 2.2), + (0.7, 2.3), + (0.8, 2.4), + (0.9, 2.5), + (1.0, 2.6), + (1.1, 2.7), + (1.2, 2.8), + (1.3, 2.9), + (1.4, 3.0), + (1.5, 3.1), + ]; + assert_eq!(decimate(signal), [(0.0, 1.6)]); + } + + #[test] + fn lockin_demodulate_valid_0() { + let mut lockin = Lockin::new( + 0., + 200, + 1, + IIR { + ba: [0_f32; 5], + y_offset: 0., + y_min: -(1 << 15) as f32, + y_max: (1 << 15) as f32 - 1., + }, + ); + assert_eq!( + lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[]), + Err("insufficient timestamps") + ); + } + + #[test] + fn lockin_demodulate_valid_1() { + let mut lockin = Lockin::new( + 0., + 200, + 1, + IIR { + ba: [0_f32; 5], + y_offset: 0., + y_min: -(1 << 15) as f32, + y_max: (1 << 15) as f32 - 1., + }, + ); + assert_eq!( + lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[0],), + Err("insufficient timestamps") + ); + } + + #[test] + fn lockin_demodulate_valid_2() { + let adc_period: u32 = 200; + let mut lockin = Lockin::new( + 0., + adc_period, + 1, + IIR { + ba: [0_f32; 5], + y_offset: 0., + y_min: -(1 << 15) as f32, + y_max: (1 << 15) as f32 - 1., + }, + ); + let adc_samples: [i16; ADC_SAMPLE_BUFFER_SIZE] = + [-8, 7, -7, 6, -6, 5, -5, 4, -4, 3, -3, 2, -2, -1, 1, 0]; + let reference_period: u16 = 2800; + let initial_phase_integer: u16 = 200; + let timestamps: &[u16] = &[ + initial_phase_integer, + initial_phase_integer + reference_period, + ]; + let initial_phase: f32 = + -(initial_phase_integer as f32) / reference_period as f32 * 2. * PI; + let phase_increment: f32 = + adc_period as f32 / reference_period as f32 * 2. * PI; + let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + for (n, s) in signal.iter_mut().enumerate() { + let adc_phase = initial_phase + n as f32 * phase_increment; + let sine = adc_phase.sin(); + let cosine = adc_phase.cos(); + s.0 = sine * adc_samples[n] as f32; + s.1 = cosine * adc_samples[n] as f32; + } + let result = lockin.demodulate(&adc_samples, timestamps).unwrap(); + assert!( + complex_allclose(&result, &signal, 0., 1e-5), + "\nsignal computed: {:?},\nsignal expected: {:?}", + result, + signal + ); + } +} diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs new file mode 100644 index 0000000..5ee054d --- /dev/null +++ b/dsp/src/pll.rs @@ -0,0 +1,97 @@ +use serde::{Deserialize, Serialize}; + +/// Type-II, sampled phase, discrete time PLL +/// +/// This PLL tracks the frequency and phase of an input signal with respect to the sampling clock. +/// The transfer function is I^2,I from input phase to output phase and P,I from input phase to +/// output frequency. +/// +/// The PLL locks to any frequency (i.e. it locks to the alias in the first Nyquist zone) and is +/// stable for any gain (1 <= shift <= 30). It has a single parameter that determines the loop +/// bandwidth in octave steps. The gain can be changed freely between updates. +/// +/// The frequency and phase settling time constants for an (any) frequency jump are `1 << shift` +/// update cycles. The loop bandwidth is about `1/(2*pi*(1 << shift))` in units of the sample rate. +/// +/// All math is naturally wrapping 32 bit integer. Phase and frequency are understood modulo that +/// overflow in the first Nyquist zone. Expressing the IIR equations in other ways (e.g. single +/// (T)-DF-{I,II} biquad/IIR) would break on overflow. +/// +/// There are no floating point rounding errors here. But there is integer quantization/truncation +/// error of the `shift` lowest bits leading to a phase offset for very low gains. Truncation +/// bias is applied. Rounding is "half up". The phase truncation error can be removed very +/// efficiently by dithering. +/// +/// This PLL does not unwrap phase slips during lock acquisition. This can and should be +/// implemented elsewhere by (down) scaling and then unwrapping the input phase and (up) scaling +/// and wrapping output phase and frequency. This affects dynamic range accordingly. +/// +/// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to +/// increase resolution for extremely narrowband applications is obvious. +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct PLL { + // last input phase + x: i32, + // filtered frequency + f: i32, + // filtered output phase + y: i32, +} + +impl PLL { + /// Update the PLL with a new phase sample. + /// + /// Args: + /// * `input`: New input phase sample. + /// * `shift`: Error scaling. The frequency gain per update is `1/(1 << shift)`. The phase gain + /// is always twice the frequency gain. + /// + /// Returns: + /// A tuple of instantaneous phase and frequency (the current phase increment). + pub fn update(&mut self, x: i32, shift: u8) -> (i32, i32) { + debug_assert!((1..=30).contains(&shift)); + let bias = 1i32 << shift; + let e = x.wrapping_sub(self.f); + self.f = self.f.wrapping_add( + (bias >> 1).wrapping_add(e).wrapping_sub(self.x) >> shift, + ); + self.x = x; + let f = self.f.wrapping_add( + bias.wrapping_add(e).wrapping_sub(self.y) >> (shift - 1), + ); + self.y = self.y.wrapping_add(f); + (self.y, f) + } +} + +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn mini() { + let mut p = PLL::default(); + let (y, f) = p.update(0x10000, 10); + assert_eq!(y, 0xc2); + assert_eq!(f, y); + } + + #[test] + fn converge() { + let mut p = PLL::default(); + let f0 = 0x71f63049_i32; + let shift = 10; + let n = 31 << shift + 2; + let mut x = 0i32; + for i in 0..n { + x = x.wrapping_add(f0); + let (y, f) = p.update(x, shift); + if i > n / 4 { + assert_eq!(f.wrapping_sub(f0).abs() <= 1, true); + } + if i > n / 2 { + // The remaining error is removed by dithering. + assert_eq!(y.wrapping_sub(x).abs() < 1 << 18, true); + } + } + } +} diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs new file mode 100644 index 0000000..1a8e109 --- /dev/null +++ b/dsp/src/testing.rs @@ -0,0 +1,27 @@ +use super::Complex; + +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_isclose( + a: Complex, + b: Complex, + rtol: f32, + atol: f32, +) -> bool { + isclose(a.0, b.0, rtol, atol) && isclose(a.1, b.1, rtol, atol) +} + +pub fn complex_allclose( + a: &[Complex], + b: &[Complex], + rtol: f32, + atol: f32, +) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= complex_isclose(*i, *j, rtol, atol); + }); + result +} diff --git a/dsp/src/unwrap.rs b/dsp/src/unwrap.rs new file mode 100644 index 0000000..870629e --- /dev/null +++ b/dsp/src/unwrap.rs @@ -0,0 +1,83 @@ +use serde::{Deserialize, Serialize}; + +/// Subtract `y - x` with signed overflow. +/// +/// This is very similar to `i32::overflowing_sub(y, x)` except that the +/// overflow indicator is not a boolean but the signum of the overflow. +/// Additionally it's typically faster. +/// +/// Returns: +/// A tuple containg the (wrapped) difference `y - x` and the signum of the +/// overflow. +#[inline(always)] +pub fn overflowing_sub(y: i32, x: i32) -> (i32, i8) { + let delta = y.wrapping_sub(x); + let wrap = (delta >= 0) as i8 - (y >= x) as i8; + (delta, wrap) +} + +/// Overflow unwrapper. +/// +/// This is unwrapping as in the phase and overflow unwrapping context, not +/// unwrapping as in the `Result`/`Option` context. +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct Unwrapper { + // last input + x: i32, + // last wraps + w: i32, +} + +impl Unwrapper { + /// Unwrap a new sample from a sequence and update the unwrapper state. + /// + /// Args: + /// * `x`: New sample + /// + /// Returns: + /// A tuple containing the (wrapped) difference `x - x_old` and the + /// signed number of wraps accumulated by the new sample. + pub fn update(&mut self, x: i32) -> (i32, i32) { + let (dx, dw) = overflowing_sub(x, self.x); + self.x = x; + self.w = self.w.wrapping_add(dw as i32); + (dx, self.w) + } +} + +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn mini() { + for (x0, x1, v) in [ + (0i32, 0i32, 0i8), + (0, 1, 0), + (0, -1, 0), + (1, 0, 0), + (-1, 0, 0), + (0, 0x7fff_ffff, 0), + (-1, 0x7fff_ffff, -1), + (-2, 0x7fff_ffff, -1), + (-1, -0x8000_0000, 0), + (0, -0x8000_0000, 0), + (1, -0x8000_0000, 1), + (-0x6000_0000, 0x6000_0000, -1), + (0x6000_0000, -0x6000_0000, 1), + (-0x4000_0000, 0x3fff_ffff, 0), + (-0x4000_0000, 0x4000_0000, -1), + (-0x4000_0000, 0x4000_0001, -1), + (0x4000_0000, -0x3fff_ffff, 0), + (0x4000_0000, -0x4000_0000, 0), + (0x4000_0000, -0x4000_0001, 1), + ] + .iter() + { + let (dx, w) = overflowing_sub(*x1, *x0); + assert_eq!(*v, w, " = overflowing_sub({:#x}, {:#x})", *x0, *x1); + let (dx0, w0) = x1.overflowing_sub(*x0); + assert_eq!(w0, w != 0); + assert_eq!(dx, dx0); + } + } +} diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs new file mode 100644 index 0000000..37ab2b0 --- /dev/null +++ b/dsp/tests/lockin_low_pass.rs @@ -0,0 +1,1137 @@ +use dsp::iir::IIR; +use dsp::lockin::{ + decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, + DECIMATED_BUFFER_SIZE, +}; +use dsp::Complex; + +use std::f64::consts::PI; +use std::vec::Vec; + +const ADC_MAX: f64 = 1.; +const ADC_MAX_COUNT: f64 = (1 << 15) as f64; + +/// Single-frequency sinusoid. +#[derive(Copy, Clone)] +struct PureSine { + // Frequency (in Hz). + frequency: f64, + // Amplitude in dBFS (decibels relative to full-scale). A 16-bit + // ADC has a minimum dBFS for each sample of -90. + amplitude_dbfs: f64, + // Phase offset (in radians). + phase_offset: f64, +} + +/// Convert a dBFS voltage ratio to a linear ratio. +/// +/// # Arguments +/// +/// * `dbfs` - dB ratio relative to full scale. +/// +/// # Returns +/// +/// Linear value. +fn linear(dbfs: f64) -> f64 { + let base = 10_f64; + ADC_MAX * base.powf(dbfs / 20.) +} + +/// Convert a linear voltage ratio to a dBFS ratio. +/// +/// # Arguments +/// +/// * `linear` - Linear voltage ratio. +/// +/// # Returns +/// +/// dBFS value. +fn dbfs(linear: f64) -> f64 { + 20. * (linear / ADC_MAX).log10() +} + +/// Convert a real ADC input value in the range `-ADC_MAX` to +/// `+ADC_MAX` to an equivalent 16-bit ADC sampled value. This models +/// the ideal ADC transfer function. +/// +/// # Arguments +/// +/// * `x` - Real ADC input value. +/// +/// # Returns +/// +/// Sampled ADC value. +fn real_to_adc_sample(x: f64) -> i16 { + let max: i32 = i16::MAX as i32; + let min: i32 = i16::MIN as i32; + + let xi: i32 = (x / ADC_MAX * ADC_MAX_COUNT) as i32; + + // It's difficult to characterize the correct output result when + // the inputs are clipped, so panic instead. + if xi > max { + panic!("Input clipped to maximum, result is unlikely to be correct."); + } else if xi < min { + panic!("Input clipped to minimum, result is unlikely to be correct."); + } + + xi as i16 +} + +/// Generate `ADC_SAMPLE_BUFFER_SIZE` values of an ADC-sampled signal +/// starting at `timestamp_start`. +/// +/// # Arguments +/// +/// * `pure_signals` - Pure sinusoidal components of the ADC-sampled +/// signal. +/// * `timestamp_start` - Starting time of ADC-sampled signal in terms +/// of the internal clock count. +/// * `internal_frequency` - Internal clock frequency (in Hz). +/// * `adc_frequency` - ADC sampling frequency (in Hz). +/// +/// # Returns +/// +/// The sampled signal at the ADC input. +fn adc_sampled_signal( + pure_signals: &Vec, + timestamp_start: u64, + internal_frequency: f64, + adc_frequency: f64, +) -> [i16; ADC_SAMPLE_BUFFER_SIZE] { + // amplitude of each pure signal + let mut amplitude: Vec = Vec::::new(); + // initial phase value for each pure signal + let mut initial_phase: Vec = Vec::::new(); + // phase increment at each ADC sample for each pure signal + let mut phase_increment: Vec = Vec::::new(); + let adc_period = internal_frequency / adc_frequency; + + // For each pure sinusoid, compute the amplitude, phase + // corresponding to the first ADC sample, and phase increment for + // each subsequent ADC sample. + for pure_signal in pure_signals.iter() { + let signal_period = internal_frequency / pure_signal.frequency; + let phase_offset_count = + pure_signal.phase_offset / (2. * PI) * signal_period; + let initial_phase_count = + (phase_offset_count + timestamp_start as f64) % signal_period; + + amplitude.push(linear(pure_signal.amplitude_dbfs)); + initial_phase.push(2. * PI * initial_phase_count / signal_period); + phase_increment.push(2. * PI * adc_period / signal_period); + } + + // Compute the input signal corresponding to each ADC sample by + // summing the contributions from each pure sinusoid. + let mut signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = [0; ADC_SAMPLE_BUFFER_SIZE]; + signal.iter_mut().enumerate().for_each(|(n, s)| { + *s = real_to_adc_sample( + amplitude + .iter() + .zip(initial_phase.iter()) + .zip(phase_increment.iter()) + .fold(0., |acc, ((a, phi), theta)| { + acc + a * (phi + theta * n as f64).sin() + }), + ); + }); + + signal +} + +/// Reference clock timestamp values in one ADC batch period starting +/// at `timestamp_start`. Also returns the number of valid timestamps. +/// +/// # Arguments +/// +/// * `reference_frequency` - External reference signal frequency (in +/// Hz). +/// * `timestamp_start` - Start time in terms of the internal clock +/// count. This is the start time of the current processing sequence +/// (i.e., for the current `ADC_SAMPLE_BUFFER_SIZE` ADC samples). +/// * `timestamp_stop` - Stop time in terms of the internal clock +/// count. +/// * `internal_frequency` - Internal clock frequency (in Hz). +/// +/// # Returns +/// +/// Tuple consisting of the number of valid timestamps in the ADC +/// batch period, followed by an array of the timestamp values. +fn adc_batch_timestamps( + reference_frequency: f64, + timestamps: &mut [u16], + timestamp_start: u64, + timestamp_stop: u64, + internal_frequency: f64, +) -> &[u16] { + let reference_period = internal_frequency / reference_frequency; + let start_count = timestamp_start as f64 % reference_period; + let mut valid_timestamps: usize = 0; + + let mut timestamp = (reference_period - start_count) % reference_period; + while timestamp < (timestamp_stop - timestamp_start) as f64 { + timestamps[valid_timestamps] = timestamp as u16; + timestamp += reference_period; + valid_timestamps += 1; + } + + ×tamps[..valid_timestamps] +} + +/// Lowpass biquad filter using cutoff and sampling frequencies. +/// Taken from: +/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html +/// +/// # Arguments +/// +/// * `corner_frequency` - Corner frequency, or 3dB cutoff frequency +/// (in Hz). +/// * `sampling_frequency` - Sampling frequency (in Hz). +/// +/// # Returns +/// +/// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 +/// is set to -1. +fn lowpass_iir_coefficients( + corner_frequency: f64, + sampling_frequency: f64, +) -> [f32; 5] { + let normalized_angular_frequency: f64 = + 2. * PI * corner_frequency / sampling_frequency; + let quality_factor: f64 = 1. / 2f64.sqrt(); + let alpha: f64 = normalized_angular_frequency.sin() / (2. * quality_factor); + // All b coefficients have been multiplied by a factor of 2 in + // comparison with the link above in order to set the passband + // gain to 2. + let mut b0: f64 = 1. - normalized_angular_frequency.cos(); + let mut b1: f64 = 2. * (1. - normalized_angular_frequency.cos()); + let mut b2: f64 = b0; + let a0: f64 = 1. + alpha; + let mut a1: f64 = -2. * normalized_angular_frequency.cos(); + let mut a2: f64 = 1. - alpha; + b0 /= a0; + b1 /= a0; + b2 /= a0; + a1 /= -a0; + a2 /= -a0; + + [b0 as f32, b1 as f32, b2 as f32, a1 as f32, a2 as f32] +} + +/// Check that a measured value is within some tolerance of the actual +/// value. This allows setting both fixed and relative tolerances. +/// +/// # Arguments +/// +/// * `actual` - Actual value with respect to which the magnitude of +/// the relative tolerance is computed. +/// * `computed` - Computed value. This is compared with the actual +/// value, `actual`. +/// * `fixed_tolerance` - Fixed tolerance. +/// * `relative_tolerance` - Relative tolerance. +/// `relative_tolerance`*`actual` gives the total contribution of the +/// relative tolerance. +/// +/// # Returns +/// +/// `true` if the `actual` and `computed` values are within the +/// specified tolerance of one another, and `false` otherwise. +fn tolerance_check( + actual: f32, + computed: f32, + fixed_tolerance: f32, + relative_tolerance: f32, +) -> bool { + (actual - computed).abs() + < max_error(actual, fixed_tolerance, relative_tolerance) +} + +/// Maximum acceptable error from an actual value given fixed and +/// relative tolerances. +/// +/// # Arguments +/// +/// * `actual` - Actual value with respect to which the magnitude of the +/// relative tolerance is computed. +/// * `fixed_tolerance` - Fixed tolerance. +/// * `relative_tolerance` - Relative tolerance. +/// `relative_tolerance`*`actual` gives the total contribution of the +/// relative tolerance. +/// +/// # Returns +/// +/// Maximum acceptable error. +fn max_error( + actual: f32, + fixed_tolerance: f32, + relative_tolerance: f32, +) -> f32 { + relative_tolerance * actual.abs() + fixed_tolerance +} + +/// Total noise amplitude of the input signal after sampling by the +/// ADC. This computes an upper bound of the total noise amplitude, +/// rather than its actual value. +/// +/// # Arguments +/// +/// * `noise_inputs` - Noise sources at the ADC input. +/// * `demodulation_frequency` - Frequency of the demodulation signal +/// (in Hz). +/// * `corner_frequency` - Low-pass filter 3dB corner (cutoff) +/// frequency. +/// +/// # Returns +/// +/// Upper bound of the total amplitude of all noise sources. +fn sampled_noise_amplitude( + noise_inputs: &Vec, + demodulation_frequency: f64, + corner_frequency: f64, +) -> f64 { + // There is not a simple way to compute the amplitude of a + // superpostition of sinusoids with different frequencies and + // phases. Although we can compute the amplitude in special cases + // (e.g., two signals whose periods have a common multiple), these + // do not help us in the general case. However, we can say that + // the total amplitude will not be greater than the sum of the + // amplitudes of the individual noise sources. We treat this as an + // upper bound, and use it as an approximation of the actual + // amplitude. + + let mut noise: f64 = noise_inputs + .iter() + .map(|n| { + // Noise inputs create an oscillation at the output, where the + // oscillation magnitude is determined by the strength of the + // noise and its attenuation (attenuation is determined by its + // proximity to the demodulation frequency and filter + // rolloff). + let octaves = ((n.frequency - demodulation_frequency).abs() + / corner_frequency) + .log2(); + // 2nd-order filter. Approximately 12dB/octave rolloff. + let attenuation = -2. * 20. * 2_f64.log10() * octaves; + linear(n.amplitude_dbfs + attenuation) + }) + .sum(); + + // Add in 1/2 LSB for the maximum amplitude deviation resulting + // from quantization. + noise += 1. / ADC_MAX_COUNT / 2.; + + noise +} + +/// Compute the maximum effect of input noise on the lock-in magnitude +/// computation. +/// +/// The maximum effect of noise on the magnitude computation is given +/// by: +/// +/// | sqrt((I+n*sin(x))**2 + (Q+n*cos(x))**2) - sqrt(I**2 + Q**2) | +/// +/// * I is the in-phase component of the portion of the input signal +/// with the same frequency as the demodulation signal. +/// * Q is the quadrature component. +/// * n is the total noise amplitude (from all contributions, after +/// attenuation from filtering). +/// * x is the phase of the demodulation signal. +/// +/// We need to find the demodulation phase (x) that maximizes this +/// expression. We can ignore the absolute value operation by also +/// considering the expression minimum. The locations of the minimum +/// and maximum can be computed analytically by finding the value of x +/// when the derivative of this expression with respect to x is +/// 0. When we solve this equation, we find: +/// +/// x = atan(I/Q) +/// +/// It's worth noting that this solution is technically only valid +/// when cos(x)!=0 (i.e., x!=pi/2,-pi/2). However, this is not a +/// problem because we only get these values when Q=0. Rust correctly +/// computes atan(inf)=pi/2, which is precisely what we want because +/// x=pi/2 maximizes sin(x) and therefore also the noise effect. +/// +/// The other maximum or minimum is pi radians away from this +/// value. +/// +/// # Arguments +/// +/// * `total_noise_amplitude` - Combined amplitude of all noise +/// sources sampled by the ADC. +/// * `in_phase_actual` - Value of the in-phase component if no noise +/// were present at the ADC input. +/// * `quadrature_actual` - Value of the quadrature component if no +/// noise were present at the ADC input. +/// * `desired_input_amplitude` - Amplitude of the desired input +/// signal. That is, the input signal component with the same +/// frequency as the demodulation signal. +/// +/// # Returns +/// +/// Approximation of the maximum effect on the magnitude computation +/// due to noise sources at the ADC input. +fn magnitude_noise( + total_noise_amplitude: f64, + in_phase_actual: f64, + quadrature_actual: f64, + desired_input_amplitude: f64, +) -> f64 { + // See function documentation for explanation. + let noise = |in_phase_delta: f64, quadrature_delta: f64| -> f64 { + (((in_phase_actual + in_phase_delta).powf(2.) + + (quadrature_actual + quadrature_delta).powf(2.)) + .sqrt() + - desired_input_amplitude) + .abs() + }; + + let phase = (in_phase_actual / quadrature_actual).atan(); + let max_noise_1 = noise( + total_noise_amplitude * phase.sin(), + total_noise_amplitude * phase.cos(), + ); + let max_noise_2 = noise( + total_noise_amplitude * (phase + PI).sin(), + total_noise_amplitude * (phase + PI).cos(), + ); + + max_noise_1.max(max_noise_2) +} + +/// Compute the maximum phase deviation from the correct value due to +/// the input noise sources. +/// +/// The maximum effect of noise on the phase computation is given by: +/// +/// | atan2(Q+n*cos(x), I+n*sin(x)) - atan2(Q, I) | +/// +/// See `magnitude_noise` for an explanation of the terms in this +/// mathematical expression. +/// +/// This expression is harder to compute analytically than the +/// expression in `magnitude_noise`. We could compute it numerically, +/// but that's expensive. However, we can use heuristics to try to +/// guess the values of x that will maximize the noise +/// effect. Intuitively, the difference will be largest when the +/// Y-argument of the atan2 function (Q+n*cos(x)) is pushed in the +/// opposite direction of the noise effect on the X-argument (i.e., +/// cos(x) and sin(x) have different signs). We can use: +/// +/// * sin(x)=+-1 (+- denotes plus or minus), cos(x)=0, +/// * sin(x)=0, cos(x)=+-1, and +/// * the value of x that maximizes |sin(x)-cos(x)| (when +/// sin(x)=1/sqrt(2) and cos(x)=-1/sqrt(2), or when the signs are +/// flipped) +/// +/// The first choice addresses cases in which |I|>>|Q|, the second +/// choice addresses cases in which |Q|>>|I|, and the third choice +/// addresses cases in which |I|~|Q|. We can test all of these cases +/// as an approximation for the real maximum. +/// +/// # Arguments +/// +/// * `total_noise_amplitude` - Total amplitude of all input noise +/// sources. +/// * `in_phase_actual` - Value of the in-phase component if no noise +/// were present at the input. +/// * `quadrature_actual` - Value of the quadrature component if no +/// noise were present at the input. +/// +/// # Returns +/// +/// Approximation of the maximum effect on the phase computation due +/// to noise sources at the ADC input. +fn phase_noise( + total_noise_amplitude: f64, + in_phase_actual: f64, + quadrature_actual: f64, +) -> f64 { + // See function documentation for explanation. + let noise = |in_phase_delta: f64, quadrature_delta: f64| -> f64 { + ((quadrature_actual + quadrature_delta) + .atan2(in_phase_actual + in_phase_delta) + - quadrature_actual.atan2(in_phase_actual)) + .abs() + }; + + let mut max_noise: f64 = 0.; + for (in_phase_delta, quadrature_delta) in [ + ( + total_noise_amplitude / 2_f64.sqrt(), + total_noise_amplitude / -2_f64.sqrt(), + ), + ( + total_noise_amplitude / -2_f64.sqrt(), + total_noise_amplitude / 2_f64.sqrt(), + ), + (total_noise_amplitude, 0.), + (-total_noise_amplitude, 0.), + (0., total_noise_amplitude), + (0., -total_noise_amplitude), + ] + .iter() + { + max_noise = max_noise.max(noise(*in_phase_delta, *quadrature_delta)); + } + + max_noise +} + +/// Lowpass filter test for in-phase/quadrature and magnitude/phase +/// computations. +/// +/// This attempts to "intelligently" model acceptable tolerance ranges +/// for the measured in-phase, quadrature, magnitude and phase results +/// of lock-in processing for a typical low-pass filter +/// application. So, instead of testing whether the lock-in processing +/// extracts the true magnitude and phase (or in-phase and quadrature +/// components) of the input signal, it attempts to calculate what the +/// lock-in processing should compute given any set of input noise +/// sources. For example, if a noise source of sufficient strength +/// differs in frequency by 1kHz from the reference frequency and the +/// filter cutoff frequency is also 1kHz, testing if the lock-in +/// amplifier extracts the amplitude and phase of the input signal +/// whose frequency is equal to the demodulation frequency is doomed +/// to failure. Instead, this function tests whether the lock-in +/// correctly adheres to its actual transfer function, whether or not +/// it was given reasonable inputs. The logic for computing acceptable +/// tolerance ranges is performed in `sampled_noise_amplitude`, +/// `magnitude_noise`, and `phase_noise`. +/// +/// # Arguments +/// +/// * `internal_frequency` - Internal clock frequency (Hz). The +/// internal clock increments timestamp counter values used to +/// record the edges of the external reference. +/// * `adc_frequency` - ADC sampling frequency (in Hz). +/// * `reference_frequency` - External reference frequency (in Hz). +/// * `demodulation_phase_offset` - Phase offset applied to the +/// in-phase and quadrature demodulation signals. +/// * `harmonic` - Scaling factor for the demodulation +/// frequency. E.g., 2 would demodulate with the first harmonic of the +/// reference frequency. +/// * `corner_frequency` - Lowpass filter 3dB cutoff frequency. +/// * `desired_input` - `PureSine` giving the frequency, amplitude and +/// phase of the desired result. +/// * `noise_inputs` - Vector of `PureSine` for any noise inputs on top +/// of `desired_input`. +/// * `time_constant_factor` - Number of time constants after which +/// the output is considered valid. +/// * `tolerance` - Acceptable relative tolerance for the magnitude +/// and angle outputs. The outputs must remain within this tolerance +/// between `time_constant_factor` and `time_constant_factor+1` time +/// constants. +fn lowpass_test( + internal_frequency: f64, + adc_frequency: f64, + reference_frequency: f64, + demodulation_phase_offset: f64, + harmonic: u32, + corner_frequency: f64, + desired_input: PureSine, + noise_inputs: &mut Vec, + time_constant_factor: f64, + tolerance: f32, +) { + let mut lockin = Lockin::new( + demodulation_phase_offset as f32, + (internal_frequency / adc_frequency) as u32, + harmonic, + IIR { + ba: lowpass_iir_coefficients(corner_frequency, adc_frequency), + y_offset: 0., + y_min: -ADC_MAX_COUNT as f32, + y_max: (ADC_MAX_COUNT - 1.) as f32, + }, + ); + + let mut timestamp_start: u64 = 0; + let time_constant: f64 = 1. / (2. * PI * corner_frequency); + let samples = + (time_constant_factor * time_constant * adc_frequency) as usize; + // Ensure the result remains within tolerance for 1 time constant + // after `time_constant_factor` time constants. + let extra_samples = (time_constant * adc_frequency) as usize; + let sample_count: u64 = (internal_frequency / adc_frequency) as u64 + * ADC_SAMPLE_BUFFER_SIZE as u64; + + let effective_phase_offset = + desired_input.phase_offset - demodulation_phase_offset; + let in_phase_actual = + linear(desired_input.amplitude_dbfs) * effective_phase_offset.cos(); + let quadrature_actual = + linear(desired_input.amplitude_dbfs) * effective_phase_offset.sin(); + + let total_noise_amplitude = sampled_noise_amplitude( + noise_inputs, + reference_frequency * harmonic as f64, + corner_frequency, + ); + let total_magnitude_noise = magnitude_noise( + total_noise_amplitude, + in_phase_actual, + quadrature_actual, + linear(desired_input.amplitude_dbfs), + ); + let total_phase_noise = + phase_noise(total_noise_amplitude, in_phase_actual, quadrature_actual); + + let pure_signals = noise_inputs; + pure_signals.push(desired_input); + + for n in 0..(samples + extra_samples) { + let adc_signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( + &pure_signals, + timestamp_start, + internal_frequency, + adc_frequency, + ); + let mut timestamps_array = [0_u16; ADC_SAMPLE_BUFFER_SIZE / 2]; + let timestamps = adc_batch_timestamps( + reference_frequency, + &mut timestamps_array, + timestamp_start, + timestamp_start + sample_count - 1, + internal_frequency, + ); + + let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE]; + match lockin.demodulate(&adc_signal, timestamps) { + Ok(s) => { + signal = s; + } + Err(_) => { + continue; + } + } + + lockin.filter(&mut signal); + let signal_decimated = decimate(signal); + + let mut magnitude_phase_decimated = signal.clone(); + // let mut magnitude_decimated = in_phase_decimated.clone(); + // let mut phase_decimated = quadrature_decimated.clone(); + + magnitude_phase(&mut magnitude_phase_decimated); + + // Ensure stable within tolerance for 1 time constant after + // `time_constant_factor`. + if n >= samples { + for k in 0..DECIMATED_BUFFER_SIZE { + let amplitude_normalized: f32 = + magnitude_phase_decimated[k].0 / ADC_MAX_COUNT as f32; + assert!( + tolerance_check(linear(desired_input.amplitude_dbfs) as f32, amplitude_normalized, total_magnitude_noise as f32, tolerance), + "magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}", + linear(desired_input.amplitude_dbfs), + desired_input.amplitude_dbfs, + amplitude_normalized, + dbfs(amplitude_normalized as f64), + max_error(linear(desired_input.amplitude_dbfs) as f32, total_magnitude_noise as f32, tolerance) + ); + assert!( + tolerance_check( + effective_phase_offset as f32, + magnitude_phase_decimated[k].1, + total_phase_noise as f32, + tolerance + ), + "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", + effective_phase_offset as f32, + magnitude_phase_decimated[k].1, + max_error( + effective_phase_offset as f32, + total_phase_noise as f32, + tolerance + ) + ); + + let in_phase_normalized: f32 = + signal_decimated[k].0 / ADC_MAX_COUNT as f32; + let quadrature_normalized: f32 = + signal_decimated[k].1 / ADC_MAX_COUNT as f32; + assert!( + tolerance_check( + in_phase_actual as f32, + in_phase_normalized, + total_noise_amplitude as f32, + tolerance + ), + "in-phase actual: {:.4}, in-phase computed: {:.3}, tolerance: {:.4}", + in_phase_actual, + in_phase_normalized, + max_error( + in_phase_actual as f32, + total_noise_amplitude as f32, + tolerance + ) + ); + assert!( + tolerance_check( + quadrature_actual as f32, + quadrature_normalized, + total_noise_amplitude as f32, + tolerance + ), + "quadrature actual: {:.4}, quadrature computed: {:.4}, tolerance: {:.4}", + quadrature_actual, + quadrature_normalized, + max_error( + quadrature_actual as f32, + total_noise_amplitude as f32, + tolerance + ) + ); + } + } + + timestamp_start += sample_count; + } +} + +#[test] +fn lowpass() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_demodulation_phase_offset_pi_2() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = PI / 2.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_phase_offset_pi_2() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 2., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_fundamental_111e3_phase_offset_pi_4() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 111e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 4., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_first_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 2; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_second_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 3; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_third_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 4; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_first_harmonic_phase_shift() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 2; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 4., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_adc_frequency_1e6() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 1e6; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_internal_frequency_125e6() { + let internal_frequency: f64 = 125e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_low_signal_frequency() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 10e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }], + time_constant_factor, + tolerance, + ); +} diff --git a/src/main.rs b/src/main.rs index f7b07e1..b37f740 100644 --- a/src/main.rs +++ b/src/main.rs @@ -60,6 +60,9 @@ const SAMPLE_FREQUENCY_KHZ: u32 = 500; // The desired ADC sample processing buffer size. const SAMPLE_BUFFER_SIZE: usize = 1; +// The number of cascaded IIR biquads per channel. Select 1 or 2! +const IIR_CASCADE_LENGTH: usize = 1; + #[link_section = ".sram3.eth"] static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); @@ -214,10 +217,11 @@ const APP: () = { pounder: Option, - #[init([[0.; 5]; 2])] - iir_state: [iir::IIRState; 2], - #[init([iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; 2])] - iir_ch: [iir::IIR; 2], + // Format: iir_state[ch][cascade-no][coeff] + #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] + iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] + iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], } #[init] @@ -809,8 +813,11 @@ const APP: () = { for channel in 0..adc_samples.len() { for sample in 0..adc_samples[0].len() { let x = f32::from(adc_samples[channel][sample] as i16); - let y = c.resources.iir_ch[channel] - .update(&mut c.resources.iir_state[channel], x); + let mut y = x; + for i in 0..c.resources.iir_state[channel].len() { + y = c.resources.iir_ch[channel][i] + .update(&mut c.resources.iir_state[channel][i], y); + } // Note(unsafe): The filter limits ensure that the value is in range. // The truncation introduces 1/2 LSB distortion. let y = unsafe { y.to_int_unchecked::() }; @@ -887,10 +894,23 @@ const APP: () = { let state = c.resources.iir_state.lock(|iir_state| server::Status { t: time, - x0: iir_state[0][0], - y0: iir_state[0][2], - x1: iir_state[1][0], - y1: iir_state[1][2], + x0: iir_state[0][0][0], + y0: iir_state[0][0][2], + x1: iir_state[1][0][0], + y1: iir_state[1][0][2], + }); + + Ok::(state) + }), + // "_b" means cascades 2nd IIR + "stabilizer/iir_b/state": (|| { + let state = c.resources.iir_state.lock(|iir_state| + server::Status { + t: time, + x0: iir_state[0][IIR_CASCADE_LENGTH-1][0], + y0: iir_state[0][IIR_CASCADE_LENGTH-1][2], + x1: iir_state[1][IIR_CASCADE_LENGTH-1][0], + y1: iir_state[1][IIR_CASCADE_LENGTH-1][2], }); Ok::(state) @@ -906,7 +926,7 @@ const APP: () = { return Err(()); } - iir_ch[req.channel as usize] = req.iir; + iir_ch[req.channel as usize][0] = req.iir; Ok::(req) }) @@ -917,7 +937,29 @@ const APP: () = { return Err(()); } - iir_ch[req.channel as usize] = req.iir; + iir_ch[req.channel as usize][0] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir_b0/state": server::IirRequest, (|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir_b1/state": server::IirRequest,(|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; Ok::(req) })