diff --git a/Cargo.lock b/Cargo.lock index 0044143..b165491 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -355,8 +355,7 @@ dependencies = [ "criterion", "libm", "ndarray", - "ndarray-stats", - "rand 0.8.3", + "rand", "serde", ] @@ -433,17 +432,6 @@ 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", - "libc", - "wasi 0.9.0+wasi-snapshot-preview1", -] - [[package]] name = "getrandom" version = "0.2.2" @@ -452,7 +440,7 @@ checksum = "c9495705279e7140bf035dde1f6e750c162df8b625267cd52cc44e0b156732c8" dependencies = [ "cfg-if", "libc", - "wasi 0.10.2+wasi-snapshot-preview1", + "wasi", ] [[package]] @@ -634,30 +622,6 @@ dependencies = [ "rawpointer", ] -[[package]] -name = "ndarray-stats" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "22c95a780960082c5746f6bf0ab22d4a3b8cee72bf580acfe9f1e10bc5ea8152" -dependencies = [ - "indexmap", - "itertools 0.9.0", - "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" @@ -782,19 +746,6 @@ 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" @@ -802,19 +753,9 @@ 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", + "rand_chacha", + "rand_core", + "rand_hc", ] [[package]] @@ -824,16 +765,7 @@ 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", + "rand_core", ] [[package]] @@ -842,16 +774,7 @@ 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", + "getrandom", ] [[package]] @@ -860,7 +783,7 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3190ef7066a446f2e7f42e239d161e905420ccab01eb967c9eb27d21b2322a73" dependencies = [ - "rand_core 0.6.1", + "rand_core", ] [[package]] @@ -1190,12 +1113,6 @@ 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" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index 7e30348..de96d81 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -12,7 +12,6 @@ serde = { version = "1.0", features = ["derive"], default-features = false } criterion = "0.3" rand = "0.8" ndarray = "0.14" -ndarray-stats = "0.4" [[bench]] name = "micro" diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs index 5a42640..56b9149 100644 --- a/dsp/benches/micro.rs +++ b/dsp/benches/micro.rs @@ -50,7 +50,7 @@ fn pll_bench(c: &mut Criterion) { fn iir_int_bench(c: &mut Criterion) { let dut = iir_int::IIR::default(); - let mut xy = iir_int::IIRState::default(); + let mut xy = iir_int::Vec5::default(); c.bench_function("int_iir::IIR::update(s, x)", |b| { b.iter(|| dut.update(&mut xy, black_box(0x2832))) }); @@ -58,7 +58,7 @@ fn iir_int_bench(c: &mut Criterion) { fn iir_bench(c: &mut Criterion) { let dut = iir::IIR::default(); - let mut xy = iir::IIRState::default(); + let mut xy = iir::Vec5::default(); c.bench_function("int::IIR::update(s, x)", |b| { b.iter(|| dut.update(&mut xy, black_box(0.32241))) }); diff --git a/dsp/src/accu.rs b/dsp/src/accu.rs new file mode 100644 index 0000000..99369b2 --- /dev/null +++ b/dsp/src/accu.rs @@ -0,0 +1,21 @@ +#[derive(Copy, Clone, Default, PartialEq, Debug)] +pub struct Accu { + state: i32, + step: i32, +} + +impl Accu { + pub fn new(state: i32, step: i32) -> Self { + Self { state, step } + } +} + +impl Iterator for Accu { + type Item = i32; + #[inline] + fn next(&mut self) -> Option { + let s = self.state; + self.state = self.state.wrapping_add(self.step); + Some(s) + } +} diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs index 2bdb9ea..9829312 100644 --- a/dsp/src/complex.rs +++ b/dsp/src/complex.rs @@ -1,7 +1,6 @@ use super::{atan2, cossin}; -use serde::{Deserialize, Serialize}; -#[derive(Copy, Clone, Default, PartialEq, Debug, Deserialize, Serialize)] +#[derive(Copy, Clone, Default, PartialEq, Debug)] pub struct Complex(pub T, pub T); impl Complex { @@ -16,8 +15,9 @@ impl Complex { /// Complex::::from_angle(-1 << 30); // -pi/2 /// ``` #[inline(always)] - pub fn from_angle(angle: i32) -> Complex { - cossin(angle) + pub fn from_angle(angle: i32) -> Self { + let (c, s) = cossin(angle); + Self(c, s) } /// Return the absolute square (the squared magnitude). diff --git a/dsp/src/cossin.rs b/dsp/src/cossin.rs index f9cc42e..e5746a3 100644 --- a/dsp/src/cossin.rs +++ b/dsp/src/cossin.rs @@ -1,4 +1,3 @@ -use super::Complex; use core::f64::consts::PI; include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); @@ -11,10 +10,10 @@ include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); /// * `phase` - 32-bit phase. /// /// # Returns -/// The cos and sin values of the provided phase as a `Complex` -/// value. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error +/// The cos and sin values of the provided phase as a `(i32, i32)` +/// tuple. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error /// in each quadrature over 20 bit phase. -pub fn cossin(phase: i32) -> Complex { +pub fn cossin(phase: i32) -> (i32, i32) { // Phase bits excluding the three highes MSB const OCTANT_BITS: usize = 32 - 3; @@ -69,12 +68,13 @@ pub fn cossin(phase: i32) -> Complex { sin *= -1; } - Complex(cos, sin) + (cos, sin) } #[cfg(test)] mod tests { use super::*; + use crate::Complex; use core::f64::consts::PI; #[test] diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 50dd919..8d0dbe4 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -12,7 +12,7 @@ use core::f32; /// coefficients (b0, b1, b2) followd by the negated feed-back coefficients /// (-a1, -a2), all five normalized such that a0 = 1. #[derive(Copy, Clone, Default, Deserialize, Serialize)] -pub struct IIRState(pub [f32; 5]); +pub struct Vec5(pub [f32; 5]); /// IIR configuration. /// @@ -41,7 +41,7 @@ pub struct IIRState(pub [f32; 5]); /// implementation of transfer functions beyond bequadratic terms. #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIR { - pub ba: IIRState, + pub ba: Vec5, pub y_offset: f32, pub y_min: f32, pub y_max: f32, @@ -108,7 +108,7 @@ impl IIR { /// # Arguments /// * `xy` - Current filter state. /// * `x0` - New input. - pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 { + pub fn update(&self, xy: &mut Vec5, x0: f32) -> f32 { let n = self.ba.0.len(); debug_assert!(xy.0.len() == n); // `xy` contains x0 x1 y0 y1 y2 diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs index 64ab175..93626de 100644 --- a/dsp/src/iir_int.rs +++ b/dsp/src/iir_int.rs @@ -5,9 +5,9 @@ use serde::{Deserialize, Serialize}; /// This struct is used to hold the x/y input/output data vector or the b/a coefficient /// vector. #[derive(Copy, Clone, Default, Deserialize, Serialize)] -pub struct IIRState(pub [i32; 5]); +pub struct Vec5(pub [i32; 5]); -impl IIRState { +impl Vec5 { /// Lowpass biquad filter using cutoff and sampling frequencies. Taken from: /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html /// @@ -19,7 +19,7 @@ impl IIRState { /// /// # Returns /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1. - pub fn lowpass(f: f32, q: f32, k: f32) -> IIRState { + pub fn lowpass(f: f32, q: f32, k: f32) -> Self { // 3rd order Taylor approximation of sin and cos. let f = f * 2. * PI; let fsin = f - f * f * f / 6.; @@ -31,7 +31,7 @@ impl IIRState { let a1 = (2. * fcos / a0) as _; let a2 = ((alpha - 1.) / a0) as _; - IIRState([b0, 2 * b0, b0, a1, a2]) + Self([b0, 2 * b0, b0, a1, a2]) } } @@ -53,7 +53,7 @@ fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { /// Coefficient scaling fixed and optimized. #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIR { - pub ba: IIRState, + pub ba: Vec5, // pub y_offset: i32, // pub y_min: i32, // pub y_max: i32, @@ -70,7 +70,7 @@ impl IIR { /// # Arguments /// * `xy` - Current filter state. /// * `x0` - New input. - pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 { + pub fn update(&self, xy: &mut Vec5, x0: i32) -> i32 { let n = self.ba.0.len(); debug_assert!(xy.0.len() == n); // `xy` contains x0 x1 y0 y1 y2 @@ -92,11 +92,11 @@ impl IIR { #[cfg(test)] mod test { - use super::IIRState; + use super::Vec5; #[test] fn lowpass_gen() { - let ba = IIRState::lowpass(1e-3, 1. / 2f32.sqrt(), 2.); + let ba = Vec5::lowpass(1e-3, 1. / 2f32.sqrt(), 2.); println!("{:?}", ba.0); } } diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 191054a..74ebdbb 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -112,6 +112,7 @@ where .fold(y0, |y, xa| y + xa) } +pub mod accu; mod atan2; mod complex; mod cossin; @@ -122,6 +123,7 @@ pub mod pll; pub mod rpll; pub mod unwrap; +pub use accu::Accu; pub use atan2::atan2; pub use complex::Complex; pub use cossin::cossin; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index b75038e..fcbb4c9 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,176 +1,44 @@ -use super::{iir_int, Complex}; +use super::{ + iir_int::{Vec5, IIR}, + Complex, +}; use serde::{Deserialize, Serialize}; #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct Lockin { - iir: iir_int::IIR, - iir_state: [iir_int::IIRState; 2], + iir: IIR, + state: [Vec5; 2], } impl Lockin { - pub fn new(ba: &iir_int::IIRState) -> Self { - let mut iir = iir_int::IIR::default(); - iir.ba.0.copy_from_slice(&ba.0); - Lockin { - iir, - iir_state: [iir_int::IIRState::default(); 2], + /// Create a new Lockin with given IIR coefficients. + pub fn new(ba: Vec5) -> Self { + Self { + iir: IIR { + ba, + ..Default::default() + }, + state: [Vec5::default(); 2], } } - pub fn update(&mut self, signal: i32, phase: i32) -> Complex { + /// Update the lockin with a sample taken at a given phase. + pub fn update(&mut self, sample: i32, phase: i32) -> Complex { // Get the LO signal for demodulation. - let m = Complex::from_angle(phase); + let lo = Complex::from_angle(phase); // Mix with the LO signal, filter with the IIR lowpass, // return IQ (in-phase and quadrature) data. // Note: 32x32 -> 64 bit multiplications are pretty much free. Complex( self.iir.update( - &mut self.iir_state[0], - ((signal as i64 * m.0 as i64) >> 32) as _, + &mut self.state[0], + ((sample as i64 * lo.0 as i64) >> 32) as _, ), self.iir.update( - &mut self.iir_state[1], - ((signal as i64 * m.1 as i64) >> 32) as _, + &mut self.state[1], + ((sample as i64 * lo.1 as i64) >> 32) as _, ), ) } - - 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::{ - iir_int::IIRState, - lockin::Lockin, - rpll::RPLL, - 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 - } } diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 0006831..79a2b30 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -22,8 +22,8 @@ impl RPLL { /// /// Returns: /// Initialized RPLL instance. - pub fn new(dt2: u8) -> RPLL { - RPLL { + pub fn new(dt2: u8) -> Self { + Self { dt2, ..Default::default() } @@ -84,7 +84,6 @@ impl RPLL { mod test { use super::RPLL; use ndarray::prelude::*; - use ndarray_stats::QuantileExt; use rand::{prelude::*, rngs::StdRng}; use std::vec::Vec; @@ -108,7 +107,7 @@ mod test { impl Harness { fn default() -> Self { - Harness { + Self { rpll: RPLL::new(8), dt2: 8, shift_frequency: 9, @@ -254,7 +253,7 @@ mod test { h.shift_frequency = 10; h.shift_phase = 9; - h.measure(1 << 16, [5e-7, 3e-2, 3e-2, 2e-2]); + h.measure(1 << 16, [5e-7, 3e-2, 2e-5, 2e-2]); } #[test] diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs index 1757d7c..ef79998 100644 --- a/src/bin/dual-iir.rs +++ b/src/bin/dual-iir.rs @@ -34,9 +34,9 @@ const APP: () = { net_interface: hardware::Ethernet, // Format: iir_state[ch][cascade-no][coeff] - #[init([[iir::IIRState([0.; 5]); IIR_CASCADE_LENGTH]; 2])] - iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], - #[init([[iir::IIR { ba: iir::IIRState([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] + #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])] + iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR { ba: iir::Vec5([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], } diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index 95c79eb..3668b60 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin-external.rs @@ -16,7 +16,7 @@ use stabilizer::{ hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2, }; -use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL}; +use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL, Accu}; use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, }; @@ -38,9 +38,9 @@ const APP: () = { net_interface: hardware::Ethernet, // Format: iir_state[ch][cascade-no][coeff] - #[init([[iir::IIRState([0.; 5]); IIR_CASCADE_LENGTH]; 2])] - iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], - #[init([[iir::IIR { ba: iir::IIRState([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] + #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])] + iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR { ba: iir::Vec5([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, @@ -56,7 +56,7 @@ const APP: () = { 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 + iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose ); // Enable ADC/DAC events @@ -133,13 +133,15 @@ const APP: () = { let sample_phase = phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); - 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, - ) { + if let Some(output) = adc_samples[0] + .iter() + .zip(Accu::new(sample_phase, sample_frequency)) + // Convert to signed, MSB align the ADC sample. + .map(|(&sample, phase)| { + lockin.update((sample as i16 as i32) << 16, phase) + }) + .last() + { // Convert from IQ to power and phase. let mut power = output.abs_sqr() as _; let mut phase = output.arg() as _; diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index 8c41cfe..e4a2521 100644 --- a/src/bin/lockin-internal.rs +++ b/src/bin/lockin-internal.rs @@ -7,7 +7,7 @@ const DAC_SEQUENCE: [f32; 8] = [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707]; -use dsp::{iir_int, lockin::Lockin}; +use dsp::{iir_int, lockin::Lockin, Accu}; use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; use stabilizer::hardware; @@ -27,7 +27,7 @@ const APP: () = { let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); let lockin = Lockin::new( - &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose + iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose ); // Enable ADC/DAC events @@ -66,6 +66,7 @@ const APP: () = { /// TODO: Document #[task(binds=DMA1_STR4, resources=[adc1, dacs, lockin], priority=2)] fn process(c: process::Context) { + let lockin = c.resources.lockin; let adc_samples = c.resources.adc1.acquire_buffer(); let dac_samples = [ c.resources.dacs.0.acquire_buffer(), @@ -96,13 +97,15 @@ const APP: () = { let sample_phase = phase_offset .wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); - if let Some(output) = c.resources.lockin.feed( - adc_samples.iter().map(|&i| - // Convert to signed, MSB align the ADC sample. - (i as i16 as i32) << 16), - sample_phase, - sample_frequency, - ) { + if let Some(output) = adc_samples + .iter() + .zip(Accu::new(sample_phase, sample_frequency)) + // Convert to signed, MSB align the ADC sample. + .map(|(&sample, phase)| { + lockin.update((sample as i16 as i32) << 16, phase) + }) + .last() + { // Convert from IQ to power and phase. let _power = output.abs_sqr(); let phase = output.arg() >> 16;