diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f4c6212..1314418 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,6 +29,11 @@ jobs: - uses: actions-rs/clippy-check@v1 with: token: ${{ secrets.GITHUB_TOKEN }} + - name: cargo check + uses: actions-rs/cargo@v1 + with: + command: check + args: --verbose compile: runs-on: ubuntu-latest @@ -37,6 +42,12 @@ jobs: toolchain: - stable - beta + bin: + - dual-iir + - lockin + features: + - '' + - pounder_v1_1 steps: - uses: actions/checkout@v2 - name: Install Rust ${{ matrix.toolchain }} @@ -46,20 +57,11 @@ jobs: target: thumbv7em-none-eabihf override: true components: llvm-tools-preview - - name: cargo check - uses: actions-rs/cargo@v1 - with: - command: check - args: --verbose - - name: cargo build - uses: actions-rs/cargo@v1 - with: - command: build - name: cargo build release uses: actions-rs/cargo@v1 with: command: build - args: --release + args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }} - name: cargo-binutils uses: actions-rs/cargo@v1 with: @@ -69,25 +71,19 @@ jobs: uses: actions-rs/cargo@v1 with: command: size - args: --release + args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }} - name: cargo objcopy uses: actions-rs/cargo@v1 with: command: objcopy - args: --release --verbose -- -O binary stabilizer-release.bin + args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }} --verbose -- -O binary ${{ matrix.bin }}-release.bin - uses: actions/upload-artifact@v2 - if: ${{ matrix.toolchain == 'stable' }} + if: ${{ matrix.toolchain == 'stable' && matrix.features == '' }} with: - name: stabilizer_${{ github.sha }} + name: stabilizer_${{ matrix.bin }} path: | - target/*/release/stabilizer - stabilizer-release.bin - - - name: Build (Pounder v1.1) - uses: actions-rs/cargo@v1 - with: - command: build - args: --features pounder_v1_1 + target/*/release/${{ matrix.bin }} + ${{ matrix.bin }}-release.bin test: runs-on: ubuntu-latest diff --git a/dsp/benches/trig.rs b/dsp/benches/trig.rs index 19b6cce..df17a65 100644 --- a/dsp/benches/trig.rs +++ b/dsp/benches/trig.rs @@ -1,6 +1,6 @@ use core::f32::consts::PI; use criterion::{black_box, criterion_group, criterion_main, Criterion}; -use dsp::trig::{atan2, cossin}; +use dsp::{atan2, cossin}; fn atan2_bench(c: &mut Criterion) { let xi = (10 << 16) as i32; diff --git a/dsp/src/atan2.rs b/dsp/src/atan2.rs new file mode 100644 index 0000000..6d6e476 --- /dev/null +++ b/dsp/src/atan2.rs @@ -0,0 +1,135 @@ +/// 2-argument arctangent function. +/// +/// This implementation uses all integer arithmetic for fast +/// computation. It is designed to have high accuracy near the axes +/// and lower away from the axes. It is additionally designed so that +/// the error changes slowly with respect to the angle. +/// +/// # Arguments +/// +/// * `y` - Y-axis component. +/// * `x` - X-axis component. +/// +/// # Returns +/// +/// The angle between the x-axis and the ray to the point (x,y). The +/// result range is from i32::MIN to i32::MAX, where i32::MIN +/// represents -pi and, equivalently, +pi. i32::MAX represents one +/// count less than +pi. +pub fn atan2(y: i32, x: i32) -> i32 { + let sign = (x < 0, y < 0); + + let mut y = y.wrapping_abs() as u32; + let mut x = x.wrapping_abs() as u32; + + let y_greater = y > x; + if y_greater { + core::mem::swap(&mut y, &mut x); + } + + let z = (16 - y.leading_zeros() as i32).max(0); + + x >>= z; + if x == 0 { + return 0; + } + y >>= z; + let r = (y << 16) / x; + debug_assert!(r <= 1 << 16); + + // Uses the general procedure described in the following + // Mathematics stack exchange answer: + // + // https://math.stackexchange.com/a/1105038/583981 + // + // The atan approximation method has been modified to be cheaper + // to compute and to be more compatible with integer + // arithmetic. The approximation technique used here is + // + // pi / 4 * r + C * r * (1 - abs(r)) + // + // which is taken from Rajan 2006: Efficient Approximations for + // the Arctangent Function. + // + // The least mean squared error solution is C = 0.279 (no the 0.285 that + // Rajan uses). K = C*4/pi. + // Q5 for K provides sufficient correction accuracy while preserving + // as much smoothness of the quadratic correction as possible. + const FP_K: usize = 5; + const K: u32 = (0.35489 * (1 << FP_K) as f64) as u32; + // debug_assert!(K == 11); + + // `r` is unsigned Q16.16 and <= 1 + // `angle` is signed Q1.31 with 1 << 31 == +- pi + // Since K < 0.5 and r*(1 - r) <= 0.25 the correction product can use + // 4 bits for K, and 15 bits for r and 1-r to remain within the u32 range. + let mut angle = ((r << 13) + + ((K * (r >> 1) * ((1 << 15) - (r >> 1))) >> (FP_K + 1))) + as i32; + + if y_greater { + angle = (1 << 30) - angle; + } + + if sign.0 { + angle = i32::MAX - angle; + } + + if sign.1 { + angle = angle.wrapping_neg(); + } + + angle +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f64::consts::PI; + + fn angle_to_axis(angle: f64) -> f64 { + let angle = angle % (PI / 2.); + (PI / 2. - angle).min(angle) + } + + #[test] + fn atan2_absolute_error() { + const N: usize = 321; + let mut test_vals = [0i32; N + 4]; + let scale = (1i64 << 31) as f64; + for i in 0..N { + test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32; + } + + assert!(test_vals.contains(&i32::MIN)); + test_vals[N] = i32::MAX; + test_vals[N + 1] = 0; + test_vals[N + 2] = -1; + test_vals[N + 3] = 1; + + let mut rms_err = 0f64; + let mut abs_err = 0f64; + let mut rel_err = 0f64; + + for &x in test_vals.iter() { + for &y in test_vals.iter() { + let want = (y as f64 / scale).atan2(x as f64 / scale); + let have = atan2(y, x) as f64 * PI / scale; + + let err = (have - want).abs(); + abs_err = abs_err.max(err); + rms_err += err * err; + if err > 3e-5 { + rel_err = rel_err.max(err / angle_to_axis(want)); + } + } + } + rms_err = rms_err.sqrt() / test_vals.len() as f64; + println!("max abs err: {:.2e}", abs_err); + println!("rms abs err: {:.2e}", rms_err); + println!("max rel err: {:.2e}", rel_err); + assert!(abs_err < 5e-3); + assert!(rms_err < 3e-3); + assert!(rel_err < 0.6); + } +} diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs new file mode 100644 index 0000000..4366b43 --- /dev/null +++ b/dsp/src/complex.rs @@ -0,0 +1,17 @@ +use super::atan2; +use serde::{Deserialize, Serialize}; + +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct Complex(pub T, pub T); + +impl Complex { + pub fn power(&self) -> i32 { + (((self.0 as i64) * (self.0 as i64) + + (self.1 as i64) * (self.1 as i64)) + >> 32) as i32 + } + + pub fn phase(&self) -> i32 { + atan2(self.1, self.0) + } +} diff --git a/dsp/src/trig.rs b/dsp/src/cossin.rs similarity index 52% rename from dsp/src/trig.rs rename to dsp/src/cossin.rs index 3f96609..f9cc42e 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/cossin.rs @@ -3,90 +3,6 @@ use core::f64::consts::PI; include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); -/// 2-argument arctangent function. -/// -/// This implementation uses all integer arithmetic for fast -/// computation. It is designed to have high accuracy near the axes -/// and lower away from the axes. It is additionally designed so that -/// the error changes slowly with respect to the angle. -/// -/// # Arguments -/// -/// * `y` - Y-axis component. -/// * `x` - X-axis component. -/// -/// # Returns -/// -/// The angle between the x-axis and the ray to the point (x,y). The -/// result range is from i32::MIN to i32::MAX, where i32::MIN -/// represents -pi and, equivalently, +pi. i32::MAX represents one -/// count less than +pi. -pub fn atan2(y: i32, x: i32) -> i32 { - let sign = (x < 0, y < 0); - - let mut y = y.wrapping_abs() as u32; - let mut x = x.wrapping_abs() as u32; - - let y_greater = y > x; - if y_greater { - core::mem::swap(&mut y, &mut x); - } - - let z = (16 - y.leading_zeros() as i32).max(0); - - x >>= z; - if x == 0 { - return 0; - } - y >>= z; - let r = (y << 16) / x; - debug_assert!(r <= 1 << 16); - - // Uses the general procedure described in the following - // Mathematics stack exchange answer: - // - // https://math.stackexchange.com/a/1105038/583981 - // - // The atan approximation method has been modified to be cheaper - // to compute and to be more compatible with integer - // arithmetic. The approximation technique used here is - // - // pi / 4 * r + C * r * (1 - abs(r)) - // - // which is taken from Rajan 2006: Efficient Approximations for - // the Arctangent Function. - // - // The least mean squared error solution is C = 0.279 (no the 0.285 that - // Rajan uses). K = C*4/pi. - // Q5 for K provides sufficient correction accuracy while preserving - // as much smoothness of the quadratic correction as possible. - const FP_K: usize = 5; - const K: u32 = (0.35489 * (1 << FP_K) as f64) as u32; - // debug_assert!(K == 11); - - // `r` is unsigned Q16.16 and <= 1 - // `angle` is signed Q1.31 with 1 << 31 == +- pi - // Since K < 0.5 and r*(1 - r) <= 0.25 the correction product can use - // 4 bits for K, and 15 bits for r and 1-r to remain within the u32 range. - let mut angle = ((r << 13) - + ((K * (r >> 1) * ((1 << 15) - (r >> 1))) >> (FP_K + 1))) - as i32; - - if y_greater { - angle = (1 << 30) - angle; - } - - if sign.0 { - angle = i32::MAX - angle; - } - - if sign.1 { - angle = angle.wrapping_neg(); - } - - angle -} - /// Compute the cosine and sine of an angle. /// This is ported from the MiSoC cossin core. /// (https://github.com/m-labs/misoc/blob/master/misoc/cores/cossin.py) @@ -153,7 +69,7 @@ pub fn cossin(phase: i32) -> Complex { sin *= -1; } - (cos, sin) + Complex(cos, sin) } #[cfg(test)] @@ -161,66 +77,21 @@ mod tests { use super::*; use core::f64::consts::PI; - fn angle_to_axis(angle: f64) -> f64 { - let angle = angle % (PI / 2.); - (PI / 2. - angle).min(angle) - } - - #[test] - fn atan2_absolute_error() { - const N: usize = 321; - let mut test_vals = [0i32; N + 4]; - let scale = (1i64 << 31) as f64; - for i in 0..N { - test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32; - } - - assert!(test_vals.contains(&i32::MIN)); - test_vals[N] = i32::MAX; - test_vals[N + 1] = 0; - test_vals[N + 2] = -1; - test_vals[N + 3] = 1; - - let mut rms_err = 0f64; - let mut abs_err = 0f64; - let mut rel_err = 0f64; - - for &x in test_vals.iter() { - for &y in test_vals.iter() { - let want = (y as f64 / scale).atan2(x as f64 / scale); - let have = atan2(y, x) as f64 * PI / scale; - - let err = (have - want).abs(); - abs_err = abs_err.max(err); - rms_err += err * err; - if err > 3e-5 { - rel_err = rel_err.max(err / angle_to_axis(want)); - } - } - } - rms_err = rms_err.sqrt() / test_vals.len() as f64; - println!("max abs err: {:.2e}", abs_err); - println!("rms abs err: {:.2e}", rms_err); - println!("max rel err: {:.2e}", rel_err); - assert!(abs_err < 5e-3); - assert!(rms_err < 3e-3); - assert!(rel_err < 0.6); - } - #[test] fn cossin_error_max_rms_all_phase() { // Constant amplitude error due to LUT data range. - const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as f64; - const MAX_PHASE: f64 = (1i64 << 32) as f64; - let mut rms_err: Complex = (0., 0.); - let mut sum_err: Complex = (0., 0.); - let mut max_err: Complex = (0., 0.); - let mut sum: Complex = (0., 0.); - let mut demod: Complex = (0., 0.); + const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as _; + const MAX_PHASE: f64 = (1i64 << 32) as _; + let mut rms_err = Complex(0f64, 0f64); + let mut sum_err = Complex(0f64, 0f64); + let mut max_err = Complex(0f64, 0f64); + let mut sum = Complex(0f64, 0f64); + let mut demod = Complex(0f64, 0f64); // use std::{fs::File, io::{BufWriter, prelude::*}, path::Path}; // let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap()); + // log2 of the number of phase values to check const PHASE_DEPTH: usize = 20; for phase in 0..(1 << PHASE_DEPTH) { diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs index 1a4a6a9..ea78dd6 100644 --- a/dsp/src/iir_int.rs +++ b/dsp/src/iir_int.rs @@ -1,6 +1,7 @@ use serde::{Deserialize, Serialize}; -pub type IIRState = [i32; 5]; +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct IIRState(pub [i32; 5]); fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { // Rounding bias, half up @@ -18,7 +19,7 @@ fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { /// See `dsp::iir::IIR` for general implementation details. /// Offset and limiting disabled to suit lowpass applications. /// Coefficient scaling fixed and optimized. -#[derive(Copy, Clone, Deserialize, Serialize)] +#[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIR { pub ba: IIRState, // pub y_offset: i32, @@ -27,9 +28,9 @@ pub struct IIR { } impl IIR { - /// Coefficient fixed point: signed Q2.30. - /// Tailored to low-passes PI, II etc. - const SHIFT: u32 = 30; + /// Coefficient fixed point format: signed Q2.30. + /// Tailored to low-passes, PI, II etc. + pub const SHIFT: u32 = 30; /// Feed a new input value into the filter, update the filter state, and /// return the new output. Only the state `xy` is modified. @@ -38,21 +39,21 @@ impl IIR { /// * `xy` - Current filter state. /// * `x0` - New input. pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 { - let n = self.ba.len(); - debug_assert!(xy.len() == n); + let n = self.ba.0.len(); + debug_assert!(xy.0.len() == n); // `xy` contains x0 x1 y0 y1 y2 // Increment time x1 x2 y1 y2 y3 // Shift x1 x1 x2 y1 y2 // This unrolls better than xy.rotate_right(1) - xy.copy_within(0..n - 1, 1); + xy.0.copy_within(0..n - 1, 1); // Store x0 x0 x1 x2 y1 y2 - xy[0] = x0; + xy.0[0] = x0; // Compute y0 by multiply-accumulate - let y0 = macc(0, xy, &self.ba, IIR::SHIFT); + let y0 = macc(0, &xy.0, &self.ba.0, IIR::SHIFT); // Limit y0 // let y0 = y0.max(self.y_min).min(self.y_max); // Store y0 x0 x1 y0 y1 y2 - xy[n / 2] = y0; + xy.0[n / 2] = y0; y0 } } diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 77d8bc1..56a3004 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -3,8 +3,6 @@ use core::ops::{Add, Mul, Neg}; -pub type Complex = (T, T); - /// Bit shift, round up half. /// /// # Arguments @@ -114,12 +112,19 @@ where .fold(y0, |y, xa| y + xa) } +mod atan2; +mod complex; +mod cossin; pub mod iir; pub mod iir_int; +pub mod lockin; pub mod pll; pub mod reciprocal_pll; -pub mod trig; pub mod unwrap; +pub use atan2::atan2; +pub use complex::Complex; +pub use cossin::cossin; + #[cfg(test)] -mod testing; +pub mod testing; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs new file mode 100644 index 0000000..72f505c --- /dev/null +++ b/dsp/src/lockin.rs @@ -0,0 +1,1067 @@ +use super::{cossin, iir_int, Complex}; +use serde::{Deserialize, Serialize}; + +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct Lockin { + iir: iir_int::IIR, + iir_state: [iir_int::IIRState; 2], +} + +impl Lockin { + pub fn new(ba: &iir_int::IIRState) -> Self { + let mut iir = iir_int::IIR::default(); + iir.ba.0.copy_from_slice(&ba.0); + Lockin { + iir, + iir_state: [iir_int::IIRState::default(); 2], + } + } + + pub fn update(&mut self, signal: i32, phase: i32) -> Complex { + // Get the LO signal for demodulation. + let m = cossin(phase); + + // Mix with the LO signal, filter with the IIR lowpass, + // return IQ (in-phase and quadrature) data. + // Note: 32x32 -> 64 bit multiplications are pretty much free. + Complex( + self.iir.update( + &mut self.iir_state[0], + ((signal as i64 * m.0 as i64) >> 32) as _, + ), + self.iir.update( + &mut self.iir_state[1], + ((signal as i64 * m.1 as i64) >> 32) as _, + ), + ) + } +} + +#[cfg(test)] +mod test { + use crate::{ + atan2, + iir_int::{IIRState, IIR}, + lockin::Lockin, + reciprocal_pll::TimestampHandler, + testing::{isclose, max_error}, + Complex, + }; + + use std::f64::consts::PI; + use std::vec::Vec; + + /// ADC full scale in machine units (16 bit signed). + const ADC_SCALE: f64 = ((1 << 15) - 1) as _; + + struct PllLockin { + harmonic: i32, + phase: i32, + lockin: Lockin, + } + + impl PllLockin { + pub fn new(harmonic: i32, phase: i32, iir: &IIRState) -> Self { + PllLockin { + harmonic, + phase, + lockin: Lockin::new(iir), + } + } + + pub fn update( + &mut self, + input: Vec, + phase: i32, + frequency: i32, + ) -> Complex { + let sample_frequency = frequency.wrapping_mul(self.harmonic); + let mut sample_phase = + self.phase.wrapping_add(phase.wrapping_mul(self.harmonic)); + input + .iter() + .map(|&s| { + let input = (s as i32) << 16; + let signal = + self.lockin.update(input, sample_phase.wrapping_neg()); + sample_phase = sample_phase.wrapping_add(sample_frequency); + signal + }) + .last() + .unwrap_or(Complex::default()) + } + } + + /// Single-frequency sinusoid. + #[derive(Copy, Clone)] + struct Tone { + // Frequency (in Hz). + frequency: f64, + // Phase offset (in radians). + phase: 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, + } + + /// Convert dBFS to a linear ratio. + fn linear(dbfs: f64) -> f64 { + 10f64.powf(dbfs / 20.) + } + + impl Tone { + fn eval(&self, time: f64) -> f64 { + linear(self.amplitude_dbfs) + * (self.phase + self.frequency * time).cos() + } + } + + /// Generate a full batch of samples with size `sample_buffer_size` starting at `time_offset`. + fn sample_tones( + tones: &Vec, + time_offset: f64, + sample_buffer_size: u32, + ) -> Vec { + (0..sample_buffer_size) + .map(|i| { + let time = 2. * PI * (time_offset + i as f64); + let x: f64 = tones.iter().map(|t| t.eval(time)).sum(); + assert!(-1. < x && x < 1.); + (x * ADC_SCALE) as i16 + }) + .collect() + } + + /// Total maximum noise amplitude of the input signal after 2nd order lowpass filter. + /// Constructive interference is assumed. + /// + /// # Args + /// * `tones` - Noise sources at the ADC input. + /// * `frequency` - Frequency of the signal of interest. + /// * `corner` - Low-pass filter 3dB corner cutoff frequency. + /// + /// # Returns + /// Upper bound of the total amplitude of all noise sources in linear units full scale. + fn sampled_noise_amplitude( + tones: &Vec, + frequency: f64, + corner: f64, + ) -> f64 { + tones + .iter() + .map(|t| { + let df = (t.frequency - frequency) / corner; + // Assuming a 2nd order lowpass filter: 40dB/decade. + linear(t.amplitude_dbfs - 40. * df.abs().max(1.).log10()) + }) + .sum::() + .max(1. / ADC_SCALE / 2.) // 1/2 LSB from quantization + } + + /// Reference clock timestamp values in one ADC batch period starting at `timestamp_start`. The + /// number of timestamps in a batch can be 0 or 1, so this returns an Option containing a timestamp + /// only if one occurred during the batch. + /// + /// # Args + /// * `reference_period` - External reference signal period in units of the internal clock period. + /// * `timestamp_start` - Start time in terms of the internal clock count. This is the start time of + /// the current processing sequence. + /// * `timestamp_stop` - Stop time in terms of the internal clock count. + /// + /// # Returns + /// An Option, containing a timestamp if one occurred during the current batch period. + fn adc_batch_timestamps( + reference_period: f64, + timestamp_start: u64, + timestamp_stop: u64, + ) -> Option { + let start_count = timestamp_start as f64 % reference_period; + + let timestamp = (reference_period - start_count) % reference_period; + + if timestamp < (timestamp_stop - timestamp_start) as f64 { + return Some( + ((timestamp_start + timestamp.round() as u64) % (1u64 << 32)) + as u32, + ); + } + + None + } + + /// Lowpass biquad filter using cutoff and sampling frequencies. Taken from: + /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html + /// + /// # Args + /// * `fc` - Corner frequency, or 3dB cutoff frequency (in units of sample rate). + /// * `q` - Quality factor (1/sqrt(2) for critical). + /// * `k` - DC gain. + /// + /// # Returns + /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1. + fn lowpass_iir_coefficients(fc: f64, q: f64, k: f64) -> IIRState { + let f = 2. * PI * fc; + let a = f.sin() / (2. * q); + // IIR uses Q2.30 fixed point + let a0 = (1. + a) / (1 << IIR::SHIFT) as f64; + let b0 = (k / 2. * (1. - f.cos()) / a0).round() as _; + let a1 = (2. * f.cos() / a0).round() as _; + let a2 = ((a - 1.) / a0).round() as _; + + IIRState([b0, 2 * b0, b0, a1, a2]) + } + + /// 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. + /// + /// # Args + /// * `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. + /// + /// # Args + /// * `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`. + /// + /// # Args + /// * `internal_frequency` - Internal clock frequency (Hz). The internal clock increments timestamp + /// counter values used to record the edges of the external reference. + /// * `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. + /// * `sample_buffer_size_log2` - The base-2 logarithm of the number of samples in a processing + /// batch. + /// * `pll_shift_frequency` - See `pll::update()`. + /// * `pll_shift_phase` - See `pll::update()`. + /// * `corner_frequency` - Lowpass filter 3dB cutoff frequency. + /// * `desired_input` - `Tone` giving the frequency, amplitude and phase of the desired result. + /// * `noise_inputs` - Vector of `Tone` 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. This is added + /// to fixed tolerance values computed inside this function. The outputs must remain within this + /// tolerance between `time_constant_factor` and `time_constant_factor+1` time constants. + fn lowpass_test( + internal_frequency: f64, + reference_frequency: f64, + demodulation_phase_offset: f64, + harmonic: i32, + sample_buffer_size_log2: usize, + pll_shift_frequency: u8, + pll_shift_phase: u8, + corner_frequency: f64, + desired_input: Tone, + tones: &mut Vec, + time_constant_factor: f64, + tolerance: f64, + ) { + assert!( + isclose((internal_frequency).log2(), (internal_frequency).log2().round(), 0., 1e-5), + "The number of internal clock cycles in one ADC sampling period must be a power-of-two." + ); + + assert!( + internal_frequency / reference_frequency + >= internal_frequency + * (1 << sample_buffer_size_log2) as f64, + "Too many timestamps per batch. Each batch can have at most 1 timestamp." + ); + + let adc_sample_ticks_log2 = + (internal_frequency).log2().round() as usize; + assert!( + adc_sample_ticks_log2 + sample_buffer_size_log2 <= 32, + "The base-2 log of the number of ADC ticks in a sampling period plus the base-2 log of the sample buffer size must be less than 32." + ); + + let mut lockin = PllLockin::new( + harmonic, + (demodulation_phase_offset / (2. * PI) * (1i64 << 32) as f64) + .round() as i32, + &lowpass_iir_coefficients( + corner_frequency, + 1. / 2f64.sqrt(), // critical q + 2., + ), // DC gain to get to full scale with the image filtered out + ); + let mut timestamp_handler = TimestampHandler::new( + pll_shift_frequency, + pll_shift_phase, + adc_sample_ticks_log2, + sample_buffer_size_log2, + ); + + let mut timestamp_start: u64 = 0; + let time_constant: f64 = 1. / (2. * PI * corner_frequency); + // Account for the pll settling time (see its documentation). + let pll_time_constant_samples = + (1 << pll_shift_phase.max(pll_shift_frequency)) as usize; + let low_pass_time_constant_samples = + (time_constant_factor * time_constant + / (1 << sample_buffer_size_log2) as f64) as usize; + let samples = + pll_time_constant_samples + low_pass_time_constant_samples; + // Ensure the result remains within tolerance for 1 time constant after `time_constant_factor` + // time constants. + let extra_samples = time_constant as usize; + let batch_sample_count = + 1_u64 << (adc_sample_ticks_log2 + sample_buffer_size_log2); + + let effective_phase_offset = + desired_input.phase - 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( + tones, + reference_frequency * harmonic as f64, + corner_frequency, + ); + // Add some fixed error to account for errors introduced by the PLL, our custom trig functions + // and integer division. It's a bit difficult to be precise about this. I've added a 1% + // (relative to full scale) error. + let total_magnitude_noise = magnitude_noise( + total_noise_amplitude, + in_phase_actual, + quadrature_actual, + linear(desired_input.amplitude_dbfs), + ) + 1e-2; + let total_phase_noise = phase_noise( + total_noise_amplitude, + in_phase_actual, + quadrature_actual, + ) + 1e-2 * 2. * PI; + + tones.push(desired_input); + + for n in 0..(samples + extra_samples) { + let adc_signal = sample_tones( + &tones, + timestamp_start as f64 / internal_frequency, + 1 << sample_buffer_size_log2, + ); + let timestamp = adc_batch_timestamps( + internal_frequency / reference_frequency, + timestamp_start, + timestamp_start + batch_sample_count - 1, + ); + timestamp_start += batch_sample_count; + + let (demodulation_initial_phase, demodulation_frequency) = + timestamp_handler.update(timestamp); + let output = lockin.update( + adc_signal, + demodulation_initial_phase as i32, + demodulation_frequency as i32, + ); + let magnitude = (((output.0 as i64) * (output.0 as i64) + + (output.1 as i64) * (output.1 as i64)) + >> 32) as i32; + let phase = atan2(output.1, output.0); + + // Ensure stable within tolerance for 1 time constant after `time_constant_factor`. + if n >= samples { + // We want our full-scale magnitude to be 1. Our fixed-point numbers treated as integers + // set the full-scale magnitude to 1<<60. So, we must divide by this number. However, + // we've already divided by 1<<32 in the magnitude computation to keep our values within + // the i32 limits, so we just need to divide by an additional 1<<28. + let amplitude_normalized = + (magnitude as f64 / (1_u64 << 28) as f64).sqrt(); + assert!( + isclose(linear(desired_input.amplitude_dbfs), amplitude_normalized, tolerance, total_magnitude_noise), + "magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}", + linear(desired_input.amplitude_dbfs), + desired_input.amplitude_dbfs, + amplitude_normalized, + 20.*amplitude_normalized.log10(), + max_error(linear(desired_input.amplitude_dbfs), amplitude_normalized, tolerance, total_magnitude_noise), + ); + let phase_normalized = + phase as f64 / (1_u64 << 32) as f64 * (2. * PI); + assert!( + isclose( + effective_phase_offset, + phase_normalized, + tolerance, + total_phase_noise + ), + "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", + effective_phase_offset, + phase_normalized, + max_error( + effective_phase_offset, + phase_normalized, + tolerance, + total_phase_noise + ), + ); + + let in_phase_normalized = output.0 as f64 / (1 << 30) as f64; + let quadrature_normalized = output.1 as f64 / (1 << 30) as f64; + + assert!( + isclose( + in_phase_actual, + in_phase_normalized, + total_noise_amplitude, + tolerance + ), + "in-phase actual: {:.4}, in-phase computed: {:.3}, tolerance: {:.4}", + in_phase_actual, + in_phase_normalized, + max_error( + in_phase_actual, + in_phase_normalized, + total_noise_amplitude, + tolerance + ), + ); + assert!( + isclose( + quadrature_actual, + quadrature_normalized, + total_noise_amplitude, + tolerance + ), + "quadrature actual: {:.4}, quadrature computed: {:.4}, tolerance: {:.4}", + quadrature_actual, + quadrature_normalized, + max_error( + quadrature_actual, + quadrature_normalized, + total_noise_amplitude, + tolerance + ), + ); + } + } + } + + #[test] + fn lowpass() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 64e-3; + let harmonic: i32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 3; + let pll_shift_phase: u8 = 2; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 6.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: 0., + }, + &mut vec![ + Tone { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_demodulation_phase_offset_pi_2() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 64e-3; + let harmonic: i32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 3; + let pll_shift_phase: u8 = 2; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = PI / 2.; + let time_constant_factor: f64 = 6.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: 0., + }, + &mut vec![ + Tone { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_phase_offset_pi_2() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 64e-3; + let harmonic: i32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 3; + let pll_shift_phase: u8 = 2; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 6.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: PI / 2., + }, + &mut vec![ + Tone { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_fundamental_71e_3_phase_offset_pi_4() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 71e-3; + let harmonic: i32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 3; + let pll_shift_phase: u8 = 2; + let corner_frequency: f64 = 0.6e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: PI / 4., + }, + &mut vec![ + Tone { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_first_harmonic() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 50e-3; + let harmonic: i32 = 2; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 1; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: 0., + }, + &mut vec![ + Tone { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_second_harmonic() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 50e-3; + let harmonic: i32 = 3; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 1; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: 0., + }, + &mut vec![ + Tone { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_third_harmonic() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 50e-3; + let harmonic: i32 = 4; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 1; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: 0., + }, + &mut vec![ + Tone { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_first_harmonic_phase_shift() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 50e-3; + let harmonic: i32 = 2; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 1; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: PI / 4., + }, + &mut vec![ + Tone { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_adc_frequency_1e6() { + let internal_frequency: f64 = 32.; + let signal_frequency: f64 = 100e-3; + let harmonic: i32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 1; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: 0., + }, + &mut vec![ + Tone { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_internal_frequency_125e6() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 100e-3; + let harmonic: i32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 1; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: 0., + }, + &mut vec![ + Tone { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + Tone { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }, + ], + time_constant_factor, + tolerance, + ); + } + + #[test] + fn lowpass_low_signal_frequency() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 10e-3; + let harmonic: i32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 1; + let corner_frequency: f64 = 1e-3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f64 = 1e-1; + + lowpass_test( + internal_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + Tone { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase: 0., + }, + &mut vec![Tone { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase: 0., + }], + time_constant_factor, + tolerance, + ); + } +} diff --git a/dsp/src/reciprocal_pll.rs b/dsp/src/reciprocal_pll.rs index eaeaac7..d06abbb 100644 --- a/dsp/src/reciprocal_pll.rs +++ b/dsp/src/reciprocal_pll.rs @@ -62,18 +62,26 @@ impl TimestampHandler { self.reference_frequency = frequency as u32 as i64; } - let demodulation_frequency = divide_round( - 1 << (32 + self.adc_sample_ticks_log2), - self.reference_frequency, - ) as u32; - let demodulation_initial_phase = divide_round( - (((self.batch_index as i64) - << (self.adc_sample_ticks_log2 - + self.sample_buffer_size_log2)) - - self.reference_phase) - << 32, - self.reference_frequency, - ) as u32; + let demodulation_frequency: u32; + let demodulation_initial_phase: u32; + + if self.reference_frequency == 0 { + demodulation_frequency = u32::MAX; + demodulation_initial_phase = u32::MAX; + } else { + demodulation_frequency = divide_round( + 1 << (32 + self.adc_sample_ticks_log2), + self.reference_frequency, + ) as u32; + demodulation_initial_phase = divide_round( + (((self.batch_index as i64) + << (self.adc_sample_ticks_log2 + + self.sample_buffer_size_log2)) + - self.reference_phase) + << 32, + self.reference_frequency, + ) as u32; + } if self.batch_index < (1 << (32 diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs index 4a14f22..f8e753d 100644 --- a/dsp/src/testing.rs +++ b/dsp/src/testing.rs @@ -1,6 +1,22 @@ #![allow(dead_code)] use super::Complex; +/// Maximum acceptable error between a computed and actual value given fixed and relative +/// tolerances. +/// +/// # Args +/// * `a` - First input. +/// * `b` - Second input. The relative tolerance is computed with respect to the maximum of the +/// absolute values of the first and second inputs. +/// * `rtol` - Relative tolerance. +/// * `atol` - Fixed tolerance. +/// +/// # Returns +/// Maximum acceptable error. +pub fn max_error(a: f64, b: f64, rtol: f64, atol: f64) -> f64 { + rtol * a.abs().max(b.abs()) + atol +} + pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool { (a - b).abs() <= a.abs().max(b.abs()) * rtol + atol } diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs new file mode 100644 index 0000000..304c35e --- /dev/null +++ b/src/bin/lockin.rs @@ -0,0 +1,353 @@ +#![deny(warnings)] +#![no_std] +#![no_main] +#![cfg_attr(feature = "nightly", feature(core_intrinsics))] + +use stm32h7xx_hal as hal; + +#[macro_use] +extern crate log; + +use rtic::cyccnt::{Instant, U32Ext}; + +use heapless::{consts::*, String}; + +use stabilizer::{ + hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2, +}; + +use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler}; +use hardware::{ + Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, +}; + +const SCALE: f32 = ((1 << 15) - 1) as f32; + +const TCP_RX_BUFFER_SIZE: usize = 8192; +const TCP_TX_BUFFER_SIZE: usize = 8192; + +// The number of cascaded IIR biquads per channel. Select 1 or 2! +const IIR_CASCADE_LENGTH: usize = 1; + +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] +const APP: () = { + struct Resources { + afes: (AFE0, AFE1), + adcs: (Adc0Input, Adc1Input), + dacs: (Dac0Output, Dac1Output), + net_interface: hardware::Ethernet, + + // 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], + + timestamper: InputStamper, + pll: TimestampHandler, + lockin: Lockin, + } + + #[init] + fn init(c: init::Context) -> init::LateResources { + // Configure the microcontroller + let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); + + let pll = TimestampHandler::new( + 4, // relative PLL frequency bandwidth: 2**-4, TODO: expose + 3, // relative PLL phase bandwidth: 2**-3, TODO: expose + ADC_SAMPLE_TICKS_LOG2 as usize, + SAMPLE_BUFFER_SIZE_LOG2, + ); + + let lockin = Lockin::new( + &iir_int::IIRState::default(), // TODO: lowpass, expose + ); + + // Enable ADC/DAC events + stabilizer.adcs.0.start(); + stabilizer.adcs.1.start(); + stabilizer.dacs.0.start(); + stabilizer.dacs.1.start(); + + // Start recording digital input timestamps. + stabilizer.timestamp_timer.start(); + + // Start sampling ADCs. + stabilizer.adc_dac_timer.start(); + + init::LateResources { + afes: stabilizer.afes, + adcs: stabilizer.adcs, + dacs: stabilizer.dacs, + net_interface: stabilizer.net.interface, + timestamper: stabilizer.timestamper, + + pll, + lockin, + } + } + + /// Main DSP processing routine for Stabilizer. + /// + /// # Note + /// Processing time for the DSP application code is bounded by the following constraints: + /// + /// DSP application code starts after the ADC has generated a batch of samples and must be + /// completed by the time the next batch of ADC samples has been acquired (plus the FIFO buffer + /// time). If this constraint is not met, firmware will panic due to an ADC input overrun. + /// + /// The DSP application code must also fill out the next DAC output buffer in time such that the + /// DAC can switch to it when it has completed the current buffer. If this constraint is not met + /// it's possible that old DAC codes will be generated on the output and the output samples will + /// be delayed by 1 batch. + /// + /// Because the ADC and DAC operate at the same rate, these two constraints actually implement + /// the same time bounds, meeting one also means the other is also met. + /// + /// TODO: document lockin + #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, lockin, timestamper, pll], priority=2)] + fn process(c: process::Context) { + let adc_samples = [ + c.resources.adcs.0.acquire_buffer(), + c.resources.adcs.1.acquire_buffer(), + ]; + + let dac_samples = [ + c.resources.dacs.0.acquire_buffer(), + c.resources.dacs.1.acquire_buffer(), + ]; + + let iir_ch = c.resources.iir_ch; + let iir_state = c.resources.iir_state; + let lockin = c.resources.lockin; + + let (pll_phase, pll_frequency) = c + .resources + .pll + .update(c.resources.timestamper.latest_timestamp()); + + // Harmonic index of the LO: -1 to _de_modulate the fundamental + let harmonic: i32 = -1; + // Demodulation LO phase offset + let phase_offset: i32 = 0; + let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic); + let mut sample_phase = phase_offset + .wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); + + for i in 0..adc_samples[0].len() { + // Convert to signed, MSB align the ADC sample. + let input = (adc_samples[0][i] as i16 as i32) << 16; + // Obtain demodulated, filtered IQ sample. + let output = lockin.update(input, sample_phase); + // Advance the sample phase. + sample_phase = sample_phase.wrapping_add(sample_frequency); + + // Convert from IQ to power and phase. + let mut power = output.power() as _; + let mut phase = output.phase() as _; + + // Filter power and phase through IIR filters. + // Note: Normalization to be done in filters. Phase will wrap happily. + for j in 0..iir_state[0].len() { + power = iir_ch[0][j].update(&mut iir_state[0][j], power); + phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); + } + + // Note(unsafe): range clipping to i16 is ensured by IIR filters above. + // Convert to DAC data. + unsafe { + dac_samples[0][i] = + power.to_int_unchecked::() as u16 ^ 0x8000; + dac_samples[1][i] = + phase.to_int_unchecked::() as u16 ^ 0x8000; + } + } + } + + #[idle(resources=[net_interface, iir_state, iir_ch, afes])] + fn idle(mut c: idle::Context) -> ! { + let mut socket_set_entries: [_; 8] = Default::default(); + let mut sockets = + smoltcp::socket::SocketSet::new(&mut socket_set_entries[..]); + + let mut rx_storage = [0; TCP_RX_BUFFER_SIZE]; + let mut tx_storage = [0; TCP_TX_BUFFER_SIZE]; + let tcp_handle = { + let tcp_rx_buffer = + smoltcp::socket::TcpSocketBuffer::new(&mut rx_storage[..]); + let tcp_tx_buffer = + smoltcp::socket::TcpSocketBuffer::new(&mut tx_storage[..]); + let tcp_socket = + smoltcp::socket::TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer); + sockets.add(tcp_socket) + }; + + let mut server = server::Server::new(); + + let mut time = 0u32; + let mut next_ms = Instant::now(); + + // TODO: Replace with reference to CPU clock from CCDR. + next_ms += 400_000.cycles(); + + loop { + let tick = Instant::now() > next_ms; + + if tick { + next_ms += 400_000.cycles(); + time += 1; + } + + { + let socket = + &mut *sockets.get::(tcp_handle); + if socket.state() == smoltcp::socket::TcpState::CloseWait { + socket.close(); + } else if !(socket.is_open() || socket.is_listening()) { + socket + .listen(1235) + .unwrap_or_else(|e| warn!("TCP listen error: {:?}", e)); + } else { + server.poll(socket, |req| { + info!("Got request: {:?}", req); + stabilizer::route_request!(req, + readable_attributes: [ + "stabilizer/iir/state": (|| { + let state = c.resources.iir_state.lock(|iir_state| + server::Status { + t: time, + 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) + }), + "stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()), + "stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain()) + ], + + modifiable_attributes: [ + "stabilizer/iir0/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][0] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir1/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][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) + }) + }), + "stabilizer/afe0/gain": hardware::AfeGain, (|gain| { + c.resources.afes.0.set_gain(gain); + Ok::<(), ()>(()) + }), + "stabilizer/afe1/gain": hardware::AfeGain, (|gain| { + c.resources.afes.1.set_gain(gain); + Ok::<(), ()>(()) + }) + ] + ) + }); + } + } + + let sleep = match c.resources.net_interface.poll( + &mut sockets, + smoltcp::time::Instant::from_millis(time as i64), + ) { + Ok(changed) => !changed, + Err(smoltcp::Error::Unrecognized) => true, + Err(e) => { + info!("iface poll error: {:?}", e); + true + } + }; + + if sleep { + cortex_m::asm::wfi(); + } + } + } + + #[task(binds = ETH, priority = 1)] + fn eth(_: eth::Context) { + unsafe { hal::ethernet::interrupt_handler() } + } + + #[task(binds = SPI2, priority = 3)] + fn spi2(_: spi2::Context) { + panic!("ADC0 input overrun"); + } + + #[task(binds = SPI3, priority = 3)] + fn spi3(_: spi3::Context) { + panic!("ADC0 input overrun"); + } + + #[task(binds = SPI4, priority = 3)] + fn spi4(_: spi4::Context) { + panic!("DAC0 output error"); + } + + #[task(binds = SPI5, priority = 3)] + fn spi5(_: spi5::Context) { + panic!("DAC1 output error"); + } + + extern "C" { + // hw interrupt handlers for RTIC to use for scheduling tasks + // one per priority + fn DCMI(); + fn JPEG(); + fn SDMMC(); + } +}; diff --git a/src/hardware/mod.rs b/src/hardware/mod.rs index 6434dbc..dc3aa25 100644 --- a/src/hardware/mod.rs +++ b/src/hardware/mod.rs @@ -20,6 +20,7 @@ mod timers; pub use adc::{Adc0Input, Adc1Input}; pub use afe::Gain as AfeGain; pub use dac::{Dac0Output, Dac1Output}; +pub use digital_input_stamper::InputStamper; pub use pounder::DdsOutput; // Type alias for the analog front-end (AFE) for ADC0. diff --git a/src/lib.rs b/src/lib.rs index b685f7a..254e626 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -9,7 +9,9 @@ pub mod server; // The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is // equal to 10ns per tick. // Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz -const ADC_SAMPLE_TICKS: u16 = 256; +pub const ADC_SAMPLE_TICKS_LOG2: u16 = 8; +pub const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2; // The desired ADC sample processing buffer size. -const SAMPLE_BUFFER_SIZE: usize = 8; +pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 3; +pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;