From c0f6c2d44509f986ff41309e1040ff01b365b02b Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 12 Jan 2021 04:03:23 +0000 Subject: [PATCH 01/25] build(deps): bump log from 0.4.11 to 0.4.13 Bumps [log](https://github.com/rust-lang/log) from 0.4.11 to 0.4.13. - [Release notes](https://github.com/rust-lang/log/releases) - [Changelog](https://github.com/rust-lang/log/blob/master/CHANGELOG.md) - [Commits](https://github.com/rust-lang/log/compare/0.4.11...0.4.13) Signed-off-by: dependabot[bot] --- Cargo.lock | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 76fb832..1dd15da 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -520,9 +520,9 @@ checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a" [[package]] name = "log" -version = "0.4.11" +version = "0.4.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fabed175da42fed1fa0746b0ea71f412aa9d35e76e95e59b192c64b9dc2bf8b" +checksum = "fcf3805d4480bb5b86070dcfeb9e2cb2ebc148adb753c5cca5f884d1d65a42b2" dependencies = [ "cfg-if 0.1.10", ] From 184a343a7a9603abcdaa3b01690bc566e152f9cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 9 Jan 2021 20:00:02 +0100 Subject: [PATCH 02/25] hitl: dispatch entire github object --- .github/workflows/ci.yml | 2 +- .github/workflows/hitl.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ae27f70..33acca9 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -79,7 +79,7 @@ jobs: - uses: actions/upload-artifact@v2 if: ${{ matrix.toolchain == 'stable' }} with: - name: stabilizer_${{ github.sha }} + name: bin path: | target/*/release/stabilizer stabilizer-release.bin diff --git a/.github/workflows/hitl.yml b/.github/workflows/hitl.yml index 3073f66..f59e8d2 100644 --- a/.github/workflows/hitl.yml +++ b/.github/workflows/hitl.yml @@ -15,4 +15,4 @@ jobs: token: ${{ secrets.DISPATCH_PAT }} event-type: stabilizer repository: quartiq/hitl - client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}' + client-payload: '{"github": ${{ toJson(github) }}}' From 9d0aa40ce883e062882f19902172f53aefbded20 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 13 Jan 2021 09:50:27 -0800 Subject: [PATCH 03/25] Revert "revert changes in main.rs and server.rs" This reverts commit e599977983dd097f9854c4f553a67ba3b55c207a. --- src/main.rs | 109 ++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 89 insertions(+), 20 deletions(-) diff --git a/src/main.rs b/src/main.rs index c5a9698..d128357 100644 --- a/src/main.rs +++ b/src/main.rs @@ -60,14 +60,21 @@ use heapless::{consts::*, String}; // 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; +const ADC_SAMPLE_TICKS_LOG2: u16 = 8; +const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2; // The desired ADC sample processing buffer size. -const SAMPLE_BUFFER_SIZE: usize = 8; +const SAMPLE_BUFFER_SIZE_LOG2: usize = 3; +const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2; // The number of cascaded IIR biquads per channel. Select 1 or 2! const IIR_CASCADE_LENGTH: usize = 1; +// Frequency scaling factor for lock-in harmonic demodulation. +const HARMONIC: u32 = 1; +// Phase offset applied to the lock-in demodulation signal. +const PHASE_OFFSET: u32 = 0; + #[link_section = ".sram3.eth"] static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); @@ -84,7 +91,12 @@ mod timers; use adc::{Adc0Input, Adc1Input}; use dac::{Dac0Output, Dac1Output}; -use dsp::iir; +use dsp::{ + iir, iir_int, + reciprocal_pll::TimestampHandler, + shift_round, + trig::{atan2, cossin}, +}; use pounder::DdsOutput; #[cfg(not(feature = "semihosting"))] @@ -205,6 +217,9 @@ const APP: () = { adcs: (Adc0Input, Adc1Input), dacs: (Dac0Output, Dac1Output), input_stamper: digital_input_stamper::InputStamper, + timestamp_handler: TimestampHandler, + iir_lockin: iir_int::IIR, + iir_state_lockin: [iir_int::IIRState; 2], eeprom_i2c: hal::i2c::I2c, @@ -927,6 +942,17 @@ const APP: () = { #[cfg(not(feature = "pounder_v1_1"))] let pounder_stamper = None; + let timestamp_handler = TimestampHandler::new( + 4, + 3, + ADC_SAMPLE_TICKS_LOG2 as usize, + SAMPLE_BUFFER_SIZE_LOG2, + ); + let iir_lockin = iir_int::IIR { + ba: [1, 0, 0, 0, 0], + }; + let iir_state_lockin = [[0; 5]; 2]; + // Start sampling ADCs. sampling_timer.start(); timestamp_timer.start(); @@ -942,6 +968,10 @@ const APP: () = { pounder: pounder_devices, pounder_stamper, + timestamp_handler, + iir_lockin, + iir_state_lockin, + eeprom_i2c, net_interface: network_interface, eth_mac, @@ -949,7 +979,7 @@ const APP: () = { } } - #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper], priority=2)] + #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper, timestamp_handler, iir_lockin, iir_state_lockin], priority=2)] fn process(c: process::Context) { if let Some(stamper) = c.resources.pounder_stamper { let pounder_timestamps = stamper.acquire_buffer(); @@ -965,23 +995,63 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; - let _timestamp = c.resources.input_stamper.latest_timestamp(); + let (demodulation_initial_phase, demodulation_frequency) = c + .resources + .timestamp_handler + .update(c.resources.input_stamper.latest_timestamp()); - 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 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); + let [dac0, dac1] = dac_samples; + let iir_lockin = c.resources.iir_lockin; + let iir_state_lockin = c.resources.iir_state_lockin; + let iir_ch = c.resources.iir_ch; + let iir_state = c.resources.iir_state; + + dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( + |(i, (d0, d1))| { + let sample_phase = (HARMONIC.wrapping_mul( + (demodulation_frequency.wrapping_mul(i as u32)) + .wrapping_add(demodulation_initial_phase), + )) + .wrapping_add(PHASE_OFFSET); + let (cos, sin) = cossin(sample_phase as i32); + + let mut signal = (0_i32, 0_i32); + + // shift cos/sin before multiplying to avoid i64 multiplication + signal.0 = + adc_samples[0][i] as i16 as i32 * shift_round(cos, 16); + signal.1 = + adc_samples[0][i] as i16 as i32 * shift_round(sin, 16); + + signal.0 = + iir_lockin.update(&mut iir_state_lockin[0], signal.0); + signal.1 = + iir_lockin.update(&mut iir_state_lockin[1], signal.1); + + let mut magnitude = f32::from(shift_round( + signal.0 * signal.0 + signal.1 * signal.1, + 16, + ) as i16); + let mut phase = f32::from(shift_round( + atan2(signal.1, signal.0), + 16, + ) as i16); + + for j in 0..iir_state[0].len() { + magnitude = + iir_ch[0][j].update(&mut iir_state[0][j], magnitude); + phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); } - // 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::() }; - // Convert to DAC code - dac_samples[channel][sample] = y as u16 ^ 0x8000; - } - } + + unsafe { + let magnitude = magnitude.to_int_unchecked::(); + let phase = phase.to_int_unchecked::(); + + *d0 = magnitude as u16 ^ 0x8000; + *d1 = phase as u16 ^ 0x8000; + } + }, + ); if let Some(dds_output) = c.resources.dds_output { let builder = dds_output.builder().update_channels( @@ -994,7 +1064,6 @@ const APP: () = { builder.write_profile(); } - let [dac0, dac1] = dac_samples; c.resources.dacs.0.release_buffer(dac0); c.resources.dacs.1.release_buffer(dac1); } From 9f0b3eb77edff8d5080b5ccae91d2db606342a95 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 14 Jan 2021 14:41:51 -0800 Subject: [PATCH 04/25] fix shift_round overflow error --- dsp/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 77d8bc1..6f521f1 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -17,7 +17,7 @@ pub type Complex = (T, T); /// Shifted and rounded value. #[inline(always)] pub fn shift_round(x: i32, shift: usize) -> i32 { - (x + (1 << (shift - 1))) >> shift + x.saturating_add(1 << (shift - 1)) >> shift } /// Integer division, round up half. From 9a3c9afa7ec11549f29e3f81f80e80c55eb7d55e Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 14 Jan 2021 14:47:55 -0800 Subject: [PATCH 05/25] fix reciprocal_pll divide error when reference frequency is 0 --- dsp/src/reciprocal_pll.rs | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) 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 From f0eb58dfb251d412b1c0efdb96d7c081a724597c Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 14 Jan 2021 14:49:07 -0800 Subject: [PATCH 06/25] swap sin and cos for demodulation The in-phase component should be multiplied by the sin value and the quadrature component should be multiplied by the cos value. --- src/main.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main.rs b/src/main.rs index d128357..a6559f6 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1019,9 +1019,9 @@ const APP: () = { // shift cos/sin before multiplying to avoid i64 multiplication signal.0 = - adc_samples[0][i] as i16 as i32 * shift_round(cos, 16); - signal.1 = adc_samples[0][i] as i16 as i32 * shift_round(sin, 16); + signal.1 = + adc_samples[0][i] as i16 as i32 * shift_round(cos, 16); signal.0 = iir_lockin.update(&mut iir_state_lockin[0], signal.0); From 73ffc873cd91942e43ed09ed3db9c48c69e62e3d Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 14 Jan 2021 15:11:58 -0800 Subject: [PATCH 07/25] add lock-in integration test --- dsp/tests/lockin.rs | 1215 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1215 insertions(+) create mode 100644 dsp/tests/lockin.rs diff --git a/dsp/tests/lockin.rs b/dsp/tests/lockin.rs new file mode 100644 index 0000000..896f871 --- /dev/null +++ b/dsp/tests/lockin.rs @@ -0,0 +1,1215 @@ +use dsp::{ + iir_int::{IIRState, IIR}, + reciprocal_pll::TimestampHandler, + shift_round, + trig::{atan2, cossin}, + Complex, +}; + +use std::f64::consts::PI; +use std::vec::Vec; + +const ADC_MAX: f64 = 1.; +const ADC_MAX_COUNT: f64 = (1 << 15) as f64; + +struct Lockin { + harmonic: u32, + phase_offset: u32, + iir: IIR, + iir_state: [IIRState; 2], +} + +impl Lockin { + /// Construct a new `Lockin` instance. + /// + /// # Args + /// * `harmonic` - Factor for harmonic demodulation. For example, a value of 1 would demodulate + /// with the reference frequency whereas a value of 2 would demodulate with the first harmonic + /// of the reference frequency. + /// * `phase_offset` - Phase offset of the scaled (see `harmonic`) demodulation signal relative + /// to the reference signal. + /// * `iir` - IIR coefficients (see `iir_int::IIR`) used for filtering the demodulated in-phase + /// and quadrature signals. + pub fn new(harmonic: u32, phase_offset: u32, iir: IIR) -> Self { + Lockin { + harmonic, + phase_offset, + iir, + iir_state: [[0; 5]; 2], + } + } + + /// Compute the in-phase and quadrature signals. This is intended to mimic the lock-in + /// processing routine invoked in main.rs. + /// + /// # Args + /// * `adc_samples` - ADC samples. + /// * `demodulation_initial_phase` - Phase value of the demodulation signal corresponding to the + /// first ADC sample. + /// * `demodulation_frequency` - Demodulation frequency. + pub fn update( + &mut self, + adc_samples: Vec, + demodulation_initial_phase: u32, + demodulation_frequency: u32, + ) -> Complex { + let mut signal = Vec::>::new(); + + adc_samples.iter().enumerate().for_each(|(i, s)| { + let sample_phase = self + .harmonic + .wrapping_mul( + (demodulation_frequency.wrapping_mul(i as u32)) + .wrapping_add(demodulation_initial_phase), + ) + .wrapping_add(self.phase_offset); + let (cos, sin) = cossin(sample_phase as i32); + + signal.push(( + *s as i32 * shift_round(sin, 16), + *s as i32 * shift_round(cos, 16), + )); + + signal[i].0 = self.iir.update(&mut self.iir_state[0], signal[i].0); + signal[i].1 = self.iir.update(&mut self.iir_state[1], signal[i].1); + }); + + (signal[0].0, signal[0].1) + } +} + +/// 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. +/// +/// # Args +/// * `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. +/// +/// # Args +/// * `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. +/// +/// # Args +/// * `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 a full batch of ADC samples starting at `timestamp_start`. +/// +/// # Args +/// * `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). +/// * `sample_buffer_size` - The number of ADC samples in one processing batch. +/// +/// # Returns +/// The sampled signal at the ADC input. +fn adc_sampled_signal( + pure_signals: &Vec, + timestamp_start: u64, + internal_frequency: f64, + adc_frequency: f64, + sample_buffer_size: u32, +) -> Vec { + // 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 = Vec::::new(); + + for i in 0..sample_buffer_size { + signal.push(real_to_adc_sample( + amplitude + .iter() + .zip(initial_phase.iter()) + .zip(phase_increment.iter()) + .fold(0., |acc, ((a, phi), theta)| { + acc + a * (phi + theta * i as f64).sin() + }), + )); + } + + signal +} + +/// 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_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. +/// * `timestamp_stop` - Stop time in terms of the internal clock count. +/// * `internal_frequency` - Internal clock frequency (in Hz). +/// +/// # Returns +/// An Option, containing a timestamp if one occurred during the current batch period. +fn adc_batch_timestamps( + reference_frequency: f64, + timestamp_start: u64, + timestamp_stop: u64, + internal_frequency: f64, +) -> Option { + let reference_period = internal_frequency / reference_frequency; + 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 +/// * `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, +) -> [i32; 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; + + // iir uses Q2.30 fixed point + [ + (b0 * (1 << 30) as f64).round() as i32, + (b1 * (1 << 30) as f64).round() as i32, + (b2 * (1 << 30) as f64).round() as i32, + (a1 * (1 << 30) as f64).round() as i32, + (a2 * (1 << 30) as f64).round() as i32, + ] +} + +/// 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. +fn max_error(a: f64, b: f64, rtol: f64, atol: f64) -> f64 { + rtol * a.abs().max(b.abs()) + atol +} + +// TODO this is (mostly) copied from testing.rs. +pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool { + (a - b).abs() <= max_error(a, b, rtol, atol) +} + +/// 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. +/// +/// # Args +/// * `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. +/// +/// # 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. +/// * `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. +/// * `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` - `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. 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, + adc_frequency: f64, + reference_frequency: f64, + demodulation_phase_offset: f64, + harmonic: u32, + sample_buffer_size_log2: usize, + pll_shift_frequency: u8, + pll_shift_phase: u8, + corner_frequency: f64, + desired_input: PureSine, + noise_inputs: &mut Vec, + time_constant_factor: f64, + tolerance: f64, +) { + assert!( + isclose((internal_frequency / adc_frequency).log2(), (internal_frequency / adc_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 / adc_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 / adc_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 = Lockin::new( + harmonic, + (demodulation_phase_offset / (2. * PI) * (1_u64 << 32) as f64).round() + as u32, + IIR { + ba: lowpass_iir_coefficients(corner_frequency, adc_frequency), + }, + ); + 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 * adc_frequency + / (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 * adc_frequency) as usize; + let batch_sample_count = + 1_u64 << (adc_sample_ticks_log2 + sample_buffer_size_log2); + + 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, + ); + // 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; + + let pure_signals = noise_inputs; + pure_signals.push(desired_input); + + for n in 0..(samples + extra_samples) { + let adc_signal = adc_sampled_signal( + &pure_signals, + timestamp_start, + internal_frequency, + adc_frequency, + 1 << sample_buffer_size_log2, + ); + let timestamp = adc_batch_timestamps( + reference_frequency, + timestamp_start, + timestamp_start + batch_sample_count - 1, + internal_frequency, + ); + + let (demodulation_initial_phase, demodulation_frequency) = + timestamp_handler.update(timestamp); + + let (in_phase, quadrature) = lockin.update( + adc_signal, + demodulation_initial_phase, + demodulation_frequency, + ); + + let magnitude = shift_round(in_phase, 16) * shift_round(in_phase, 16) + + shift_round(quadrature, 16) * shift_round(quadrature, 16); + let phase = atan2(quadrature, in_phase); + + // 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, + dbfs(amplitude_normalized), + 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 = in_phase as f64 / (1 << 30) as f64; + let quadrature_normalized = quadrature 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 + ), + ); + } + + timestamp_start += batch_sample_count; + } +} + +#[test] +fn lowpass() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = internal_frequency / 64.; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 3; + let pll_shift_phase: u8 = 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 = 6.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 3; + let pll_shift_phase: u8 = 2; + 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 = 6.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 3; + let pll_shift_phase: u8 = 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 = 6.; + let tolerance: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 111e3; + let harmonic: u32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 3; + let pll_shift_phase: u8 = 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: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 2; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 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: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 3; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 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: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 4; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 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: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 2; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 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: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 32.; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 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: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 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: f64 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + 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 = internal_frequency / 64.; + let signal_frequency: f64 = 10e3; + let harmonic: u32 = 1; + let sample_buffer_size_log2: usize = 2; + let pll_shift_frequency: u8 = 2; + let pll_shift_phase: u8 = 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: f64 = 1e-1; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + sample_buffer_size_log2, + pll_shift_frequency, + pll_shift_phase, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }], + time_constant_factor, + tolerance, + ); +} From 20488ea3bc0ea98f8bf2af4f7e948d7f610eb923 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 17 Jan 2021 22:19:14 +0100 Subject: [PATCH 08/25] lockin: refine --- dsp/src/iir_int.rs | 19 +- dsp/src/lib.rs | 8 +- dsp/src/testing.rs | 16 ++ dsp/src/trig.rs | 16 +- dsp/tests/lockin.rs | 459 ++++++++++++++++---------------------------- src/main.rs | 62 +++--- 6 files changed, 235 insertions(+), 345 deletions(-) diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs index 1a4a6a9..ac8b7eb 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, @@ -29,7 +30,7 @@ pub struct IIR { impl IIR { /// Coefficient fixed point: signed Q2.30. /// Tailored to low-passes PI, II etc. - const SHIFT: u32 = 30; + 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 6f521f1..02e7beb 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -2,8 +2,10 @@ #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] use core::ops::{Add, Mul, Neg}; +use serde::{Deserialize, Serialize}; -pub type Complex = (T, T); +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct Complex(pub T, pub T); /// Bit shift, round up half. /// @@ -17,7 +19,7 @@ pub type Complex = (T, T); /// Shifted and rounded value. #[inline(always)] pub fn shift_round(x: i32, shift: usize) -> i32 { - x.saturating_add(1 << (shift - 1)) >> shift + (x + (1 << (shift - 1))) >> shift } /// Integer division, round up half. @@ -122,4 +124,4 @@ pub mod trig; pub mod unwrap; #[cfg(test)] -mod testing; +pub mod testing; 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/dsp/src/trig.rs b/dsp/src/trig.rs index 3f96609..2269fe7 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -153,7 +153,7 @@ pub fn cossin(phase: i32) -> Complex { sin *= -1; } - (cos, sin) + Complex(cos, sin) } #[cfg(test)] @@ -210,13 +210,13 @@ mod tests { #[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(0., 0.); + let mut sum_err = Complex(0., 0.); + let mut max_err = Complex(0f64, 0.); + let mut sum = Complex(0., 0.); + let mut demod = Complex(0., 0.); // use std::{fs::File, io::{BufWriter, prelude::*}, path::Path}; // let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap()); diff --git a/dsp/tests/lockin.rs b/dsp/tests/lockin.rs index 896f871..f48eab4 100644 --- a/dsp/tests/lockin.rs +++ b/dsp/tests/lockin.rs @@ -1,7 +1,6 @@ use dsp::{ iir_int::{IIRState, IIR}, reciprocal_pll::TimestampHandler, - shift_round, trig::{atan2, cossin}, Complex, }; @@ -9,191 +8,113 @@ use dsp::{ use std::f64::consts::PI; use std::vec::Vec; -const ADC_MAX: f64 = 1.; +// TODO: -> dsp/src/testing.rs +/// 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 +} + const ADC_MAX_COUNT: f64 = (1 << 15) as f64; struct Lockin { harmonic: u32, - phase_offset: u32, + phase: u32, iir: IIR, iir_state: [IIRState; 2], } impl Lockin { - /// Construct a new `Lockin` instance. - /// - /// # Args - /// * `harmonic` - Factor for harmonic demodulation. For example, a value of 1 would demodulate - /// with the reference frequency whereas a value of 2 would demodulate with the first harmonic - /// of the reference frequency. - /// * `phase_offset` - Phase offset of the scaled (see `harmonic`) demodulation signal relative - /// to the reference signal. - /// * `iir` - IIR coefficients (see `iir_int::IIR`) used for filtering the demodulated in-phase - /// and quadrature signals. - pub fn new(harmonic: u32, phase_offset: u32, iir: IIR) -> Self { + pub fn new(harmonic: u32, phase: u32, iir: IIR) -> Self { Lockin { harmonic, - phase_offset, + phase, iir, - iir_state: [[0; 5]; 2], + iir_state: [IIRState::default(); 2], } } - /// Compute the in-phase and quadrature signals. This is intended to mimic the lock-in - /// processing routine invoked in main.rs. - /// - /// # Args - /// * `adc_samples` - ADC samples. - /// * `demodulation_initial_phase` - Phase value of the demodulation signal corresponding to the - /// first ADC sample. - /// * `demodulation_frequency` - Demodulation frequency. pub fn update( &mut self, - adc_samples: Vec, - demodulation_initial_phase: u32, - demodulation_frequency: u32, + input: Vec, + phase: u32, + frequency: u32, ) -> Complex { - let mut signal = Vec::>::new(); + let frequency = frequency.wrapping_mul(self.harmonic); + let mut phase = + self.phase.wrapping_add(phase.wrapping_mul(self.harmonic)); + let mut last = Complex::default(); - adc_samples.iter().enumerate().for_each(|(i, s)| { - let sample_phase = self - .harmonic - .wrapping_mul( - (demodulation_frequency.wrapping_mul(i as u32)) - .wrapping_add(demodulation_initial_phase), - ) - .wrapping_add(self.phase_offset); - let (cos, sin) = cossin(sample_phase as i32); + for s in input.iter() { + let m = cossin(-(phase as i32)); + phase = phase.wrapping_add(frequency); - signal.push(( - *s as i32 * shift_round(sin, 16), - *s as i32 * shift_round(cos, 16), - )); - - signal[i].0 = self.iir.update(&mut self.iir_state[0], signal[i].0); - signal[i].1 = self.iir.update(&mut self.iir_state[1], signal[i].1); - }); - - (signal[0].0, signal[0].1) + last = Complex( + self.iir.update( + &mut self.iir_state[0], + ((*s as i64 * m.0 as i64) >> 16) as i32, + ), + self.iir.update( + &mut self.iir_state[1], + ((*s as i64 * m.1 as i64) >> 16) as i32, + ), + ); + } + last } } /// Single-frequency sinusoid. #[derive(Copy, Clone)] -struct PureSine { +struct Tone { // 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, + 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 a dBFS voltage ratio to a linear ratio. -/// -/// # Args -/// * `dbfs` - dB ratio relative to full scale. -/// -/// # Returns -/// Linear value. +/// Convert dBFS to a linear ratio. fn linear(dbfs: f64) -> f64 { - let base = 10_f64; - ADC_MAX * base.powf(dbfs / 20.) + 10f64.powf(dbfs / 20.) } -/// Convert a linear voltage ratio to a dBFS ratio. -/// -/// # Args -/// * `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. -/// -/// # Args -/// * `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 a full batch of ADC samples starting at `timestamp_start`. -/// -/// # Args -/// * `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). -/// * `sample_buffer_size` - The number of ADC samples in one processing batch. -/// -/// # Returns -/// The sampled signal at the ADC input. +/// Generate a full batch of samples starting at `time_offset`. fn adc_sampled_signal( - pure_signals: &Vec, - timestamp_start: u64, - internal_frequency: f64, - adc_frequency: f64, + tones: &Vec, + time_offset: f64, + sampling_frequency: f64, sample_buffer_size: u32, ) -> Vec { - // 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 = Vec::::new(); for i in 0..sample_buffer_size { - signal.push(real_to_adc_sample( - amplitude - .iter() - .zip(initial_phase.iter()) - .zip(phase_increment.iter()) - .fold(0., |acc, ((a, phi), theta)| { - acc + a * (phi + theta * i as f64).sin() - }), - )); + let time = 2. * PI * (time_offset + i as f64 / sampling_frequency); + let x: f64 = tones + .iter() + .map(|&t| { + linear(t.amplitude_dbfs) * (t.phase + t.frequency * time).cos() + }) + .sum(); + assert!(-1. < x && x < 1.); + signal.push((x * ADC_MAX_COUNT) as i16); } - signal } @@ -235,76 +156,36 @@ fn adc_batch_timestamps( /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html /// /// # Args -/// * `corner_frequency` - Corner frequency, or 3dB cutoff frequency (in Hz). -/// * `sampling_frequency` - Sampling frequency (in Hz). +/// * `fc` - Corner frequency, or 3dB cutoff frequency (in Hz). +/// * `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( - corner_frequency: f64, - sampling_frequency: f64, -) -> [i32; 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; +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 _; - // iir uses Q2.30 fixed point - [ - (b0 * (1 << 30) as f64).round() as i32, - (b1 * (1 << 30) as f64).round() as i32, - (b2 * (1 << 30) as f64).round() as i32, - (a1 * (1 << 30) as f64).round() as i32, - (a2 * (1 << 30) as f64).round() as i32, - ] -} - -/// 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. -fn max_error(a: f64, b: f64, rtol: f64, atol: f64) -> f64 { - rtol * a.abs().max(b.abs()) + atol -} - -// TODO this is (mostly) copied from testing.rs. -pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool { - (a - b).abs() <= max_error(a, b, rtol, atol) + IIRState([b0, 2 * b0, b0, a1, a2]) } /// 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. /// /// # Args -/// * `noise_inputs` - Noise sources at the ADC input. +/// * `tones` - 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, + tones: &Vec, demodulation_frequency: f64, corner_frequency: f64, ) -> f64 { @@ -315,7 +196,7 @@ fn sampled_noise_amplitude( // 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 + let mut noise: f64 = tones .iter() .map(|n| { // Noise inputs create an oscillation at the output, where the oscillation magnitude is @@ -325,7 +206,7 @@ fn sampled_noise_amplitude( / corner_frequency) .log2(); // 2nd-order filter. Approximately 12dB/octave rolloff. - let attenuation = -2. * 20. * 2_f64.log10() * octaves; + let attenuation = -2. * 20. * 2f64.log10() * octaves; linear(n.amplitude_dbfs + attenuation) }) .sum(); @@ -498,8 +379,8 @@ fn phase_noise( /// * `pll_shift_frequency` - See `pll::update()`. /// * `pll_shift_phase` - See `pll::update()`. /// * `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`. +/// * `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 @@ -514,8 +395,8 @@ fn lowpass_test( pll_shift_frequency: u8, pll_shift_phase: u8, corner_frequency: f64, - desired_input: PureSine, - noise_inputs: &mut Vec, + desired_input: Tone, + tones: &mut Vec, time_constant_factor: f64, tolerance: f64, ) { @@ -543,7 +424,11 @@ fn lowpass_test( (demodulation_phase_offset / (2. * PI) * (1_u64 << 32) as f64).round() as u32, IIR { - ba: lowpass_iir_coefficients(corner_frequency, adc_frequency), + ba: lowpass_iir_coefficients( + corner_frequency / adc_frequency, + 1. / 2f64.sqrt(), + 2., + ), }, ); let mut timestamp_handler = TimestampHandler::new( @@ -569,14 +454,14 @@ fn lowpass_test( 1_u64 << (adc_sample_ticks_log2 + sample_buffer_size_log2); let effective_phase_offset = - desired_input.phase_offset - demodulation_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( - noise_inputs, + tones, reference_frequency * harmonic as f64, corner_frequency, ); @@ -593,14 +478,12 @@ fn lowpass_test( phase_noise(total_noise_amplitude, in_phase_actual, quadrature_actual) + 1e-2 * 2. * PI; - let pure_signals = noise_inputs; - pure_signals.push(desired_input); + tones.push(desired_input); for n in 0..(samples + extra_samples) { let adc_signal = adc_sampled_signal( - &pure_signals, - timestamp_start, - internal_frequency, + &tones, + timestamp_start as f64 / internal_frequency, adc_frequency, 1 << sample_buffer_size_log2, ); @@ -610,19 +493,19 @@ fn lowpass_test( timestamp_start + batch_sample_count - 1, internal_frequency, ); + timestamp_start += batch_sample_count; let (demodulation_initial_phase, demodulation_frequency) = timestamp_handler.update(timestamp); - - let (in_phase, quadrature) = lockin.update( + let output = lockin.update( adc_signal, demodulation_initial_phase, demodulation_frequency, ); - - let magnitude = shift_round(in_phase, 16) * shift_round(in_phase, 16) - + shift_round(quadrature, 16) * shift_round(quadrature, 16); - let phase = atan2(quadrature, in_phase); + 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 { @@ -638,7 +521,7 @@ fn lowpass_test( linear(desired_input.amplitude_dbfs), desired_input.amplitude_dbfs, amplitude_normalized, - dbfs(amplitude_normalized), + 20.*amplitude_normalized.log10(), max_error(linear(desired_input.amplitude_dbfs), amplitude_normalized, tolerance, total_magnitude_noise), ); let phase_normalized = @@ -661,8 +544,8 @@ fn lowpass_test( ), ); - let in_phase_normalized = in_phase as f64 / (1 << 30) as f64; - let quadrature_normalized = quadrature as f64 / (1 << 30) as f64; + 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( @@ -699,8 +582,6 @@ fn lowpass_test( ), ); } - - timestamp_start += batch_sample_count; } } @@ -729,21 +610,21 @@ fn lowpass() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: 0., + phase: 0., }, &mut vec![ - PureSine { + Tone { frequency: 1.1 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.9 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -776,21 +657,21 @@ fn lowpass_demodulation_phase_offset_pi_2() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: 0., + phase: 0., }, &mut vec![ - PureSine { + Tone { frequency: 1.1 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.9 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -823,21 +704,21 @@ fn lowpass_phase_offset_pi_2() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: PI / 2., + phase: PI / 2., }, &mut vec![ - PureSine { + Tone { frequency: 1.1 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.9 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -870,21 +751,21 @@ fn lowpass_fundamental_111e3_phase_offset_pi_4() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: PI / 4., + phase: PI / 4., }, &mut vec![ - PureSine { + Tone { frequency: 1.1 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.9 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -917,21 +798,21 @@ fn lowpass_first_harmonic() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: 0., + phase: 0., }, &mut vec![ - PureSine { + Tone { frequency: 1.2 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.8 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -964,21 +845,21 @@ fn lowpass_second_harmonic() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: 0., + phase: 0., }, &mut vec![ - PureSine { + Tone { frequency: 1.2 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.8 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -1011,21 +892,21 @@ fn lowpass_third_harmonic() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: 0., + phase: 0., }, &mut vec![ - PureSine { + Tone { frequency: 1.2 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.8 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -1058,21 +939,21 @@ fn lowpass_first_harmonic_phase_shift() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: PI / 4., + phase: PI / 4., }, &mut vec![ - PureSine { + Tone { frequency: 1.2 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.8 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -1105,21 +986,21 @@ fn lowpass_adc_frequency_1e6() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: 0., + phase: 0., }, &mut vec![ - PureSine { + Tone { frequency: 1.2 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.8 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -1152,21 +1033,21 @@ fn lowpass_internal_frequency_125e6() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: 0., + phase: 0., }, &mut vec![ - PureSine { + Tone { frequency: 1.2 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, - PureSine { + Tone { frequency: 0.8 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }, ], time_constant_factor, @@ -1199,15 +1080,15 @@ fn lowpass_low_signal_frequency() { pll_shift_frequency, pll_shift_phase, corner_frequency, - PureSine { + Tone { frequency: demodulation_frequency, amplitude_dbfs: -30., - phase_offset: 0., + phase: 0., }, - &mut vec![PureSine { + &mut vec![Tone { frequency: 1.1 * demodulation_frequency, amplitude_dbfs: -20., - phase_offset: 0., + phase: 0., }], time_constant_factor, tolerance, diff --git a/src/main.rs b/src/main.rs index a6559f6..5732474 100644 --- a/src/main.rs +++ b/src/main.rs @@ -94,8 +94,8 @@ use dac::{Dac0Output, Dac1Output}; use dsp::{ iir, iir_int, reciprocal_pll::TimestampHandler, - shift_round, trig::{atan2, cossin}, + Complex, }; use pounder::DdsOutput; @@ -948,10 +948,8 @@ const APP: () = { ADC_SAMPLE_TICKS_LOG2 as usize, SAMPLE_BUFFER_SIZE_LOG2, ); - let iir_lockin = iir_int::IIR { - ba: [1, 0, 0, 0, 0], - }; - let iir_state_lockin = [[0; 5]; 2]; + let iir_lockin = iir_int::IIR::default(); + let iir_state_lockin = [iir_int::IIRState::default(); 2]; // Start sampling ADCs. sampling_timer.start(); @@ -995,47 +993,39 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; - let (demodulation_initial_phase, demodulation_frequency) = c - .resources - .timestamp_handler - .update(c.resources.input_stamper.latest_timestamp()); - let [dac0, dac1] = dac_samples; let iir_lockin = c.resources.iir_lockin; let iir_state_lockin = c.resources.iir_state_lockin; let iir_ch = c.resources.iir_ch; let iir_state = c.resources.iir_state; + let (pll_phase, pll_frequency) = c + .resources + .timestamp_handler + .update(c.resources.input_stamper.latest_timestamp()); + let frequency = pll_frequency.wrapping_mul(HARMONIC); + let mut phase = + PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC)); + dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( |(i, (d0, d1))| { - let sample_phase = (HARMONIC.wrapping_mul( - (demodulation_frequency.wrapping_mul(i as u32)) - .wrapping_add(demodulation_initial_phase), - )) - .wrapping_add(PHASE_OFFSET); - let (cos, sin) = cossin(sample_phase as i32); + let m = cossin(-(phase as i32)); + phase = phase.wrapping_add(frequency); - let mut signal = (0_i32, 0_i32); + let signal = Complex( + iir_lockin.update( + &mut iir_state_lockin[0], + ((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as i32, + ), + iir_lockin.update( + &mut iir_state_lockin[1], + ((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as i32, + ), + ); - // shift cos/sin before multiplying to avoid i64 multiplication - signal.0 = - adc_samples[0][i] as i16 as i32 * shift_round(sin, 16); - signal.1 = - adc_samples[0][i] as i16 as i32 * shift_round(cos, 16); - - signal.0 = - iir_lockin.update(&mut iir_state_lockin[0], signal.0); - signal.1 = - iir_lockin.update(&mut iir_state_lockin[1], signal.1); - - let mut magnitude = f32::from(shift_round( - signal.0 * signal.0 + signal.1 * signal.1, - 16, - ) as i16); - let mut phase = f32::from(shift_round( - atan2(signal.1, signal.0), - 16, - ) as i16); + let mut magnitude = + (signal.0 * signal.0 + signal.1 * signal.1) as f32; + let mut phase = atan2(signal.1, signal.0) as f32; for j in 0..iir_state[0].len() { magnitude = From 778f4ac4d5c4cc18a092fc4bf1f17f4a865598e3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 19 Jan 2021 11:30:12 +0100 Subject: [PATCH 09/25] lockin: wrapping_neg --- dsp/tests/lockin.rs | 2 +- src/main.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dsp/tests/lockin.rs b/dsp/tests/lockin.rs index f48eab4..0f4450a 100644 --- a/dsp/tests/lockin.rs +++ b/dsp/tests/lockin.rs @@ -60,7 +60,7 @@ impl Lockin { let mut last = Complex::default(); for s in input.iter() { - let m = cossin(-(phase as i32)); + let m = cossin((phase as i32).wrapping_neg()); phase = phase.wrapping_add(frequency); last = Complex( diff --git a/src/main.rs b/src/main.rs index eaf3979..0226495 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1026,7 +1026,7 @@ const APP: () = { dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( |(i, (d0, d1))| { - let m = cossin(-(phase as i32)); + let m = cossin((phase as i32).wrapping_neg()); phase = phase.wrapping_add(frequency); let signal = Complex( From 1f43e4d0b502a27ae3d1440a0e0c9625a6f11485 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 20 Jan 2021 04:01:24 +0000 Subject: [PATCH 10/25] build(deps): bump serde from 1.0.118 to 1.0.120 Bumps [serde](https://github.com/serde-rs/serde) from 1.0.118 to 1.0.120. - [Release notes](https://github.com/serde-rs/serde/releases) - [Commits](https://github.com/serde-rs/serde/compare/v1.0.118...v1.0.120) Signed-off-by: dependabot[bot] --- Cargo.lock | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 76fb832..a9ea25c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -767,9 +767,9 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" [[package]] name = "serde" -version = "1.0.118" +version = "1.0.120" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "06c64263859d87aa2eb554587e2d23183398d617427327cf2b3d0ed8c69e4800" +checksum = "166b2349061381baf54a58e4b13c89369feb0ef2eaa57198899e2312aac30aab" dependencies = [ "serde_derive", ] @@ -796,9 +796,9 @@ dependencies = [ [[package]] name = "serde_derive" -version = "1.0.118" +version = "1.0.120" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c84d3526699cd55261af4b941e4e725444df67aa4f9e6a3564f18030d12672df" +checksum = "0ca2a8cb5805ce9e3b95435e3765b7b553cecc762d938d409434338386cb5775" dependencies = [ "proc-macro2", "quote", @@ -891,9 +891,9 @@ dependencies = [ [[package]] name = "syn" -version = "1.0.53" +version = "1.0.58" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8833e20724c24de12bbaba5ad230ea61c3eafb05b881c7c9d3cfe8638b187e68" +checksum = "cc60a3d73ea6594cd712d830cc1f0390fd71542d8c8cd24e70cc54cdfd5e05d5" dependencies = [ "proc-macro2", "quote", From 26677063eaac28431c52403b929ca2a9739da1b0 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 20 Jan 2021 13:43:34 +0100 Subject: [PATCH 11/25] Adding support for multiple applications --- src/bin/dual-iir.rs | 295 ++++++++++++++++++++++++++++++++++++++++++++ src/lib.rs | 16 +++ src/server.rs | 10 +- 3 files changed, 315 insertions(+), 6 deletions(-) create mode 100644 src/bin/dual-iir.rs create mode 100644 src/lib.rs diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs new file mode 100644 index 0000000..fd8f5e4 --- /dev/null +++ b/src/bin/dual-iir.rs @@ -0,0 +1,295 @@ +#![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}; + +use dsp::iir; +use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, 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], + } + + #[init] + fn init(c: init::Context) -> init::LateResources { + // Configure the microcontroller + let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); + + // Enable ADC/DAC events + stabilizer.adcs.0.start(); + stabilizer.adcs.1.start(); + stabilizer.dacs.0.start(); + stabilizer.dacs.1.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, + } + } + + /// 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. + #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch], 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(), + ]; + + 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 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::() }; + // Convert to DAC code + dac_samples[channel][sample] = y 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/lib.rs b/src/lib.rs new file mode 100644 index 0000000..a51024a --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,16 @@ +#![no_std] + +pub mod server; + +#[macro_use] +extern crate log; + +pub mod hardware; + +// 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; + +// The desired ADC sample processing buffer size. +const SAMPLE_BUFFER_SIZE: usize = 8; diff --git a/src/server.rs b/src/server.rs index f434060..5e6950d 100644 --- a/src/server.rs +++ b/src/server.rs @@ -1,14 +1,12 @@ -use heapless::{consts::*, String, Vec}; - use core::fmt::Write; - +use heapless::{consts::*, String, Vec}; use serde::{Deserialize, Serialize}; - use serde_json_core::{de::from_slice, ser::to_string}; - -use super::iir; use smoltcp as net; +use dsp::iir; + +#[macro_export] macro_rules! route_request { ($request:ident, readable_attributes: [$($read_attribute:tt: $getter:tt),*], From 86355c9c5d4e8c6cac5d786cb61fafba61b829da Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 20 Jan 2021 13:44:16 +0100 Subject: [PATCH 12/25] Removing main.rs --- src/main.rs | 304 ---------------------------------------------------- 1 file changed, 304 deletions(-) delete mode 100644 src/main.rs diff --git a/src/main.rs b/src/main.rs deleted file mode 100644 index 3b86c37..0000000 --- a/src/main.rs +++ /dev/null @@ -1,304 +0,0 @@ -#![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}; - -// 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; - -// The desired ADC sample processing buffer size. -const SAMPLE_BUFFER_SIZE: usize = 8; - -// The number of cascaded IIR biquads per channel. Select 1 or 2! -const IIR_CASCADE_LENGTH: usize = 1; - -#[macro_use] -mod server; -mod hardware; -use dsp::iir; -use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; - -const SCALE: f32 = ((1 << 15) - 1) as f32; - -const TCP_RX_BUFFER_SIZE: usize = 8192; -const TCP_TX_BUFFER_SIZE: usize = 8192; - -#[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], - } - - #[init] - fn init(c: init::Context) -> init::LateResources { - // Configure the microcontroller - let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); - - // Enable ADC/DAC events - stabilizer.adcs.0.start(); - stabilizer.adcs.1.start(); - stabilizer.dacs.0.start(); - stabilizer.dacs.1.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, - } - } - - /// 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. - #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch], 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(), - ]; - - 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 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::() }; - // Convert to DAC code - dac_samples[channel][sample] = y 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); - 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(); - } -}; From 4d0b1b55660d3a7614d7a8decb1962dff8a2c669 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 20 Jan 2021 13:44:53 +0100 Subject: [PATCH 13/25] Reordering lib.rs --- src/lib.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index a51024a..b685f7a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,11 +1,10 @@ #![no_std] -pub mod server; - #[macro_use] extern crate log; pub mod hardware; +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. From 507e334ec5297b5f5a55851d31e7313e9e3ab3b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 20 Jan 2021 14:07:57 +0100 Subject: [PATCH 14/25] lockin: tweak impl --- src/main.rs | 61 +++++++++++++++++++++++++---------------------------- 1 file changed, 29 insertions(+), 32 deletions(-) diff --git a/src/main.rs b/src/main.rs index 0226495..0930d5a 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1010,7 +1010,6 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; - let [dac0, dac1] = dac_samples; let iir_lockin = c.resources.iir_lockin; let iir_state_lockin = c.resources.iir_state_lockin; let iir_ch = c.resources.iir_ch; @@ -1024,41 +1023,39 @@ const APP: () = { let mut phase = PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC)); - dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( - |(i, (d0, d1))| { - let m = cossin((phase as i32).wrapping_neg()); - phase = phase.wrapping_add(frequency); + for i in 0..adc_samples[0].len() { + let m = cossin((phase as i32).wrapping_neg()); + phase = phase.wrapping_add(frequency); - let signal = Complex( - iir_lockin.update( - &mut iir_state_lockin[0], - ((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as i32, - ), - iir_lockin.update( - &mut iir_state_lockin[1], - ((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as i32, - ), - ); + let signal = Complex( + iir_lockin.update( + &mut iir_state_lockin[0], + ((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as _, + ), + iir_lockin.update( + &mut iir_state_lockin[1], + ((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as _, + ), + ); - let mut magnitude = - (signal.0 * signal.0 + signal.1 * signal.1) as f32; - let mut phase = atan2(signal.1, signal.0) as f32; + let mut magnitude = + (signal.0 * signal.0 + signal.1 * signal.1) as _; + let mut phase = atan2(signal.1, signal.0) as _; - for j in 0..iir_state[0].len() { - magnitude = - iir_ch[0][j].update(&mut iir_state[0][j], magnitude); - phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); - } + for j in 0..iir_state[0].len() { + magnitude = + iir_ch[0][j].update(&mut iir_state[0][j], magnitude); + phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); + } - unsafe { - let magnitude = magnitude.to_int_unchecked::(); - let phase = phase.to_int_unchecked::(); - - *d0 = magnitude as u16 ^ 0x8000; - *d1 = phase as u16 ^ 0x8000; - } - }, - ); + // Note(unsafe): range clipping to i16 is ensured by IIR filters above. + unsafe { + dac_samples[0][i] = + magnitude.to_int_unchecked::() as u16 ^ 0x8000; + dac_samples[1][i] = + phase.to_int_unchecked::() as u16 ^ 0x8000; + } + } if let Some(dds_output) = c.resources.dds_output { let builder = dds_output.builder().update_channels( From d014ed0fe01afe59791c1b3fb47301247f20eb53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 20 Jan 2021 14:29:29 +0100 Subject: [PATCH 15/25] add lockin bin --- src/bin/lockin.rs | 41 +++++++++++++++++++++++++++++++++++++---- src/hardware/mod.rs | 1 + 2 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 810b8e5..fef0e38 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -14,8 +14,22 @@ use heapless::{consts::*, String}; use stabilizer::{hardware, server}; -use dsp::iir; -use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; +use dsp::{ + iir, iir_int, + reciprocal_pll::TimestampHandler, + trig::{atan2, cossin}, + Complex, +}; +use hardware::{ + Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, +}; + +// Frequency scaling factor for lock-in harmonic demodulation. +const HARMONIC: u32 = 1; +// Phase offset applied to the lock-in demodulation signal. +const PHASE_OFFSET: u32 = 0; +const ADC_SAMPLE_TICKS_LOG2: u16 = 8; +const SAMPLE_BUFFER_SIZE_LOG2: usize = 3; const SCALE: f32 = ((1 << 15) - 1) as f32; @@ -38,6 +52,11 @@ const APP: () = { 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, + timestamp_handler: TimestampHandler, + iir_lockin: iir_int::IIR, + iir_state_lockin: [iir_int::IIRState; 2], } #[init] @@ -45,6 +64,15 @@ const APP: () = { // Configure the microcontroller let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); + let timestamp_handler = TimestampHandler::new( + 4, + 3, + ADC_SAMPLE_TICKS_LOG2 as usize, + SAMPLE_BUFFER_SIZE_LOG2, + ); + let iir_lockin = iir_int::IIR::default(); + let iir_state_lockin = [iir_int::IIRState::default(); 2]; + // Enable ADC/DAC events stabilizer.adcs.0.start(); stabilizer.adcs.1.start(); @@ -59,6 +87,11 @@ const APP: () = { adcs: stabilizer.adcs, dacs: stabilizer.dacs, net_interface: stabilizer.net.interface, + timestamper: stabilizer.timestamper, + + timestamp_handler, + iir_lockin, + iir_state_lockin, } } @@ -78,7 +111,7 @@ const APP: () = { /// /// 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. - #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch], priority=2)] + #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, iir_lockin, iir_state_lockin, timestamp_handler, timestamper], priority=2)] fn process(c: process::Context) { let adc_samples = [ c.resources.adcs.0.acquire_buffer(), @@ -98,7 +131,7 @@ const APP: () = { let (pll_phase, pll_frequency) = c .resources .timestamp_handler - .update(c.resources.input_stamper.latest_timestamp()); + .update(c.resources.timestamper.latest_timestamp()); let frequency = pll_frequency.wrapping_mul(HARMONIC); let mut phase = PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC)); 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. From 775fb79ed9705f82e952d58cb8a2543af16865cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 20 Jan 2021 15:02:35 +0100 Subject: [PATCH 16/25] ci: update --- .github/workflows/ci.yml | 40 ++++++++++++++++++---------------------- 1 file changed, 18 insertions(+), 22 deletions(-) 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 From 94c4f8e6f7cc04cb83205c00e442bbdb9a4fc483 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 20 Jan 2021 15:09:50 +0100 Subject: [PATCH 17/25] hitl: undo bin change to make merging easier --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index caf03f0..f4c6212 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -78,7 +78,7 @@ jobs: - uses: actions/upload-artifact@v2 if: ${{ matrix.toolchain == 'stable' }} with: - name: bin + name: stabilizer_${{ github.sha }} path: | target/*/release/stabilizer stabilizer-release.bin From c078de05cc830a7f2ed6ae71d98beda3245afd04 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 20 Jan 2021 15:31:46 +0100 Subject: [PATCH 18/25] lockin: fix adc value conversion --- dsp/tests/lockin.rs | 7 ++++--- src/bin/lockin.rs | 7 +++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/dsp/tests/lockin.rs b/dsp/tests/lockin.rs index 0f4450a..392ce4d 100644 --- a/dsp/tests/lockin.rs +++ b/dsp/tests/lockin.rs @@ -59,18 +59,19 @@ impl Lockin { self.phase.wrapping_add(phase.wrapping_mul(self.harmonic)); let mut last = Complex::default(); - for s in input.iter() { + for &s in input.iter() { let m = cossin((phase as i32).wrapping_neg()); phase = phase.wrapping_add(frequency); + let signal = (s as i32) << 16; last = Complex( self.iir.update( &mut self.iir_state[0], - ((*s as i64 * m.0 as i64) >> 16) as i32, + ((signal as i64 * m.0 as i64) >> 32) as i32, ), self.iir.update( &mut self.iir_state[1], - ((*s as i64 * m.1 as i64) >> 16) as i32, + ((signal as i64 * m.1 as i64) >> 32) as i32, ), ); } diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index fef0e38..865561b 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -140,14 +140,13 @@ const APP: () = { let m = cossin((phase as i32).wrapping_neg()); phase = phase.wrapping_add(frequency); + let signal = (adc_samples[0][i] as i16 as i32) << 16; let signal = Complex( iir_lockin.update( - &mut iir_state_lockin[0], - ((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as _, + &mut iir_state_lockin[0], ((signal as i64 * m.0 as i64) >> 32) as _, ), iir_lockin.update( - &mut iir_state_lockin[1], - ((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as _, + &mut iir_state_lockin[1], ((signal as i64 * m.1 as i64) >> 16) as _, ), ); From 5af2b9c63a1d585cdd9f81d9eb1d99122744d889 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 20 Jan 2021 15:34:56 +0100 Subject: [PATCH 19/25] fmt --- src/bin/lockin.rs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 865561b..40a7aeb 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -143,10 +143,12 @@ const APP: () = { let signal = (adc_samples[0][i] as i16 as i32) << 16; let signal = Complex( iir_lockin.update( - &mut iir_state_lockin[0], ((signal as i64 * m.0 as i64) >> 32) as _, + &mut iir_state_lockin[0], + ((signal as i64 * m.0 as i64) >> 32) as _, ), iir_lockin.update( - &mut iir_state_lockin[1], ((signal as i64 * m.1 as i64) >> 16) as _, + &mut iir_state_lockin[1], + ((signal as i64 * m.1 as i64) >> 16) as _, ), ); From 948e58c9101dfb4e4668f5261343bd93bf85dac8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 21 Jan 2021 14:55:33 +0100 Subject: [PATCH 20/25] lockin: refactor Lockin --- dsp/src/iir_int.rs | 14 +++++++ dsp/src/lib.rs | 1 + dsp/src/lockin.rs | 55 ++++++++++++++++++++++++++ src/bin/lockin.rs | 97 +++++++++++++++++++++------------------------- src/lib.rs | 6 ++- 5 files changed, 119 insertions(+), 54 deletions(-) create mode 100644 dsp/src/lockin.rs diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs index ac8b7eb..c31f208 100644 --- a/dsp/src/iir_int.rs +++ b/dsp/src/iir_int.rs @@ -3,6 +3,20 @@ use serde::{Deserialize, Serialize}; #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIRState(pub [i32; 5]); +impl IIRState { + #[inline(always)] + pub fn get_x(&self, index: usize) -> i32 { + // x0 is at index 0 in a biquad between updates + self.0[index] + } + + #[inline(always)] + pub fn get_y(&self, index: usize) -> i32 { + // y0 is at index 2 in a biquad between updates + self.0[2 + index] + } +} + fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { // Rounding bias, half up let y0 = ((y0 as i64) << shift) + (1 << (shift - 1)); diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 02e7beb..4e9b642 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -118,6 +118,7 @@ where pub mod iir; pub mod iir_int; +pub mod lockin; pub mod pll; pub mod reciprocal_pll; pub mod trig; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs new file mode 100644 index 0000000..d4339ac --- /dev/null +++ b/dsp/src/lockin.rs @@ -0,0 +1,55 @@ +use super::{ + iir_int, + trig::{atan2, cossin}, + 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(_corner: u8) -> Self { + Lockin { + iir: iir_int::IIR::default(), // TODO: lowpass coefficients from corner + 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.wrapping_neg()); + + // 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 _, + ), + ) + } + + pub fn iq(&self) -> Complex { + Complex(self.iir_state[0].get_y(0), self.iir_state[1].get_y(0)) + } + + pub fn power(&self) -> i32 { + let iq = self.iq(); + (((iq.0 as i64) * (iq.0 as i64) + (iq.1 as i64) * (iq.1 as i64)) >> 32) + as i32 + } + + pub fn phase(&self) -> i32 { + let iq = self.iq(); + atan2(iq.1, iq.0) + } +} diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 40a7aeb..8fbf96f 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -12,25 +12,15 @@ use rtic::cyccnt::{Instant, U32Ext}; use heapless::{consts::*, String}; -use stabilizer::{hardware, server}; - -use dsp::{ - iir, iir_int, - reciprocal_pll::TimestampHandler, - trig::{atan2, cossin}, - Complex, +use stabilizer::{ + hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2, }; + +use dsp::{iir, lockin::Lockin, reciprocal_pll::TimestampHandler}; use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, }; -// Frequency scaling factor for lock-in harmonic demodulation. -const HARMONIC: u32 = 1; -// Phase offset applied to the lock-in demodulation signal. -const PHASE_OFFSET: u32 = 0; -const ADC_SAMPLE_TICKS_LOG2: u16 = 8; -const SAMPLE_BUFFER_SIZE_LOG2: usize = 3; - const SCALE: f32 = ((1 << 15) - 1) as f32; const TCP_RX_BUFFER_SIZE: usize = 8192; @@ -54,9 +44,8 @@ const APP: () = { iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], timestamper: InputStamper, - timestamp_handler: TimestampHandler, - iir_lockin: iir_int::IIR, - iir_state_lockin: [iir_int::IIRState; 2], + pll: TimestampHandler, + lockin: Lockin, } #[init] @@ -64,14 +53,16 @@ const APP: () = { // Configure the microcontroller let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); - let timestamp_handler = TimestampHandler::new( - 4, - 3, + 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 iir_lockin = iir_int::IIR::default(); - let iir_state_lockin = [iir_int::IIRState::default(); 2]; + + let lockin = Lockin::new( + 10, // relative Locking lowpass filter bandwidth, TODO: expose + ); // Enable ADC/DAC events stabilizer.adcs.0.start(); @@ -82,6 +73,9 @@ const APP: () = { // Start sampling ADCs. stabilizer.adc_dac_timer.start(); + // Start recording digital input timestamps. + stabilizer.timestamp_timer.start(); + init::LateResources { afes: stabilizer.afes, adcs: stabilizer.adcs, @@ -89,9 +83,8 @@ const APP: () = { net_interface: stabilizer.net.interface, timestamper: stabilizer.timestamper, - timestamp_handler, - iir_lockin, - iir_state_lockin, + pll, + lockin, } } @@ -111,7 +104,9 @@ const APP: () = { /// /// 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. - #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, iir_lockin, iir_state_lockin, timestamp_handler, timestamper], priority=2)] + /// + /// 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(), @@ -123,49 +118,47 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; - let iir_lockin = c.resources.iir_lockin; - let iir_state_lockin = c.resources.iir_state_lockin; 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 - .timestamp_handler + .pll .update(c.resources.timestamper.latest_timestamp()); - let frequency = pll_frequency.wrapping_mul(HARMONIC); - let mut phase = - PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC)); + + // Harmonic index to demodulate + 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() { - let m = cossin((phase as i32).wrapping_neg()); - phase = phase.wrapping_add(frequency); + // Convert to signed, MSB align the ADC sample. + let input = (adc_samples[0][i] as i16 as i32) << 16; + // Obtain demodulated, filtered IQ sample. + lockin.update(input, sample_phase); + // Advance the sample phase. + sample_phase = sample_phase.wrapping_add(sample_frequency); - let signal = (adc_samples[0][i] as i16 as i32) << 16; - let signal = Complex( - iir_lockin.update( - &mut iir_state_lockin[0], - ((signal as i64 * m.0 as i64) >> 32) as _, - ), - iir_lockin.update( - &mut iir_state_lockin[1], - ((signal as i64 * m.1 as i64) >> 16) as _, - ), - ); - - let mut magnitude = - (signal.0 * signal.0 + signal.1 * signal.1) as _; - let mut phase = atan2(signal.1, signal.0) as _; + // Convert from IQ to power and phase. + let mut power = lockin.power() as _; + let mut phase = lockin.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() { - magnitude = - iir_ch[0][j].update(&mut iir_state[0][j], magnitude); + 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] = - magnitude.to_int_unchecked::() as u16 ^ 0x8000; + power.to_int_unchecked::() as u16 ^ 0x8000; dac_samples[1][i] = phase.to_int_unchecked::() as u16 ^ 0x8000; } 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; From cb280c33032b74999abcac68df9ca738bd65924b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 21 Jan 2021 15:01:17 +0100 Subject: [PATCH 21/25] lockin integration: reduce and refactor further --- dsp/tests/lockin.rs | 253 +++++++++++++++++++------------------------- 1 file changed, 106 insertions(+), 147 deletions(-) diff --git a/dsp/tests/lockin.rs b/dsp/tests/lockin.rs index 392ce4d..015bb58 100644 --- a/dsp/tests/lockin.rs +++ b/dsp/tests/lockin.rs @@ -29,7 +29,8 @@ pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool { (a - b).abs() <= a.abs().max(b.abs()) * rtol + atol } -const ADC_MAX_COUNT: f64 = (1 << 15) as f64; +/// ADC full scale in machine units (16 bit signed). +const ADC_SCALE: f64 = ((1 << 15) - 1) as f64; struct Lockin { harmonic: u32, @@ -57,25 +58,26 @@ impl Lockin { let frequency = frequency.wrapping_mul(self.harmonic); let mut phase = self.phase.wrapping_add(phase.wrapping_mul(self.harmonic)); - let mut last = Complex::default(); + input + .iter() + .map(|&s| { + let m = cossin((phase as i32).wrapping_neg()); + phase = phase.wrapping_add(frequency); - for &s in input.iter() { - let m = cossin((phase as i32).wrapping_neg()); - phase = phase.wrapping_add(frequency); - - let signal = (s as i32) << 16; - last = Complex( - self.iir.update( - &mut self.iir_state[0], - ((signal as i64 * m.0 as i64) >> 32) as i32, - ), - self.iir.update( - &mut self.iir_state[1], - ((signal as i64 * m.1 as i64) >> 32) as i32, - ), - ); - } - last + let signal = (s as i32) << 16; + 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 _, + ), + ) + }) + .last() + .unwrap_or(Complex::default()) } } @@ -96,27 +98,53 @@ fn linear(dbfs: f64) -> f64 { 10f64.powf(dbfs / 20.) } -/// Generate a full batch of samples starting at `time_offset`. -fn adc_sampled_signal( +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, - sampling_frequency: f64, sample_buffer_size: u32, ) -> Vec { - let mut signal = Vec::::new(); + (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() +} - for i in 0..sample_buffer_size { - let time = 2. * PI * (time_offset + i as f64 / sampling_frequency); - let x: f64 = tones - .iter() - .map(|&t| { - linear(t.amplitude_dbfs) * (t.phase + t.frequency * time).cos() - }) - .sum(); - assert!(-1. < x && x < 1.); - signal.push((x * ADC_MAX_COUNT) as i16); - } - signal +/// 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 decades = + ((t.frequency - frequency) / corner).abs().max(1.).log10(); + // 2nd-order filter: 40dB/decade rolloff. + linear(t.amplitude_dbfs - 40. * decades) + }) + .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 @@ -175,49 +203,6 @@ fn lowpass_iir_coefficients(fc: f64, q: f64, k: f64) -> IIRState { IIRState([b0, 2 * b0, b0, a1, a2]) } -/// 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. -/// -/// # Args -/// * `tones` - 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( - tones: &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 = tones - .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. * 2f64.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: @@ -369,7 +354,6 @@ fn 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. -/// * `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. @@ -388,7 +372,6 @@ fn phase_noise( /// 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, @@ -402,19 +385,18 @@ fn lowpass_test( tolerance: f64, ) { assert!( - isclose((internal_frequency / adc_frequency).log2(), (internal_frequency / adc_frequency).log2().round(), 0., 1e-5), + 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 / adc_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 / adc_frequency).log2().round() as usize; + 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." @@ -426,7 +408,7 @@ fn lowpass_test( as u32, IIR { ba: lowpass_iir_coefficients( - corner_frequency / adc_frequency, + corner_frequency, 1. / 2f64.sqrt(), 2., ), @@ -444,13 +426,13 @@ fn lowpass_test( // 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 * adc_frequency - / (1 << sample_buffer_size_log2) as f64) 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 * adc_frequency) as usize; + let extra_samples = time_constant as usize; let batch_sample_count = 1_u64 << (adc_sample_ticks_log2 + sample_buffer_size_log2); @@ -482,10 +464,9 @@ fn lowpass_test( tones.push(desired_input); for n in 0..(samples + extra_samples) { - let adc_signal = adc_sampled_signal( + let adc_signal = sample_tones( &tones, timestamp_start as f64 / internal_frequency, - adc_frequency, 1 << sample_buffer_size_log2, ); let timestamp = adc_batch_timestamps( @@ -588,14 +569,13 @@ fn lowpass_test( #[test] fn lowpass() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 100e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 64e-3; let harmonic: u32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 3; let pll_shift_phase: u8 = 2; - let corner_frequency: f64 = 1e3; + 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.; @@ -603,7 +583,6 @@ fn lowpass() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -635,14 +614,13 @@ fn lowpass() { #[test] fn lowpass_demodulation_phase_offset_pi_2() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 100e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 64e-3; let harmonic: u32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 3; let pll_shift_phase: u8 = 2; - let corner_frequency: f64 = 1e3; + 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.; @@ -650,7 +628,6 @@ fn lowpass_demodulation_phase_offset_pi_2() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -682,14 +659,13 @@ fn lowpass_demodulation_phase_offset_pi_2() { #[test] fn lowpass_phase_offset_pi_2() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 100e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 64e-3; let harmonic: u32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 3; let pll_shift_phase: u8 = 2; - let corner_frequency: f64 = 1e3; + 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.; @@ -697,7 +673,6 @@ fn lowpass_phase_offset_pi_2() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -728,15 +703,14 @@ fn lowpass_phase_offset_pi_2() { } #[test] -fn lowpass_fundamental_111e3_phase_offset_pi_4() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 111e3; +fn lowpass_fundamental_71e_3_phase_offset_pi_4() { + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 71e-3; let harmonic: u32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 3; let pll_shift_phase: u8 = 2; - let corner_frequency: f64 = 1e3; + 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.; @@ -744,7 +718,6 @@ fn lowpass_fundamental_111e3_phase_offset_pi_4() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -776,14 +749,13 @@ fn lowpass_fundamental_111e3_phase_offset_pi_4() { #[test] fn lowpass_first_harmonic() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 50e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 50e-3; let harmonic: u32 = 2; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e3; + 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.; @@ -791,7 +763,6 @@ fn lowpass_first_harmonic() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -823,14 +794,13 @@ fn lowpass_first_harmonic() { #[test] fn lowpass_second_harmonic() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 50e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 50e-3; let harmonic: u32 = 3; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e3; + 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.; @@ -838,7 +808,6 @@ fn lowpass_second_harmonic() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -870,14 +839,13 @@ fn lowpass_second_harmonic() { #[test] fn lowpass_third_harmonic() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 50e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 50e-3; let harmonic: u32 = 4; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e3; + 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.; @@ -885,7 +853,6 @@ fn lowpass_third_harmonic() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -917,14 +884,13 @@ fn lowpass_third_harmonic() { #[test] fn lowpass_first_harmonic_phase_shift() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 50e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 50e-3; let harmonic: u32 = 2; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e3; + 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.; @@ -932,7 +898,6 @@ fn lowpass_first_harmonic_phase_shift() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -964,14 +929,13 @@ fn lowpass_first_harmonic_phase_shift() { #[test] fn lowpass_adc_frequency_1e6() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 32.; - let signal_frequency: f64 = 100e3; + let internal_frequency: f64 = 32.; + let signal_frequency: f64 = 100e-3; let harmonic: u32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e3; + 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.; @@ -979,7 +943,6 @@ fn lowpass_adc_frequency_1e6() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -1011,14 +974,13 @@ fn lowpass_adc_frequency_1e6() { #[test] fn lowpass_internal_frequency_125e6() { - let internal_frequency: f64 = 125e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 100e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 100e-3; let harmonic: u32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e3; + 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.; @@ -1026,7 +988,6 @@ fn lowpass_internal_frequency_125e6() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, @@ -1058,14 +1019,13 @@ fn lowpass_internal_frequency_125e6() { #[test] fn lowpass_low_signal_frequency() { - let internal_frequency: f64 = 100e6; - let adc_frequency: f64 = internal_frequency / 64.; - let signal_frequency: f64 = 10e3; + let internal_frequency: f64 = 64.; + let signal_frequency: f64 = 10e-3; let harmonic: u32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e3; + 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.; @@ -1073,7 +1033,6 @@ fn lowpass_low_signal_frequency() { lowpass_test( internal_frequency, - adc_frequency, signal_frequency, demodulation_phase_offset, harmonic, From 0cd2140668d7f02ab09a9f04a35b29e094f1fc20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 21 Jan 2021 16:12:59 +0100 Subject: [PATCH 22/25] rafactor complex, cossin, atan2 --- dsp/src/atan2.rs | 135 +++++++++++++++++++++++++++++++ dsp/src/complex.rs | 17 ++++ dsp/src/{trig.rs => cossin.rs} | 141 ++------------------------------- dsp/src/lib.rs | 12 +-- dsp/src/lockin.rs | 21 +---- dsp/tests/lockin.rs | 4 +- src/bin/lockin.rs | 14 ++-- 7 files changed, 175 insertions(+), 169 deletions(-) create mode 100644 dsp/src/atan2.rs create mode 100644 dsp/src/complex.rs rename dsp/src/{trig.rs => cossin.rs} (53%) 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 53% rename from dsp/src/trig.rs rename to dsp/src/cossin.rs index 2269fe7..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) @@ -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 _; const MAX_PHASE: f64 = (1i64 << 32) as _; - let mut rms_err = Complex(0., 0.); - let mut sum_err = Complex(0., 0.); - let mut max_err = Complex(0f64, 0.); - let mut sum = Complex(0., 0.); - let mut demod = Complex(0., 0.); + 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/lib.rs b/dsp/src/lib.rs index 4e9b642..56a3004 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -2,10 +2,6 @@ #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] use core::ops::{Add, Mul, Neg}; -use serde::{Deserialize, Serialize}; - -#[derive(Copy, Clone, Default, Deserialize, Serialize)] -pub struct Complex(pub T, pub T); /// Bit shift, round up half. /// @@ -116,13 +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)] pub mod testing; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index d4339ac..591d7e0 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,8 +1,4 @@ -use super::{ - iir_int, - trig::{atan2, cossin}, - Complex, -}; +use super::{cossin, iir_int, Complex}; use serde::{Deserialize, Serialize}; #[derive(Copy, Clone, Default, Deserialize, Serialize)] @@ -37,19 +33,4 @@ impl Lockin { ), ) } - - pub fn iq(&self) -> Complex { - Complex(self.iir_state[0].get_y(0), self.iir_state[1].get_y(0)) - } - - pub fn power(&self) -> i32 { - let iq = self.iq(); - (((iq.0 as i64) * (iq.0 as i64) + (iq.1 as i64) * (iq.1 as i64)) >> 32) - as i32 - } - - pub fn phase(&self) -> i32 { - let iq = self.iq(); - atan2(iq.1, iq.0) - } } diff --git a/dsp/tests/lockin.rs b/dsp/tests/lockin.rs index 015bb58..dc57dcb 100644 --- a/dsp/tests/lockin.rs +++ b/dsp/tests/lockin.rs @@ -1,7 +1,7 @@ use dsp::{ + atan2, cossin, iir_int::{IIRState, IIR}, reciprocal_pll::TimestampHandler, - trig::{atan2, cossin}, Complex, }; @@ -185,7 +185,7 @@ fn adc_batch_timestamps( /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html /// /// # Args -/// * `fc` - Corner frequency, or 3dB cutoff frequency (in Hz). +/// * `fc` - Corner frequency, or 3dB cutoff frequency (in units of sample rate). /// * `q` - Quality factor (1/sqrt(2) for critical). /// * `k` - DC gain. /// diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 8fbf96f..a9a63d4 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -70,12 +70,12 @@ const APP: () = { stabilizer.dacs.0.start(); stabilizer.dacs.1.start(); - // Start sampling ADCs. - stabilizer.adc_dac_timer.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, @@ -127,7 +127,7 @@ const APP: () = { .pll .update(c.resources.timestamper.latest_timestamp()); - // Harmonic index to demodulate + // Harmonic index of the LO: -1 to _de_modulate the fundamental let harmonic: i32 = -1; // Demodulation LO phase offset let phase_offset: i32 = 0; @@ -139,13 +139,13 @@ const APP: () = { // Convert to signed, MSB align the ADC sample. let input = (adc_samples[0][i] as i16 as i32) << 16; // Obtain demodulated, filtered IQ sample. - lockin.update(input, sample_phase); + 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 = lockin.power() as _; - let mut phase = lockin.phase() as _; + 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. From eea5033d36bdaa2a6596b2ce31ffe938fdcf7316 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 22 Jan 2021 11:38:38 +0100 Subject: [PATCH 23/25] dsp bench: fix --- dsp/benches/trig.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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; From d0d2c6352d642259ff2059841d3732480d9c1407 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 22 Jan 2021 15:11:16 +0100 Subject: [PATCH 24/25] lockin: refactor to use common lockin processing --- dsp/src/iir_int.rs | 18 +------- dsp/src/lockin.rs | 8 ++-- dsp/tests/lockin.rs | 109 ++++++++++++++++++-------------------------- src/bin/lockin.rs | 4 +- 4 files changed, 54 insertions(+), 85 deletions(-) diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs index c31f208..ea78dd6 100644 --- a/dsp/src/iir_int.rs +++ b/dsp/src/iir_int.rs @@ -3,20 +3,6 @@ use serde::{Deserialize, Serialize}; #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIRState(pub [i32; 5]); -impl IIRState { - #[inline(always)] - pub fn get_x(&self, index: usize) -> i32 { - // x0 is at index 0 in a biquad between updates - self.0[index] - } - - #[inline(always)] - pub fn get_y(&self, index: usize) -> i32 { - // y0 is at index 2 in a biquad between updates - self.0[2 + index] - } -} - fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { // Rounding bias, half up let y0 = ((y0 as i64) << shift) + (1 << (shift - 1)); @@ -42,8 +28,8 @@ pub struct IIR { } impl IIR { - /// Coefficient fixed point: signed Q2.30. - /// Tailored to low-passes PI, II etc. + /// 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 diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 591d7e0..a7c9e21 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -8,16 +8,18 @@ pub struct Lockin { } impl Lockin { - pub fn new(_corner: u8) -> Self { + 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_int::IIR::default(), // TODO: lowpass coefficients from corner + 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.wrapping_neg()); + let m = cossin(phase); // Mix with the LO signal, filter with the IIR lowpass, // return IQ (in-phase and quadrature) data. diff --git a/dsp/tests/lockin.rs b/dsp/tests/lockin.rs index dc57dcb..19d8c4c 100644 --- a/dsp/tests/lockin.rs +++ b/dsp/tests/lockin.rs @@ -1,7 +1,8 @@ use dsp::{ - atan2, cossin, + atan2, iir_int::{IIRState, IIR}, reciprocal_pll::TimestampHandler, + lockin::Lockin, Complex, }; @@ -30,51 +31,39 @@ pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool { } /// ADC full scale in machine units (16 bit signed). -const ADC_SCALE: f64 = ((1 << 15) - 1) as f64; +const ADC_SCALE: f64 = ((1 << 15) - 1) as _; -struct Lockin { - harmonic: u32, - phase: u32, - iir: IIR, - iir_state: [IIRState; 2], +struct PllLockin { + harmonic: i32, + phase: i32, + lockin: Lockin, } -impl Lockin { - pub fn new(harmonic: u32, phase: u32, iir: IIR) -> Self { - Lockin { +impl PllLockin { + pub fn new(harmonic: i32, phase: i32, iir: &IIRState) -> Self { + PllLockin { harmonic, phase, - iir, - iir_state: [IIRState::default(); 2], + lockin: Lockin::new(iir) } } pub fn update( &mut self, input: Vec, - phase: u32, - frequency: u32, + phase: i32, + frequency: i32, ) -> Complex { - let frequency = frequency.wrapping_mul(self.harmonic); - let mut phase = + 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 m = cossin((phase as i32).wrapping_neg()); - phase = phase.wrapping_add(frequency); - - let signal = (s as i32) << 16; - 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 _, - ), - ) + 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()) @@ -138,10 +127,9 @@ fn sampled_noise_amplitude( tones .iter() .map(|t| { - let decades = - ((t.frequency - frequency) / corner).abs().max(1.).log10(); - // 2nd-order filter: 40dB/decade rolloff. - linear(t.amplitude_dbfs - 40. * decades) + 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 @@ -152,21 +140,18 @@ fn sampled_noise_amplitude( /// only if one occurred during the batch. /// /// # Args -/// * `reference_frequency` - External reference signal frequency (in Hz). +/// * `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. -/// * `internal_frequency` - Internal clock frequency (in Hz). /// /// # Returns /// An Option, containing a timestamp if one occurred during the current batch period. fn adc_batch_timestamps( - reference_frequency: f64, + reference_period: f64, timestamp_start: u64, timestamp_stop: u64, - internal_frequency: f64, ) -> Option { - let reference_period = internal_frequency / reference_frequency; let start_count = timestamp_start as f64 % reference_period; let timestamp = (reference_period - start_count) % reference_period; @@ -374,7 +359,7 @@ fn lowpass_test( internal_frequency: f64, reference_frequency: f64, demodulation_phase_offset: f64, - harmonic: u32, + harmonic: i32, sample_buffer_size_log2: usize, pll_shift_frequency: u8, pll_shift_phase: u8, @@ -402,17 +387,14 @@ fn lowpass_test( "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 = Lockin::new( + let mut lockin = PllLockin::new( harmonic, - (demodulation_phase_offset / (2. * PI) * (1_u64 << 32) as f64).round() - as u32, - IIR { - ba: lowpass_iir_coefficients( + (demodulation_phase_offset / (2. * PI) * (1i64 << 32) as f64).round() + as i32, + &lowpass_iir_coefficients( corner_frequency, - 1. / 2f64.sqrt(), - 2., - ), - }, + 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, @@ -470,10 +452,9 @@ fn lowpass_test( 1 << sample_buffer_size_log2, ); let timestamp = adc_batch_timestamps( - reference_frequency, + internal_frequency/reference_frequency, timestamp_start, timestamp_start + batch_sample_count - 1, - internal_frequency, ); timestamp_start += batch_sample_count; @@ -481,8 +462,8 @@ fn lowpass_test( timestamp_handler.update(timestamp); let output = lockin.update( adc_signal, - demodulation_initial_phase, - demodulation_frequency, + 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)) @@ -571,7 +552,7 @@ fn lowpass_test( fn lowpass() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 64e-3; - let harmonic: u32 = 1; + let harmonic: i32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 3; let pll_shift_phase: u8 = 2; @@ -616,7 +597,7 @@ fn lowpass() { fn lowpass_demodulation_phase_offset_pi_2() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 64e-3; - let harmonic: u32 = 1; + let harmonic: i32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 3; let pll_shift_phase: u8 = 2; @@ -661,7 +642,7 @@ fn lowpass_demodulation_phase_offset_pi_2() { fn lowpass_phase_offset_pi_2() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 64e-3; - let harmonic: u32 = 1; + let harmonic: i32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 3; let pll_shift_phase: u8 = 2; @@ -706,7 +687,7 @@ fn lowpass_phase_offset_pi_2() { fn lowpass_fundamental_71e_3_phase_offset_pi_4() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 71e-3; - let harmonic: u32 = 1; + let harmonic: i32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 3; let pll_shift_phase: u8 = 2; @@ -751,7 +732,7 @@ fn lowpass_fundamental_71e_3_phase_offset_pi_4() { fn lowpass_first_harmonic() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 50e-3; - let harmonic: u32 = 2; + let harmonic: i32 = 2; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; @@ -796,7 +777,7 @@ fn lowpass_first_harmonic() { fn lowpass_second_harmonic() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 50e-3; - let harmonic: u32 = 3; + let harmonic: i32 = 3; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; @@ -841,7 +822,7 @@ fn lowpass_second_harmonic() { fn lowpass_third_harmonic() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 50e-3; - let harmonic: u32 = 4; + let harmonic: i32 = 4; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; @@ -886,7 +867,7 @@ fn lowpass_third_harmonic() { fn lowpass_first_harmonic_phase_shift() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 50e-3; - let harmonic: u32 = 2; + let harmonic: i32 = 2; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; @@ -931,7 +912,7 @@ fn lowpass_first_harmonic_phase_shift() { fn lowpass_adc_frequency_1e6() { let internal_frequency: f64 = 32.; let signal_frequency: f64 = 100e-3; - let harmonic: u32 = 1; + let harmonic: i32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; @@ -976,7 +957,7 @@ fn lowpass_adc_frequency_1e6() { fn lowpass_internal_frequency_125e6() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 100e-3; - let harmonic: u32 = 1; + let harmonic: i32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; @@ -1021,7 +1002,7 @@ fn lowpass_internal_frequency_125e6() { fn lowpass_low_signal_frequency() { let internal_frequency: f64 = 64.; let signal_frequency: f64 = 10e-3; - let harmonic: u32 = 1; + let harmonic: i32 = 1; let sample_buffer_size_log2: usize = 2; let pll_shift_frequency: u8 = 2; let pll_shift_phase: u8 = 1; diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index a9a63d4..4315fbf 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -16,7 +16,7 @@ use stabilizer::{ hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2, }; -use dsp::{iir, lockin::Lockin, reciprocal_pll::TimestampHandler}; +use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler}; use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, }; @@ -61,7 +61,7 @@ const APP: () = { ); let lockin = Lockin::new( - 10, // relative Locking lowpass filter bandwidth, TODO: expose + &iir_int::IIRState::default(), // TODO: lowpass, expose ); // Enable ADC/DAC events From 57a5c4ff9b28a39536381d44265887dd7ead7ee1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 22 Jan 2021 16:03:26 +0100 Subject: [PATCH 25/25] make lockin a unittest, not integration test --- dsp/src/lockin.rs | 1029 ++++++++++++++++++++++++++++++++++++++++++ dsp/tests/lockin.rs | 1037 ------------------------------------------- src/bin/lockin.rs | 2 +- 3 files changed, 1030 insertions(+), 1038 deletions(-) delete mode 100644 dsp/tests/lockin.rs diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index a7c9e21..72f505c 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -36,3 +36,1032 @@ impl Lockin { ) } } + +#[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/tests/lockin.rs b/dsp/tests/lockin.rs deleted file mode 100644 index 19d8c4c..0000000 --- a/dsp/tests/lockin.rs +++ /dev/null @@ -1,1037 +0,0 @@ -use dsp::{ - atan2, - iir_int::{IIRState, IIR}, - reciprocal_pll::TimestampHandler, - lockin::Lockin, - Complex, -}; - -use std::f64::consts::PI; -use std::vec::Vec; - -// TODO: -> dsp/src/testing.rs -/// 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 -} - -/// 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/src/bin/lockin.rs b/src/bin/lockin.rs index 4315fbf..304c35e 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -61,7 +61,7 @@ const APP: () = { ); let lockin = Lockin::new( - &iir_int::IIRState::default(), // TODO: lowpass, expose + &iir_int::IIRState::default(), // TODO: lowpass, expose ); // Enable ADC/DAC events