diff --git a/dsp/src/accu.rs b/dsp/src/accu.rs new file mode 100644 index 0000000..6560896 --- /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) -> Accu { + Accu { 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/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..2edca3a 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) -> Vec5 { // 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]) + Vec5([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 0f6b78f..19f13e2 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,20 +1,20 @@ -use super::{iir_int, Complex}; +use super::{iir_int::{IIR, Vec5}, Accu, 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 { /// Create a new Lockin with given IIR coefficients. - pub fn new(ba: &iir_int::IIRState) -> Self { - let mut iir = iir_int::IIR::default(); - iir.ba.0.copy_from_slice(&ba.0); + pub fn new(ba: Vec5) -> Self { + let mut iir = IIR::default(); + iir.ba = ba; Lockin { iir, - iir_state: [iir_int::IIRState::default(); 2], + state: [Vec5::default(); 2], } } @@ -28,11 +28,11 @@ impl Lockin { // Note: 32x32 -> 64 bit multiplications are pretty much free. Complex( self.iir.update( - &mut self.iir_state[0], + &mut self.state[0], ((sample as i64 * lo.0 as i64) >> 32) as _, ), self.iir.update( - &mut self.iir_state[1], + &mut self.state[1], ((sample as i64 * lo.1 as i64) >> 32) as _, ), ) @@ -47,14 +47,10 @@ impl Lockin { phase: i32, frequency: i32, ) -> Option> { - let mut phase = phase; - signal .into_iter() - .map(|sample| { - phase = phase.wrapping_add(frequency); - self.update(sample, phase) - }) + .zip(Accu::new(phase, frequency)) + .map(|(sample, phase)| self.update(sample, phase)) .last() } } 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..d337662 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin-external.rs @@ -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 diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index 8c41cfe..f72c3ca 100644 --- a/src/bin/lockin-internal.rs +++ b/src/bin/lockin-internal.rs @@ -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