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

This commit is contained in:
Robert Jördens 2021-02-01 12:22:50 +01:00
parent 0fd4b167b4
commit 2c60103696
8 changed files with 53 additions and 34 deletions

21
dsp/src/accu.rs Normal file
View File

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

View File

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

View File

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

View File

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

View File

@ -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<Complex<i32>> {
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()
}
}

View File

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

View File

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

View File

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