GitHub
commit
6e1444f070
|
|
@ -15,7 +15,7 @@ ndarray = "0.14"
|
|||
ndarray-stats = "0.4"
|
||||
|
||||
[[bench]]
|
||||
name = "trig"
|
||||
name = "micro"
|
||||
harness = false
|
||||
|
||||
[features]
|
||||
|
|
|
|||
|
|
@ -0,0 +1,70 @@
|
|||
use core::f32::consts::PI;
|
||||
use criterion::{black_box, criterion_group, criterion_main, Criterion};
|
||||
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;
|
||||
let xf = xi as f32 / i32::MAX as f32;
|
||||
|
||||
let yi = (-26_328 << 16) as i32;
|
||||
let yf = yi as f32 / i32::MAX as f32;
|
||||
|
||||
c.bench_function("atan2(y, x)", |b| {
|
||||
b.iter(|| atan2(black_box(yi), black_box(xi)))
|
||||
});
|
||||
c.bench_function("y.atan2(x)", |b| {
|
||||
b.iter(|| black_box(yf).atan2(black_box(xf)))
|
||||
});
|
||||
}
|
||||
|
||||
fn cossin_bench(c: &mut Criterion) {
|
||||
let zi = -0x7304_2531_i32;
|
||||
let zf = zi as f32 / i32::MAX as f32 * PI;
|
||||
c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi))));
|
||||
c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos()));
|
||||
}
|
||||
|
||||
fn rpll_bench(c: &mut Criterion) {
|
||||
let mut dut = RPLL::new(8);
|
||||
c.bench_function("RPLL::update(Some(t), 21, 20)", |b| {
|
||||
b.iter(|| dut.update(black_box(Some(0x241)), 21, 20))
|
||||
});
|
||||
c.bench_function("RPLL::update(Some(t), sf, sp)", |b| {
|
||||
b.iter(|| {
|
||||
dut.update(black_box(Some(0x241)), black_box(21), black_box(20))
|
||||
})
|
||||
});
|
||||
}
|
||||
|
||||
fn pll_bench(c: &mut Criterion) {
|
||||
let mut dut = PLL::default();
|
||||
c.bench_function("PLL::update(t, 12, 11)", |b| {
|
||||
b.iter(|| dut.update(black_box(0x1234), 12, 1))
|
||||
});
|
||||
c.bench_function("PLL::update(t, sf, sp)", |b| {
|
||||
b.iter(|| dut.update(black_box(0x241), black_box(21), black_box(20)))
|
||||
});
|
||||
}
|
||||
|
||||
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_group!(iir, iir_int_bench, iir_bench);
|
||||
criterion_main!(trig, pll, iir);
|
||||
|
|
@ -1,28 +0,0 @@
|
|||
use core::f32::consts::PI;
|
||||
use criterion::{black_box, criterion_group, criterion_main, Criterion};
|
||||
use dsp::{atan2, cossin};
|
||||
|
||||
fn atan2_bench(c: &mut Criterion) {
|
||||
let xi = (10 << 16) as i32;
|
||||
let xf = xi as f32 / i32::MAX as f32;
|
||||
|
||||
let yi = (-26_328 << 16) as i32;
|
||||
let yf = yi as f32 / i32::MAX as f32;
|
||||
|
||||
c.bench_function("atan2(y, x)", |b| {
|
||||
b.iter(|| atan2(black_box(yi), black_box(xi)))
|
||||
});
|
||||
c.bench_function("y.atan2(x)", |b| {
|
||||
b.iter(|| black_box(yf).atan2(black_box(xf)))
|
||||
});
|
||||
}
|
||||
|
||||
fn cossin_bench(c: &mut Criterion) {
|
||||
let zi = -0x7304_2531_i32;
|
||||
let zf = zi as f32 / i32::MAX as f32 * PI;
|
||||
c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi))));
|
||||
c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos()));
|
||||
}
|
||||
|
||||
criterion_group!(benches, atan2_bench, cossin_bench);
|
||||
criterion_main!(benches);
|
||||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue