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" 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/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..b75038e 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,12 +35,28 @@ 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)] 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 2bfd29c..0006831 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -3,13 +3,14 @@ /// 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 - 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 } @@ -18,14 +19,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() } } @@ -43,42 +42,231 @@ 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, shift_frequency: u8, shift_phase: u8, - ) -> (i32, i32) { + ) -> (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); + 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 * dx as u64; // Add half-up rounding bias and apply gain/attenuation - let p_sig = (p_sig_long.wrapping_add(1i64 << (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)); + 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)) as u32; // Reference phase estimate "now" - let y_ref = (self.f >> self.dt2).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)); + self.f = self.ff.wrapping_add(dy as u32); } - // Advance time - self.t = self.t.wrapping_add(1 << self.dt2); (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); + } + + struct Harness { + rpll: RPLL, + dt2: u8, + shift_frequency: u8, + shift_phase: u8, + noise: i32, + period: i32, + next: i32, + next_noisy: i32, + time: i32, + rng: StdRng, + } + + impl Harness { + fn default() -> Self { + Harness { + 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), + } + } + + 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_noisy >= 0 { + assert!(self.time - self.next_noisy < 1 << self.dt2); + 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) + } 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; + // 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 u64 * self.period as u64; + // relative frequency error + 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); + } + (y, f) + } + + 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); + 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); + + 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(); + 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); + + 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!(); + } + } + + #[test] + fn default() { + let mut h = Harness::default(); + + h.measure(1 << 16, [1e-11, 4e-8, 2e-8, 2e-8]); + } + + #[test] + fn noisy() { + let mut h = Harness::default(); + h.noise = 10; + h.shift_frequency = 23; + h.shift_phase = 22; + + h.measure(1 << 16, [3e-9, 3e-6, 4e-4, 2e-4]); + } + + #[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; + + h.measure(1 << 16, [2e-9, 2e-6, 1e-3, 1e-4]); + } + + #[test] + fn narrow_slow() { + let mut h = Harness::default(); + h.period = 1818181; + h.next = 35281; + h.next_noisy = h.next; + h.noise = 1000; + h.shift_frequency = 23; + h.shift_phase = 22; + + h.measure(1 << 16, [2e-5, 6e-4, 2e-4, 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; + + h.measure(1 << 16, [5e-7, 3e-2, 3e-2, 2e-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; + + h.measure(1 << 16, [2e-4, 6e-3, 2e-4, 2e-3]); + } +} diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 7066ad8..f9e2f58 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 @@ -119,30 +119,30 @@ 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 ); - // 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).wrapping_mul(harmonic); - let mut sample_phase = + let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2) + as i32) // TODO: maybe rounding bias + .wrapping_mul(harmonic); + 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. @@ -153,11 +153,13 @@ const APP: () = { // 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::() >> 16) as u16 ^ 0x8000; + dac_samples[1][i] = + (phase.to_int_unchecked::() >> 16) as u16 ^ 0x8000; + } } } }