diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index 16fd500..7e30348 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -15,7 +15,7 @@ ndarray = "0.14" ndarray-stats = "0.4" [[bench]] -name = "trig" +name = "micro" harness = false [features] diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs index b0f5016..5a42640 100644 --- a/dsp/benches/micro.rs +++ b/dsp/benches/micro.rs @@ -1,6 +1,8 @@ use core::f32::consts::PI; use criterion::{black_box, criterion_group, criterion_main, Criterion}; -use dsp::{atan2, cossin, pll::PLL, rpll::RPLL}; +use dsp::{atan2, cossin}; +use dsp::{iir, iir_int}; +use dsp::{pll::PLL, rpll::RPLL}; fn atan2_bench(c: &mut Criterion) { let xi = (10 << 16) as i32; @@ -46,6 +48,23 @@ 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(); + c.bench_function("int_iir::IIR::update(s, x)", |b| { + b.iter(|| dut.update(&mut xy, black_box(0x2832))) + }); +} + +fn iir_bench(c: &mut Criterion) { + let dut = iir::IIR::default(); + let mut xy = iir::IIRState::default(); + c.bench_function("int::IIR::update(s, x)", |b| { + b.iter(|| dut.update(&mut xy, black_box(0.32241))) + }); +} + criterion_group!(trig, atan2_bench, cossin_bench); criterion_group!(pll, rpll_bench, pll_bench); -criterion_main!(trig, pll); +criterion_group!(iir, iir_int_bench, iir_bench); +criterion_main!(trig, pll, iir); diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index dbec8d8..50dd919 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -11,7 +11,8 @@ use core::f32; /// To represent the IIR coefficients, this contains the feed-forward /// coefficients (b0, b1, b2) followd by the negated feed-back coefficients /// (-a1, -a2), all five normalized such that a0 = 1. -pub type IIRState = [f32; 5]; +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct IIRState(pub [f32; 5]); /// IIR configuration. /// @@ -38,7 +39,7 @@ pub type IIRState = [f32; 5]; /// Therefore it can trivially implement bump-less transfer. /// * Cascading multiple IIR filters allows stable and robust /// implementation of transfer functions beyond bequadratic terms. -#[derive(Copy, Clone, Deserialize, Serialize)] +#[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIR { pub ba: IIRState, pub y_offset: f32, @@ -74,13 +75,13 @@ impl IIR { } (a1, b0, b1) }; - self.ba.copy_from_slice(&[b0, b1, 0., a1, 0.]); + self.ba.0.copy_from_slice(&[b0, b1, 0., a1, 0.]); Ok(()) } /// Compute the overall (DC feed-forward) gain. pub fn get_k(&self) -> f32 { - self.ba[..3].iter().sum() + self.ba.0[..3].iter().sum() } /// Compute input-referred (`x`) offset from output (`y`) offset. @@ -108,21 +109,21 @@ impl IIR { /// * `xy` - Current filter state. /// * `x0` - New input. pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 { - let n = self.ba.len(); - debug_assert!(xy.len() == n); + let n = self.ba.0.len(); + debug_assert!(xy.0.len() == n); // `xy` contains x0 x1 y0 y1 y2 // Increment time x1 x2 y1 y2 y3 // Shift x1 x1 x2 y1 y2 // This unrolls better than xy.rotate_right(1) - xy.copy_within(0..n - 1, 1); + xy.0.copy_within(0..n - 1, 1); // Store x0 x0 x1 x2 y1 y2 - xy[0] = x0; + xy.0[0] = x0; // Compute y0 by multiply-accumulate - let y0 = macc(self.y_offset, xy, &self.ba); + let y0 = macc(self.y_offset, &xy.0, &self.ba.0); // Limit y0 let y0 = max(self.y_min, min(self.y_max, y0)); // Store y0 x0 x1 y0 y1 y2 - xy[n / 2] = y0; + xy.0[n / 2] = y0; y0 } } diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs index fd8f5e4..1757d7c 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([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] + #[init([[iir::IIRState([0.; 5]); IIR_CASCADE_LENGTH]; 2])] iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], - #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; 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])] iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], } @@ -159,10 +159,10 @@ const APP: () = { let state = c.resources.iir_state.lock(|iir_state| server::Status { t: time, - x0: iir_state[0][0][0], - y0: iir_state[0][0][2], - x1: iir_state[1][0][0], - y1: iir_state[1][0][2], + x0: iir_state[0][0].0[0], + y0: iir_state[0][0].0[2], + x1: iir_state[1][0].0[0], + y1: iir_state[1][0].0[2], }); Ok::(state) @@ -171,10 +171,10 @@ const APP: () = { "stabilizer/iir_b/state": (|| { let state = c.resources.iir_state.lock(|iir_state| server::Status { t: time, - x0: iir_state[0][IIR_CASCADE_LENGTH-1][0], - y0: iir_state[0][IIR_CASCADE_LENGTH-1][2], - x1: iir_state[1][IIR_CASCADE_LENGTH-1][0], - y1: iir_state[1][IIR_CASCADE_LENGTH-1][2], + x0: iir_state[0][IIR_CASCADE_LENGTH-1].0[0], + y0: iir_state[0][IIR_CASCADE_LENGTH-1].0[2], + x1: iir_state[1][IIR_CASCADE_LENGTH-1].0[0], + y1: iir_state[1][IIR_CASCADE_LENGTH-1].0[2], }); Ok::(state) diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index f9e2f58..95c79eb 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -38,9 +38,9 @@ const APP: () = { net_interface: hardware::Ethernet, // Format: iir_state[ch][cascade-no][coeff] - #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] + #[init([[iir::IIRState([0.; 5]); IIR_CASCADE_LENGTH]; 2])] iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], - #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; 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])] iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], timestamper: InputStamper, @@ -216,10 +216,10 @@ const APP: () = { let state = c.resources.iir_state.lock(|iir_state| server::Status { t: time, - x0: iir_state[0][0][0], - y0: iir_state[0][0][2], - x1: iir_state[1][0][0], - y1: iir_state[1][0][2], + x0: iir_state[0][0].0[0], + y0: iir_state[0][0].0[2], + x1: iir_state[1][0].0[0], + y1: iir_state[1][0].0[2], }); Ok::(state) @@ -228,10 +228,10 @@ const APP: () = { "stabilizer/iir_b/state": (|| { let state = c.resources.iir_state.lock(|iir_state| server::Status { t: time, - x0: iir_state[0][IIR_CASCADE_LENGTH-1][0], - y0: iir_state[0][IIR_CASCADE_LENGTH-1][2], - x1: iir_state[1][IIR_CASCADE_LENGTH-1][0], - y1: iir_state[1][IIR_CASCADE_LENGTH-1][2], + x0: iir_state[0][IIR_CASCADE_LENGTH-1].0[0], + y0: iir_state[0][IIR_CASCADE_LENGTH-1].0[2], + x1: iir_state[1][IIR_CASCADE_LENGTH-1].0[0], + y1: iir_state[1][IIR_CASCADE_LENGTH-1].0[2], }); Ok::(state)