diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index 24832a8..39abffb 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -15,19 +15,20 @@ impl> Lowpass { /// /// # Args /// * `x`: Input data - /// * `k`: Log2 time constant + /// * `k`: Log2 time constant, 1..32 /// /// # Return /// Filtered output y pub fn update(&mut self, x: i32, k: u8) -> i32 { + debug_assert!((1..32).contains(&k)); // 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). + // Note the zero(s) at Nyquist. let mut x0 = x; let mut x1 = self.xy[0]; self.xy[0] = x0; for y1 in self.xy[1..].iter_mut() { - x0 = *y1 + (((x0 >> 1) + (x1 >> 1) - *y1 + (1 << (k - 1))) >> k); + x0 = *y1 + (((x0 >> 2) + (x1 >> 2) - (*y1 >> 1) + (1 << (k - 1))) >> k); x1 = *y1; *y1 = x0; }