lowpass: i32, no multiplies

This commit is contained in:
Robert Jördens 2021-02-10 09:46:49 +01:00
parent 208ba8379a
commit 30c2c2aac2
4 changed files with 12 additions and 20 deletions

View File

@ -4,12 +4,12 @@ use generic_array::typenum::U3;
#[derive(Clone, Default)]
pub struct Lockin {
state: [Lowpass<U3>; 2],
k: u32,
k: u8,
}
impl Lockin {
/// Create a new Lockin with given IIR coefficients.
pub fn new(k: u32) -> Self {
pub fn new(k: u8) -> Self {
let lp = Lowpass::default();
Self {
state: [lp.clone(), lp.clone()],

View File

@ -15,27 +15,19 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
///
/// # Args
/// * `x`: Input data
/// * `k`: Cutoff, `u32::MAX` being Nyquist
/// * `k`: Log2 time constant
///
/// # Returns
/// # Return
/// Filtered output y
pub fn update(&mut self, x: i32, k: u32) -> i32 {
let mut x1 = self.xy[0];
self.xy[0] = x;
let mut x0 = x;
pub fn update(&mut self, x: i32, k: u8) -> i32 {
// This is an unrolled and optimized first-order IIR loop
// that works for all possible time constants.
// Note the zero(s) at Nyquist and the benign overflow (DF-I).
let mut x0 = x;
let mut x1 = self.xy[0];
self.xy[0] = x;
for y1 in self.xy[1..].iter_mut() {
// Optimized first order lowpass expression
// Note the zero at Nyquist
let mut y0 =
((x0 >> 1) as i64 + (x1 >> 1) as i64 - *y1 as i64) * k as i64;
y0 += (*y1 as i64) << 32;
y0 += 1i64 << 31; // Half-up rounding bias
// Store and advance
x0 = (y0 >> 32) as i32;
x0 = *y1 + (((x0 >> 1) + (x1 >> 1) - *y1 + (1 << k - 1)) >> k);
x1 = *y1;
*y1 = x0;
}

View File

@ -34,7 +34,7 @@ const APP: () = {
+ design_parameters::SAMPLE_BUFFER_SIZE_LOG2,
);
let lockin = Lockin::new(1 << 22);
let lockin = Lockin::new(10);
// Enable ADC/DAC events
stabilizer.adcs.0.start();

View File

@ -28,7 +28,7 @@ const APP: () = {
// Configure the microcontroller
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
let lockin = Lockin::new(1 << 22); // TODO: expose
let lockin = Lockin::new(10); // TODO: expose
// Enable ADC/DAC events
stabilizer.adcs.1.start();