diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 6abd277..d78c01b 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -4,12 +4,12 @@ use generic_array::typenum::U3; #[derive(Clone, Default)] pub struct Lockin { state: [Lowpass; 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()], diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index 50ca604..e3a1353 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -15,27 +15,19 @@ impl> Lowpass { /// /// # 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; } diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index ba576e9..fad27ec 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin-external.rs @@ -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(); diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index e25c9fb..287815a 100644 --- a/src/bin/lockin-internal.rs +++ b/src/bin/lockin-internal.rs @@ -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();