dsp: accu: add, iir: rename IIRState to Vec5

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<i32> {
+        let s = self.state;
+        self.state = self.state.wrapping_add(self.step);
+        Some(s)
+    }
+}

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

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);
     }
 }

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;

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: IIR,
-    iir_state: [iir_int::IIRState; 2],
+    state: [Vec5; 2],
 }
 
 impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
-    pub fn new(ba: &iir_int::IIRState) -> Self {
+    pub fn new(ba: Vec5) -> Self {
-        let mut iir = iir_int::IIR::default();
+        let mut iir = IIR::default();
-        iir.ba.0.copy_from_slice(&ba.0);
+        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<Complex<i32>> {
-        let mut phase = phase;
-
         signal
             .into_iter()
-            .map(|sample| {
+            .zip(Accu::new(phase, frequency))
-                phase = phase.wrapping_add(frequency);
+            .map(|(sample, phase)| self.update(sample, phase))
-                self.update(sample, phase)
-            })
             .last()
     }
 }

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])]
+        #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
-        iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2],
+        iir_state: [[iir::Vec5; 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::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],
     }
 

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])]
+        #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
-        iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2],
+        iir_state: [[iir::Vec5; 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::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

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