From 45e7d6de3cb09d7b722bfcbc5b47d2cd42a3ca51 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 26 Jan 2021 22:30:46 +0100 Subject: [PATCH 01/13] rpll: auto-align counter --- dsp/src/rpll.rs | 18 ++++++++++++++---- src/bin/lockin.rs | 2 +- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 2bfd29c..292c523 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -18,14 +18,12 @@ impl RPLL { /// /// Args: /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio. - /// * t: Counter time. Counter value at the first update() call. Typically 0. /// /// Returns: /// Initialized RPLL instance. - pub fn new(dt2: u8, t: i32) -> RPLL { + pub fn new(dt2: u8) -> RPLL { RPLL { dt2, - t, ..Default::default() } } @@ -69,7 +67,19 @@ impl RPLL { // Update frequency lock self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig)); // Time in counter cycles between timestamp and "now" - let dt = self.t.wrapping_sub(x); + let mut dt = self.t.wrapping_sub(x); + + if dt < 0 { + // Timestamp is in the future + // Advance phase and time until we are just past the most recent + // timestamp. + let mut dt_ceil = dt >> self.dt2; + self.y = self.y.wrapping_sub(self.f.wrapping_mul(dt_ceil)); + dt_ceil <<= self.dt2; + self.t = self.t.wrapping_sub(dt_ceil); + dt = dt.wrapping_sub(dt_ceil); + } + // Reference phase estimate "now" let y_ref = (self.f >> self.dt2).wrapping_mul(dt); // Phase error diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 7066ad8..1c2aa1f 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -53,7 +53,7 @@ const APP: () = { // Configure the microcontroller let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); - let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0); + let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2); let lockin = Lockin::new( &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose From 1749d48ca390a4df27dd770a998914fd5fd5fded Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 27 Jan 2021 09:00:48 +0100 Subject: [PATCH 02/13] Revert "rpll: auto-align counter" This reverts commit dbacc5293e12f712fef7bd85848e1b0bd8fde823. --- dsp/src/rpll.rs | 18 ++++-------------- src/bin/lockin.rs | 2 +- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 292c523..2bfd29c 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -18,12 +18,14 @@ impl RPLL { /// /// Args: /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio. + /// * t: Counter time. Counter value at the first update() call. Typically 0. /// /// Returns: /// Initialized RPLL instance. - pub fn new(dt2: u8) -> RPLL { + pub fn new(dt2: u8, t: i32) -> RPLL { RPLL { dt2, + t, ..Default::default() } } @@ -67,19 +69,7 @@ impl RPLL { // Update frequency lock self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig)); // Time in counter cycles between timestamp and "now" - let mut dt = self.t.wrapping_sub(x); - - if dt < 0 { - // Timestamp is in the future - // Advance phase and time until we are just past the most recent - // timestamp. - let mut dt_ceil = dt >> self.dt2; - self.y = self.y.wrapping_sub(self.f.wrapping_mul(dt_ceil)); - dt_ceil <<= self.dt2; - self.t = self.t.wrapping_sub(dt_ceil); - dt = dt.wrapping_sub(dt_ceil); - } - + let dt = self.t.wrapping_sub(x); // Reference phase estimate "now" let y_ref = (self.f >> self.dt2).wrapping_mul(dt); // Phase error diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 1c2aa1f..7066ad8 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -53,7 +53,7 @@ const APP: () = { // Configure the microcontroller let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); - let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2); + let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0); let lockin = Lockin::new( &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose From 36288225b3898ff0ab132077314a02f82fe3899a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 28 Jan 2021 22:21:42 +0100 Subject: [PATCH 03/13] rpll: extend to above-nyquist frequencies --- dsp/src/rpll.rs | 20 +++++++++++--------- src/bin/lockin.rs | 5 +++-- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 2bfd29c..e3b3894 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -8,8 +8,8 @@ pub struct RPLL { dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio t: i32, // current counter time x: i32, // previous timestamp - ff: i32, // current frequency estimate from frequency loop - f: i32, // current frequency estimate from both frequency and phase loop + ff: u32, // current frequency estimate from frequency loop + f: u32, // current frequency estimate from both frequency and phase loop y: i32, // current phase estimate } @@ -49,33 +49,35 @@ impl RPLL { input: Option, shift_frequency: u8, shift_phase: u8, - ) -> (i32, i32) { + ) -> (i32, u32) { debug_assert!(shift_frequency > self.dt2); debug_assert!(shift_phase > self.dt2); // Advance phase - self.y = self.y.wrapping_add(self.f); + self.y = self.y.wrapping_add(self.f as i32); if let Some(x) = input { // Reference period in counter cycles let dx = x.wrapping_sub(self.x); // Store timestamp for next time. self.x = x; // Phase using the current frequency estimate - let p_sig_long = (self.ff as i64).wrapping_mul(dx as i64); + let p_sig_64 = (self.ff as u64).wrapping_mul(dx as u64); // Add half-up rounding bias and apply gain/attenuation - let p_sig = (p_sig_long.wrapping_add(1i64 << (shift_frequency - 1)) + let p_sig = (p_sig_64.wrapping_add(1u64 << (shift_frequency - 1)) >> shift_frequency) as i32; // Reference phase (1 << dt2 full turns) with gain/attenuation applied let p_ref = 1i32 << (32 + self.dt2 - shift_frequency); // Update frequency lock - self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig)); + self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32); // Time in counter cycles between timestamp and "now" let dt = self.t.wrapping_sub(x); // Reference phase estimate "now" - let y_ref = (self.f >> self.dt2).wrapping_mul(dt); + let y_ref = ((self.f >> self.dt2) as i32).wrapping_mul(dt); // Phase error let dy = y_ref.wrapping_sub(self.y); // Current frequency estimate from frequency lock and phase error - self.f = self.ff.wrapping_add(dy >> (shift_phase - self.dt2)); + self.f = self + .ff + .wrapping_add((dy >> (shift_phase - self.dt2)) as u32); } // Advance time self.t = self.t.wrapping_add(1 << self.dt2); diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 7066ad8..cbdda51 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -127,8 +127,9 @@ const APP: () = { let harmonic: i32 = -1; // Demodulation LO phase offset let phase_offset: i32 = 0; - let sample_frequency = - (pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2).wrapping_mul(harmonic); + let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2) + as i32) + .wrapping_mul(harmonic); // TODO: maybe rounding bias let mut sample_phase = phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); From c34e33066376a9904130dcebfb3748d3bffcb82b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 28 Jan 2021 23:00:55 +0100 Subject: [PATCH 04/13] lockin: fmt --- src/bin/lockin.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index cbdda51..1519853 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -129,7 +129,7 @@ const APP: () = { let phase_offset: i32 = 0; let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2) as i32) - .wrapping_mul(harmonic); // TODO: maybe rounding bias + .wrapping_mul(harmonic); // TODO: maybe rounding bias let mut sample_phase = phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); From 0d1b237202e67c9f2d27cf57806bbf2199e84c15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 30 Jan 2021 18:05:54 +0100 Subject: [PATCH 05/13] complex: richer API --- dsp/src/complex.rs | 55 ++++++++++++++++++++++++++++++++++++++++------ dsp/src/lockin.rs | 21 ++++++++++++++++-- src/bin/lockin.rs | 37 +++++++++++++++++-------------- 3 files changed, 87 insertions(+), 26 deletions(-) diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs index 4366b43..2bdb9ea 100644 --- a/dsp/src/complex.rs +++ b/dsp/src/complex.rs @@ -1,17 +1,58 @@ -use super::atan2; +use super::{atan2, cossin}; use serde::{Deserialize, Serialize}; -#[derive(Copy, Clone, Default, Deserialize, Serialize)] +#[derive(Copy, Clone, Default, PartialEq, Debug, 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 + /// Return a Complex on the unit circle given an angle. + /// + /// Example: + /// + /// ``` + /// use dsp::Complex; + /// Complex::::from_angle(0); + /// Complex::::from_angle(1 << 30); // pi/2 + /// Complex::::from_angle(-1 << 30); // -pi/2 + /// ``` + #[inline(always)] + pub fn from_angle(angle: i32) -> Complex { + cossin(angle) } - pub fn phase(&self) -> i32 { + /// Return the absolute square (the squared magnitude). + /// + /// Note: Normalization is `1 << 31`, i.e. Q0.31. + /// + /// Example: + /// + /// ``` + /// use dsp::Complex; + /// assert_eq!(Complex(i32::MAX, 0).abs_sqr(), i32::MAX - 1); + /// assert_eq!(Complex(i32::MIN + 1, 0).abs_sqr(), i32::MAX - 1); + /// ``` + pub fn abs_sqr(&self) -> i32 { + (((self.0 as i64) * (self.0 as i64) + + (self.1 as i64) * (self.1 as i64)) + >> 31) as i32 + } + + /// Return the angle. + /// + /// Note: Normalization is `1 << 31 == pi`. + /// + /// Example: + /// + /// ``` + /// use dsp::Complex; + /// assert_eq!(Complex(i32::MAX, 0).arg(), 0); + /// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX); + /// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX); + /// assert_eq!(Complex(0, -i32::MAX).arg(), -i32::MAX >> 1); + /// assert_eq!(Complex(0, i32::MAX).arg(), (i32::MAX >> 1) + 1); + /// assert_eq!(Complex(i32::MAX, i32::MAX).arg(), (i32::MAX >> 2) + 1); + /// ``` + pub fn arg(&self) -> i32 { atan2(self.1, self.0) } } diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index aa397bb..b254bea 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,4 +1,4 @@ -use super::{cossin, iir_int, Complex}; +use super::{iir_int, Complex}; use serde::{Deserialize, Serialize}; #[derive(Copy, Clone, Default, Deserialize, Serialize)] @@ -19,7 +19,7 @@ impl Lockin { pub fn update(&mut self, signal: i32, phase: i32) -> Complex { // Get the LO signal for demodulation. - let m = cossin(phase); + let m = Complex::from_angle(phase); // Mix with the LO signal, filter with the IIR lowpass, // return IQ (in-phase and quadrature) data. @@ -35,6 +35,23 @@ impl Lockin { ), ) } + + pub fn feed>( + &mut self, + signal: I, + phase: i32, + frequency: i32, + ) -> Option> { + let mut phase = phase; + + signal + .into_iter() + .map(|s| { + phase = phase.wrapping_add(frequency); + self.update(s, phase) + }) + .last() + } } #[cfg(test)] diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 1519853..2b40dbd 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -123,27 +123,26 @@ const APP: () = { 22, // relative PLL phase bandwidth: 2**-22, TODO: expose ); - // Harmonic index of the LO: -1 to _de_modulate the fundamental + // Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate) let harmonic: i32 = -1; // Demodulation LO phase offset let phase_offset: i32 = 0; let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2) as i32) .wrapping_mul(harmonic); // TODO: maybe rounding bias - let mut sample_phase = + let sample_phase = phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); - for i in 0..adc_samples[0].len() { - // Convert to signed, MSB align the ADC sample. - let input = (adc_samples[0][i] as i16 as i32) << 16; - // Obtain demodulated, filtered IQ sample. - let output = lockin.update(input, sample_phase); - // Advance the sample phase. - sample_phase = sample_phase.wrapping_add(sample_frequency); - + if let Some(output) = lockin.feed( + adc_samples[0].iter().map(|&i| + // Convert to signed, MSB align the ADC sample. + (i as i16 as i32) << 16), + sample_phase, + sample_frequency, + ) { // Convert from IQ to power and phase. - let mut power = output.power() as _; - let mut phase = output.phase() as _; + let mut power = output.abs_sqr() as _; + let mut phase = output.arg() as _; // Filter power and phase through IIR filters. // Note: Normalization to be done in filters. Phase will wrap happily. @@ -152,13 +151,17 @@ const APP: () = { phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); } + // TODO: IIR filter DC gain needs to be 1/(1 << 16) + // Note(unsafe): range clipping to i16 is ensured by IIR filters above. // Convert to DAC data. - unsafe { - dac_samples[0][i] = - power.to_int_unchecked::() as u16 ^ 0x8000; - dac_samples[1][i] = - phase.to_int_unchecked::() as u16 ^ 0x8000; + for i in 0..dac_samples[0].len() { + unsafe { + dac_samples[0][i] = + power.to_int_unchecked::() as u16 ^ 0x8000; + dac_samples[1][i] = + phase.to_int_unchecked::() as u16 ^ 0x8000; + } } } } From be7aad1b81d9d55e922788fdfa38971874b9b129 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 30 Jan 2021 20:49:31 +0100 Subject: [PATCH 06/13] rpll: add unittest --- dsp/Cargo.toml | 3 ++ dsp/src/lockin.rs | 1 - dsp/src/rpll.rs | 130 +++++++++++++++++++++++++++++++++++++++++++++- 3 files changed, 132 insertions(+), 2 deletions(-) diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index 548e64f..16fd500 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -10,6 +10,9 @@ serde = { version = "1.0", features = ["derive"], default-features = false } [dev-dependencies] criterion = "0.3" +rand = "0.8" +ndarray = "0.14" +ndarray-stats = "0.4" [[bench]] name = "trig" diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index b254bea..b75038e 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -57,7 +57,6 @@ impl Lockin { #[cfg(test)] mod test { use crate::{ - atan2, iir_int::IIRState, lockin::Lockin, rpll::RPLL, diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index e3b3894..057abc8 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -51,7 +51,7 @@ impl RPLL { shift_phase: u8, ) -> (i32, u32) { debug_assert!(shift_frequency > self.dt2); - debug_assert!(shift_phase > self.dt2); + debug_assert!(shift_phase >= self.dt2); // Advance phase self.y = self.y.wrapping_add(self.f as i32); if let Some(x) = input { @@ -84,3 +84,131 @@ impl RPLL { (self.y, self.f) } } + +#[cfg(test)] +mod test { + use super::RPLL; + use ndarray::prelude::*; + use ndarray_stats::QuantileExt; + use rand::{prelude::*, rngs::StdRng}; + use std::vec::Vec; + + #[test] + fn make() { + let _ = RPLL::new(8, 0); + } + + struct Harness { + rpll: RPLL, + dt2: u8, + shift_frequency: u8, + shift_phase: u8, + noise: i32, + period: i32, + next: i32, + time: i32, + rng: StdRng, + } + + impl Harness { + fn default() -> Self { + Harness { + rpll: RPLL::new(8, 0), + dt2: 8, + shift_frequency: 9, + shift_phase: 8, + noise: 0, + period: 333, + next: 111, + time: 0, + rng: StdRng::seed_from_u64(42), + } + } + + fn run(&mut self, n: i32) -> (Vec, Vec) { + let mut y = Vec::::new(); + let mut f = Vec::::new(); + for _ in 0..n { + let timestamp = if self.time - self.next >= 0 { + let p_noise = self.rng.gen_range(-self.noise..=self.noise); + let timestamp = self.next.wrapping_add(p_noise); + self.next = self.next.wrapping_add(self.period); + Some(timestamp) + } else { + None + }; + let (yi, fi) = self.rpll.update( + timestamp, + self.shift_frequency, + self.shift_phase, + ); + let y_ref = (self.time.wrapping_sub(self.next) as i64 + * (1i64 << 32) + / self.period as i64) as i32; + y.push((yi as i64).wrapping_sub(y_ref as i64)); // phase error + let p_ref = 1 << 32 + self.dt2; + let p_sig = fi as i64 * self.period as i64; + f.push(p_sig.wrapping_sub(p_ref)); // relative frequency error + // advance time + self.time = self.time.wrapping_add(1 << self.dt2); + } + (y, f) + } + } + + #[test] + fn default() { + let mut h = Harness::default(); + h.run(1 << 10); // settle + + let (y, f) = h.run(1 << 10); + let y = Array::from(y); + let f = Array::from(f); + + println!( + "f: {}+-{}", + f.mean().unwrap(), + *f.mapv(i64::abs).max().unwrap() + ); + println!( + "y: {}+-{}", + y.mean().unwrap(), + *y.mapv(i64::abs).max().unwrap() + ); + + assert!(f.mean().unwrap().abs() < 20); + assert!(*f.mapv(i64::abs).max().unwrap() < 1 << 16); + assert!(y.mean().unwrap().abs() < 1 << 6); + assert!(*y.mapv(i64::abs).max().unwrap() < 1 << 8); + } + + #[test] + fn noise() { + let mut h = Harness::default(); + h.noise = 10; + h.shift_frequency = 23; + h.shift_phase = 22; + + h.run(1 << 20); // settle + + let (y, f) = h.run(1 << 10); + let y = Array::from(y); + let f = Array::from(f); + + println!( + "f: {}+-{}", + f.mean().unwrap(), + *f.mapv(i64::abs).max().unwrap() + ); + println!( + "y: {}+-{}", + y.mean().unwrap(), + *y.mapv(i64::abs).max().unwrap() + ); + + assert!(f.mean().unwrap().abs() < 1 << 14); + assert!(*f.mapv(i64::abs).max().unwrap() < 1 << 22); + assert!(y.mean().unwrap().abs() < 1 << 19); + assert!(*y.mapv(i64::abs).max().unwrap() < 1 << 19); + } +} From 183d84ad7af028b749951a1d10ecb9036f13e472 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 30 Jan 2021 20:57:44 +0100 Subject: [PATCH 07/13] cargo.lock: update --- Cargo.lock | 195 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 195 insertions(+) diff --git a/Cargo.lock b/Cargo.lock index c6083ae..4f6bca2 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -347,6 +347,9 @@ version = "0.1.0" dependencies = [ "criterion", "libm", + "ndarray", + "ndarray-stats", + "rand 0.8.3", "serde", ] @@ -423,6 +426,28 @@ dependencies = [ "version_check", ] +[[package]] +name = "getrandom" +version = "0.1.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fc3cb4d91f53b50155bdcfd23f6a4c39ae1969c2ae85982b135750cccaf5fce" +dependencies = [ + "cfg-if 1.0.0", + "libc", + "wasi 0.9.0+wasi-snapshot-preview1", +] + +[[package]] +name = "getrandom" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c9495705279e7140bf035dde1f6e750c162df8b625267cd52cc44e0b156732c8" +dependencies = [ + "cfg-if 1.0.0", + "libc", + "wasi 0.10.2+wasi-snapshot-preview1", +] + [[package]] name = "half" version = "1.6.0" @@ -533,6 +558,15 @@ version = "0.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c75de51135344a4f8ed3cfe2720dc27736f7711989703a0b43aadf3753c55577" +[[package]] +name = "matrixmultiply" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "916806ba0031cd542105d916a97c8572e1fa6dd79c9c51e7eb43a09ec2dd84c1" +dependencies = [ + "rawpointer", +] + [[package]] name = "mcp23017" version = "0.1.1" @@ -571,6 +605,62 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae" +[[package]] +name = "ndarray" +version = "0.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6c0d5c9540a691d153064dc47a4db2504587a75eae07bf1d73f7a596ebc73c04" +dependencies = [ + "matrixmultiply", + "num-complex", + "num-integer", + "num-traits", + "rawpointer", +] + +[[package]] +name = "ndarray-stats" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22c95a780960082c5746f6bf0ab22d4a3b8cee72bf580acfe9f1e10bc5ea8152" +dependencies = [ + "indexmap", + "itertools", + "ndarray", + "noisy_float", + "num-integer", + "num-traits", + "rand 0.7.3", +] + +[[package]] +name = "noisy_float" +version = "0.1.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a14c16cde392a1cd18084ffd8348cb8937525130e62f0478d72dcc683698809d" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-complex" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "747d632c0c558b87dbabbe6a82f3b4ae03720d0646ac5b7b4dae89394be5f2c5" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2cc698a63b549a70bc047073d2949cce27cd1c7b0a4a862d08a8031bc2801db" +dependencies = [ + "autocfg", + "num-traits", +] + [[package]] name = "num-traits" version = "0.2.14" @@ -630,6 +720,12 @@ dependencies = [ "web-sys", ] +[[package]] +name = "ppv-lite86" +version = "0.2.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac74c624d6b2d21f425f752262f42188365d7b8ff1aff74c82e45136510a4857" + [[package]] name = "proc-macro2" version = "1.0.24" @@ -654,6 +750,93 @@ version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f" +[[package]] +name = "rand" +version = "0.7.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a6b1679d49b24bbfe0c803429aa1874472f50d9b363131f0e89fc356b544d03" +dependencies = [ + "getrandom 0.1.16", + "libc", + "rand_chacha 0.2.2", + "rand_core 0.5.1", + "rand_hc 0.2.0", +] + +[[package]] +name = "rand" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ef9e7e66b4468674bfcb0c81af8b7fa0bb154fa9f28eb840da5c447baeb8d7e" +dependencies = [ + "libc", + "rand_chacha 0.3.0", + "rand_core 0.6.1", + "rand_hc 0.3.0", +] + +[[package]] +name = "rand_chacha" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f4c8ed856279c9737206bf725bf36935d8666ead7aa69b52be55af369d193402" +dependencies = [ + "ppv-lite86", + "rand_core 0.5.1", +] + +[[package]] +name = "rand_chacha" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e12735cf05c9e10bf21534da50a147b924d555dc7a547c42e6bb2d5b6017ae0d" +dependencies = [ + "ppv-lite86", + "rand_core 0.6.1", +] + +[[package]] +name = "rand_core" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "90bde5296fc891b0cef12a6d03ddccc162ce7b2aff54160af9338f8d40df6d19" +dependencies = [ + "getrandom 0.1.16", +] + +[[package]] +name = "rand_core" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c026d7df8b298d90ccbbc5190bd04d85e159eaf5576caeacf8741da93ccbd2e5" +dependencies = [ + "getrandom 0.2.2", +] + +[[package]] +name = "rand_hc" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ca3129af7b92a17112d59ad498c6f81eaf463253766b90396d39ea7a39d6613c" +dependencies = [ + "rand_core 0.5.1", +] + +[[package]] +name = "rand_hc" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3190ef7066a446f2e7f42e239d161e905420ccab01eb967c9eb27d21b2322a73" +dependencies = [ + "rand_core 0.6.1", +] + +[[package]] +name = "rawpointer" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" + [[package]] name = "rayon" version = "1.5.0" @@ -975,6 +1158,18 @@ dependencies = [ "winapi-util", ] +[[package]] +name = "wasi" +version = "0.9.0+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cccddf32554fecc6acb585f82a32a72e28b48f8c4c1883ddfeeeaa96f7d8e519" + +[[package]] +name = "wasi" +version = "0.10.2+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fd6fbd9a79829dd1ad0cc20627bf1ed606756a7f77edff7b66b7064f9cb327c6" + [[package]] name = "wasm-bindgen" version = "0.2.69" From 6b2d8169f068f1587e7305218cd66c235dec7bbf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 31 Jan 2021 13:25:01 +0100 Subject: [PATCH 08/13] rpll: more/cleaner tests --- dsp/src/rpll.rs | 116 ++++++++++++++++++++++++++++-------------------- 1 file changed, 68 insertions(+), 48 deletions(-) diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 057abc8..d17d42f 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -125,9 +125,9 @@ mod test { } } - fn run(&mut self, n: i32) -> (Vec, Vec) { - let mut y = Vec::::new(); - let mut f = Vec::::new(); + fn run(&mut self, n: usize) -> (Vec, Vec) { + let mut y = Vec::::new(); + let mut f = Vec::::new(); for _ in 0..n { let timestamp = if self.time - self.next >= 0 { let p_noise = self.rng.gen_range(-self.noise..=self.noise); @@ -145,70 +145,90 @@ mod test { let y_ref = (self.time.wrapping_sub(self.next) as i64 * (1i64 << 32) / self.period as i64) as i32; - y.push((yi as i64).wrapping_sub(y_ref as i64)); // phase error + // phase error + y.push(yi.wrapping_sub(y_ref) as f32 / 2f32.powi(32)); let p_ref = 1 << 32 + self.dt2; let p_sig = fi as i64 * self.period as i64; - f.push(p_sig.wrapping_sub(p_ref)); // relative frequency error - // advance time + // relative frequency error + f.push(p_sig.wrapping_sub(p_ref) as f32 / 2f32.powi(32)); + // advance time self.time = self.time.wrapping_add(1 << self.dt2); } (y, f) } + + fn measure(&mut self, n: usize) -> (f32, f32, f32, f32) { + let t_settle = (1 << self.shift_frequency - self.dt2 + 4) + + (1 << self.shift_phase - self.dt2 + 4); + self.run(t_settle); + + let (y, f) = self.run(n); + let y = Array::from(y); + let f = Array::from(f); + + let fm = f.mean().unwrap(); + let fs = f.std_axis(Axis(0), 0.).into_scalar(); + let ym = y.mean().unwrap(); + let ys = y.std_axis(Axis(0), 0.).into_scalar(); + + println!("f: {:.2e}±{:.2e}; y: {:.2e}±{:.2e}", fm, fs, ym, ys); + (fm, fs, ym, ys) + } } #[test] fn default() { let mut h = Harness::default(); - h.run(1 << 10); // settle - let (y, f) = h.run(1 << 10); - let y = Array::from(y); - let f = Array::from(f); - - println!( - "f: {}+-{}", - f.mean().unwrap(), - *f.mapv(i64::abs).max().unwrap() - ); - println!( - "y: {}+-{}", - y.mean().unwrap(), - *y.mapv(i64::abs).max().unwrap() - ); - - assert!(f.mean().unwrap().abs() < 20); - assert!(*f.mapv(i64::abs).max().unwrap() < 1 << 16); - assert!(y.mean().unwrap().abs() < 1 << 6); - assert!(*y.mapv(i64::abs).max().unwrap() < 1 << 8); + let (fm, fs, ym, ys) = h.measure(1 << 16); + assert!(fm.abs() < 1e-9); + assert!(fs.abs() < 8e-6); + assert!(ym.abs() < 2e-8); + assert!(ys.abs() < 2e-8); } #[test] - fn noise() { + fn noisy() { let mut h = Harness::default(); h.noise = 10; h.shift_frequency = 23; h.shift_phase = 22; - h.run(1 << 20); // settle - - let (y, f) = h.run(1 << 10); - let y = Array::from(y); - let f = Array::from(f); - - println!( - "f: {}+-{}", - f.mean().unwrap(), - *f.mapv(i64::abs).max().unwrap() - ); - println!( - "y: {}+-{}", - y.mean().unwrap(), - *y.mapv(i64::abs).max().unwrap() - ); - - assert!(f.mean().unwrap().abs() < 1 << 14); - assert!(*f.mapv(i64::abs).max().unwrap() < 1 << 22); - assert!(y.mean().unwrap().abs() < 1 << 19); - assert!(*y.mapv(i64::abs).max().unwrap() < 1 << 19); + let (fm, fs, ym, ys) = h.measure(1 << 16); + assert!(fm.abs() < 1e-6); + assert!(fs.abs() < 6e-4); + assert!(ym.abs() < 2e-4); + assert!(ys.abs() < 2e-4); } + + #[test] + fn narrow_fast() { + let mut h = Harness::default(); + h.period = 990; + h.noise = 5; + h.shift_frequency = 23; + h.shift_phase = 22; + + let (fm, fs, ym, ys) = h.measure(1 << 16); + assert!(fm.abs() < 7e-6); + assert!(fs.abs() < 6e-4); + assert!(ym.abs() < 1e-3); + assert!(ys.abs() < 1e-4); + } + + /*#[test] + fn narrow_slow() { + let mut h = Harness::default(); + h.period = 1818181; + h.noise = 1800; + h.shift_frequency = 23; + h.shift_phase = 22; + + let (fm, fs, ym, ys) = h.measure(1 << 16); + assert!(fm.abs() < 1e-8); + assert!(fs.abs() < 6e-4); + assert!(ym.abs() < 2e-4); + assert!(ys.abs() < 2e-4); + } + */ } From ab20d67a073fb703fa37dfd82a593a500ba32e74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 31 Jan 2021 13:42:15 +0100 Subject: [PATCH 09/13] rpll: remove redundant time tracking --- dsp/src/rpll.rs | 28 ++++++++++++++-------------- src/bin/lockin.rs | 2 +- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index d17d42f..4cd74b8 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -6,7 +6,6 @@ #[derive(Copy, Clone, Default)] pub struct RPLL { dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio - t: i32, // current counter time x: i32, // previous timestamp ff: u32, // current frequency estimate from frequency loop f: u32, // current frequency estimate from both frequency and phase loop @@ -18,14 +17,12 @@ impl RPLL { /// /// Args: /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio. - /// * t: Counter time. Counter value at the first update() call. Typically 0. /// /// Returns: /// Initialized RPLL instance. - pub fn new(dt2: u8, t: i32) -> RPLL { + pub fn new(dt2: u8) -> RPLL { RPLL { dt2, - t, ..Default::default() } } @@ -69,7 +66,7 @@ impl RPLL { // Update frequency lock self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32); // Time in counter cycles between timestamp and "now" - let dt = self.t.wrapping_sub(x); + let dt = x.wrapping_neg() & ((1 << self.dt2) - 1); // Reference phase estimate "now" let y_ref = ((self.f >> self.dt2) as i32).wrapping_mul(dt); // Phase error @@ -79,8 +76,6 @@ impl RPLL { .ff .wrapping_add((dy >> (shift_phase - self.dt2)) as u32); } - // Advance time - self.t = self.t.wrapping_add(1 << self.dt2); (self.y, self.f) } } @@ -95,7 +90,7 @@ mod test { #[test] fn make() { - let _ = RPLL::new(8, 0); + let _ = RPLL::new(8); } struct Harness { @@ -106,6 +101,7 @@ mod test { noise: i32, period: i32, next: i32, + next_noisy: i32, time: i32, rng: StdRng, } @@ -113,13 +109,14 @@ mod test { impl Harness { fn default() -> Self { Harness { - rpll: RPLL::new(8, 0), + rpll: RPLL::new(8), dt2: 8, shift_frequency: 9, shift_phase: 8, noise: 0, period: 333, next: 111, + next_noisy: 111, time: 0, rng: StdRng::seed_from_u64(42), } @@ -129,10 +126,12 @@ mod test { let mut y = Vec::::new(); let mut f = Vec::::new(); for _ in 0..n { - let timestamp = if self.time - self.next >= 0 { - let p_noise = self.rng.gen_range(-self.noise..=self.noise); - let timestamp = self.next.wrapping_add(p_noise); + let timestamp = if self.time - self.next_noisy >= 0 { + assert!(self.time - self.next_noisy < 1 << self.dt2); + let timestamp = self.next_noisy; self.next = self.next.wrapping_add(self.period); + let p_noise = self.rng.gen_range(-self.noise..=self.noise); + self.next_noisy = self.next.wrapping_add(p_noise); Some(timestamp) } else { None @@ -197,7 +196,7 @@ mod test { let (fm, fs, ym, ys) = h.measure(1 << 16); assert!(fm.abs() < 1e-6); assert!(fs.abs() < 6e-4); - assert!(ym.abs() < 2e-4); + assert!(ym.abs() < 4e-4); assert!(ys.abs() < 2e-4); } @@ -216,7 +215,8 @@ mod test { assert!(ys.abs() < 1e-4); } - /*#[test] + /* + #[test] fn narrow_slow() { let mut h = Harness::default(); h.period = 1818181; diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 2b40dbd..f687c21 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -53,7 +53,7 @@ const APP: () = { // Configure the microcontroller let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); - let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0); + let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2); let lockin = Lockin::new( &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose From 82c8fa1a07abf5d41b70ff4e71ebd798f1c48f9b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 31 Jan 2021 17:10:03 +0100 Subject: [PATCH 10/13] rpll: extend tests --- dsp/src/rpll.rs | 93 +++++++++++++++++++++++++++++++++++------------ src/bin/lockin.rs | 6 +-- 2 files changed, 72 insertions(+), 27 deletions(-) diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 4cd74b8..4744093 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -57,24 +57,22 @@ impl RPLL { // Store timestamp for next time. self.x = x; // Phase using the current frequency estimate - let p_sig_64 = (self.ff as u64).wrapping_mul(dx as u64); + let p_sig_64 = self.ff as u64 * dx as u64; // Add half-up rounding bias and apply gain/attenuation - let p_sig = (p_sig_64.wrapping_add(1u64 << (shift_frequency - 1)) - >> shift_frequency) as i32; + let p_sig = ((p_sig_64 + (1u32 << (shift_frequency - 1)) as u64) + >> shift_frequency) as u32; // Reference phase (1 << dt2 full turns) with gain/attenuation applied - let p_ref = 1i32 << (32 + self.dt2 - shift_frequency); + let p_ref = 1u32 << (32 + self.dt2 - shift_frequency); // Update frequency lock self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32); // Time in counter cycles between timestamp and "now" - let dt = x.wrapping_neg() & ((1 << self.dt2) - 1); + let dt = (x.wrapping_neg() & ((1 << self.dt2) - 1)) as u32; // Reference phase estimate "now" - let y_ref = ((self.f >> self.dt2) as i32).wrapping_mul(dt); - // Phase error - let dy = y_ref.wrapping_sub(self.y); + let y_ref = (self.f >> self.dt2).wrapping_mul(dt) as i32; + // Phase error with gain + let dy = y_ref.wrapping_sub(self.y) >> (shift_phase - self.dt2); // Current frequency estimate from frequency lock and phase error - self.f = self - .ff - .wrapping_add((dy >> (shift_phase - self.dt2)) as u32); + self.f = self.ff.wrapping_add(dy as u32); } (self.y, self.f) } @@ -128,8 +126,8 @@ mod test { for _ in 0..n { let timestamp = if self.time - self.next_noisy >= 0 { assert!(self.time - self.next_noisy < 1 << self.dt2); - let timestamp = self.next_noisy; self.next = self.next.wrapping_add(self.period); + let timestamp = self.next_noisy; let p_noise = self.rng.gen_range(-self.noise..=self.noise); self.next_noisy = self.next.wrapping_add(p_noise); Some(timestamp) @@ -141,15 +139,21 @@ mod test { self.shift_frequency, self.shift_phase, ); + let y_ref = (self.time.wrapping_sub(self.next) as i64 * (1i64 << 32) / self.period as i64) as i32; // phase error y.push(yi.wrapping_sub(y_ref) as f32 / 2f32.powi(32)); + let p_ref = 1 << 32 + self.dt2; - let p_sig = fi as i64 * self.period as i64; + let p_sig = fi as u64 * self.period as u64; // relative frequency error - f.push(p_sig.wrapping_sub(p_ref) as f32 / 2f32.powi(32)); + f.push( + p_sig.wrapping_sub(p_ref) as i64 as f32 + / 2f32.powi(32 + self.dt2 as i32), + ); + // advance time self.time = self.time.wrapping_add(1 << self.dt2); } @@ -157,6 +161,10 @@ mod test { } fn measure(&mut self, n: usize) -> (f32, f32, f32, f32) { + assert!(self.period >= 1 << self.dt2); + assert!(self.dt2 <= self.shift_frequency); + assert!(self.period < 1 << self.shift_frequency); + assert!(self.period < 1 << self.shift_frequency + 1); let t_settle = (1 << self.shift_frequency - self.dt2 + 4) + (1 << self.shift_phase - self.dt2 + 4); self.run(t_settle); @@ -164,6 +172,7 @@ mod test { let (y, f) = self.run(n); let y = Array::from(y); let f = Array::from(f); + // println!("{:?} {:?}", f, y); let fm = f.mean().unwrap(); let fs = f.std_axis(Axis(0), 0.).into_scalar(); @@ -180,8 +189,8 @@ mod test { let mut h = Harness::default(); let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 1e-9); - assert!(fs.abs() < 8e-6); + assert!(fm.abs() < 1e-11); + assert!(fs.abs() < 4e-8); assert!(ym.abs() < 2e-8); assert!(ys.abs() < 2e-8); } @@ -194,8 +203,8 @@ mod test { h.shift_phase = 22; let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 1e-6); - assert!(fs.abs() < 6e-4); + assert!(fm.abs() < 3e-9); + assert!(fs.abs() < 3e-6); assert!(ym.abs() < 4e-4); assert!(ys.abs() < 2e-4); } @@ -204,31 +213,67 @@ mod test { fn narrow_fast() { let mut h = Harness::default(); h.period = 990; + h.next = 351; + h.next_noisy = h.next; h.noise = 5; h.shift_frequency = 23; h.shift_phase = 22; let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 7e-6); - assert!(fs.abs() < 6e-4); + assert!(fm.abs() < 2e-9); + assert!(fs.abs() < 2e-6); assert!(ym.abs() < 1e-3); assert!(ys.abs() < 1e-4); } - /* #[test] fn narrow_slow() { let mut h = Harness::default(); h.period = 1818181; - h.noise = 1800; + h.next = 35281; + h.next_noisy = h.next; + h.noise = 1000; h.shift_frequency = 23; h.shift_phase = 22; let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 1e-8); + assert!(fm.abs() < 2e-5); assert!(fs.abs() < 6e-4); assert!(ym.abs() < 2e-4); assert!(ys.abs() < 2e-4); } - */ + + #[test] + fn wide_fast() { + let mut h = Harness::default(); + h.period = 990; + h.next = 351; + h.next_noisy = h.next; + h.noise = 5; + h.shift_frequency = 10; + h.shift_phase = 9; + + let (fm, fs, ym, ys) = h.measure(1 << 16); + assert!(fm.abs() < 1e-3); + assert!(fs.abs() < 1e-1); + assert!(ym.abs() < 1e-4); + assert!(ys.abs() < 3e-2); + } + + #[test] + fn wide_slow() { + let mut h = Harness::default(); + h.period = 1818181; + h.next = 35281; + h.next_noisy = h.next; + h.noise = 1000; + h.shift_frequency = 21; + h.shift_phase = 20; + + let (fm, fs, ym, ys) = h.measure(1 << 16); + assert!(fm.abs() < 2e-4); + assert!(fs.abs() < 6e-3); + assert!(ym.abs() < 2e-4); + assert!(ys.abs() < 2e-3); + } } diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index f687c21..48c717a 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -119,7 +119,7 @@ const APP: () = { let (pll_phase, pll_frequency) = c.resources.pll.update( c.resources.timestamper.latest_timestamp().map(|t| t as i32), - 22, // relative PLL frequency bandwidth: 2**-22, TODO: expose + 23, // relative PLL frequency bandwidth: 2**-23, TODO: expose 22, // relative PLL phase bandwidth: 2**-22, TODO: expose ); @@ -128,8 +128,8 @@ const APP: () = { // Demodulation LO phase offset let phase_offset: i32 = 0; let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2) - as i32) - .wrapping_mul(harmonic); // TODO: maybe rounding bias + as i32) // TODO: maybe rounding bias + .wrapping_mul(harmonic); let sample_phase = phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); From 80055076b8b6af141543f466b45c9e29c0bf3b1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 31 Jan 2021 17:41:20 +0100 Subject: [PATCH 11/13] lockin: scale output --- src/bin/lockin.rs | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 48c717a..f9e2f58 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -151,16 +151,14 @@ const APP: () = { phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); } - // TODO: IIR filter DC gain needs to be 1/(1 << 16) - // Note(unsafe): range clipping to i16 is ensured by IIR filters above. // Convert to DAC data. for i in 0..dac_samples[0].len() { unsafe { dac_samples[0][i] = - power.to_int_unchecked::() as u16 ^ 0x8000; + (power.to_int_unchecked::() >> 16) as u16 ^ 0x8000; dac_samples[1][i] = - phase.to_int_unchecked::() as u16 ^ 0x8000; + (phase.to_int_unchecked::() >> 16) as u16 ^ 0x8000; } } } From d281783f2e60870632174fb9c791736b7ec7e898 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 31 Jan 2021 18:10:13 +0100 Subject: [PATCH 12/13] rpll: reduce code --- dsp/src/rpll.rs | 55 +++++++++++++++++++++---------------------------- 1 file changed, 23 insertions(+), 32 deletions(-) diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 4744093..9dee2a3 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -160,7 +160,7 @@ mod test { (y, f) } - fn measure(&mut self, n: usize) -> (f32, f32, f32, f32) { + fn measure(&mut self, n: usize, limits: [f32; 4]) { assert!(self.period >= 1 << self.dt2); assert!(self.dt2 <= self.shift_frequency); assert!(self.period < 1 << self.shift_frequency); @@ -180,7 +180,22 @@ mod test { let ys = y.std_axis(Axis(0), 0.).into_scalar(); println!("f: {:.2e}±{:.2e}; y: {:.2e}±{:.2e}", fm, fs, ym, ys); - (fm, fs, ym, ys) + + let m = [fm, fs, ym, ys]; + + print!("relative: "); + for i in 0..m.len() { + let rel = m[i].abs() / limits[i].abs(); + print!("{:.2e} ", rel); + assert!( + rel <= 1., + "idx {}, have |{}| > want {}", + i, + m[i], + limits[i] + ); + } + println!(""); } } @@ -188,11 +203,7 @@ mod test { fn default() { let mut h = Harness::default(); - let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 1e-11); - assert!(fs.abs() < 4e-8); - assert!(ym.abs() < 2e-8); - assert!(ys.abs() < 2e-8); + h.measure(1 << 16, [1e-11, 4e-8, 2e-8, 2e-8]); } #[test] @@ -202,11 +213,7 @@ mod test { h.shift_frequency = 23; h.shift_phase = 22; - let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 3e-9); - assert!(fs.abs() < 3e-6); - assert!(ym.abs() < 4e-4); - assert!(ys.abs() < 2e-4); + h.measure(1 << 16, [3e-9, 3e-6, 4e-4, 2e-4]); } #[test] @@ -219,11 +226,7 @@ mod test { h.shift_frequency = 23; h.shift_phase = 22; - let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 2e-9); - assert!(fs.abs() < 2e-6); - assert!(ym.abs() < 1e-3); - assert!(ys.abs() < 1e-4); + h.measure(1 << 16, [2e-9, 2e-6, 1e-3, 1e-4]); } #[test] @@ -236,11 +239,7 @@ mod test { h.shift_frequency = 23; h.shift_phase = 22; - let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 2e-5); - assert!(fs.abs() < 6e-4); - assert!(ym.abs() < 2e-4); - assert!(ys.abs() < 2e-4); + h.measure(1 << 16, [2e-5, 6e-4, 2e-4, 2e-4]); } #[test] @@ -253,11 +252,7 @@ mod test { h.shift_frequency = 10; h.shift_phase = 9; - let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 1e-3); - assert!(fs.abs() < 1e-1); - assert!(ym.abs() < 1e-4); - assert!(ys.abs() < 3e-2); + h.measure(1 << 16, [5e-7, 3e-2, 3e-2, 2e-2]); } #[test] @@ -270,10 +265,6 @@ mod test { h.shift_frequency = 21; h.shift_phase = 20; - let (fm, fs, ym, ys) = h.measure(1 << 16); - assert!(fm.abs() < 2e-4); - assert!(fs.abs() < 6e-3); - assert!(ym.abs() < 2e-4); - assert!(ys.abs() < 2e-3); + h.measure(1 << 16, [2e-4, 6e-3, 2e-4, 2e-3]); } } From 43342cef91eaaca5dbbac0b0e6ec4993af4ac8de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 31 Jan 2021 18:21:47 +0100 Subject: [PATCH 13/13] rpll: docs --- dsp/src/rpll.rs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 9dee2a3..0006831 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -3,6 +3,8 @@ /// Consumes noisy, quantized timestamps of a reference signal and reconstructs /// the phase and frequency of the update() invocations with respect to (and in units of /// 1 << 32 of) that reference. +/// In other words, `update()` rate ralative to reference frequency, +/// `u32::MAX` corresponding to both being equal. #[derive(Copy, Clone, Default)] pub struct RPLL { dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio @@ -40,7 +42,7 @@ impl RPLL { /// /// Returns: /// A tuple containing the current phase (wrapping at the i32 boundary, pi) and - /// frequency (wrapping at the i32 boundary, Nyquist) estimate. + /// frequency. pub fn update( &mut self, input: Option, @@ -195,7 +197,7 @@ mod test { limits[i] ); } - println!(""); + println!(); } }