kai
/
pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

dsp: align iir and iir_int, add iir micro benches

master
Robert Jördens 2021-01-31 19:12:24 +01:00
parent 8408bc5811
commit 47089c267c
5 changed files with 53 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
dsp/Cargo.toml
View File

@ -15,7 +15,7 @@ ndarray = "0.14"
ndarray-stats = "0.4"
[[bench]]
name = "trig"
name = "micro"
harness = false
[features]

23
dsp/benches/micro.rs
View File

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

21
dsp/src/iir.rs
View File

@ -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
}
}

20
src/bin/dual-iir.rs
View File

@ -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::<server::Status, ()>(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::<server::Status, ()>(state)

20
src/bin/lockin.rs
View File

@ -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::<server::Status, ()>(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::<server::Status, ()>(state)