lowpass: saturating math since it's free

dsp/src/lowpass.rs

@@ -14,19 +14,20 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
     /// Update the filter with a new sample.
     ///
     /// # Args
-    /// * `x`: Input data. Needs 1 bit headroom.
+    /// * `x`: Input data. Needs 1 bit headroom but will saturate cleanly beyond that.
-    /// * `k`: Log2 time constant, 0..=31.
+    /// * `k`: Log2 time constant, 1..=31.
     ///
     /// # Return
     /// Filtered output y.
     pub fn update(&mut self, x: i32, k: u8) -> i32 {
         debug_assert!(k & 31 == k);
+        debug_assert!((k - 1) & 31 == k - 1);
         // This is an unrolled and optimized first-order IIR loop
         // that works for all possible time constants.
         // Note T-DF-I and the zeros at Nyquist.
         let mut x = x;
         for y in self.y.iter_mut() {
-            let dy = (x - *y + (1 << (k - 1))) >> k;
+            let dy = x.saturating_sub(*y).saturating_add(1 << (k - 1)) >> k;
             *y += dy;
             x = *y - (dy >> 1);
         }

src/bin/lockin-external.rs

@@ -118,12 +118,13 @@ const APP: () = {
             .zip(Accu::new(sample_phase, sample_frequency))
             // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
             .map(|(&sample, phase)| {
-                let s = (sample as i16 as i32) << (15 + 1);
+                let s = (sample as i16 as i32) << 16;
                 lockin.update(s, phase, time_constant)
             })
             // Decimate
             .last()
-            .unwrap();
+            .unwrap()
+            * 2; // Full scale assuming the 2f component is gone.
 
         #[allow(dead_code)]
         enum Conf {

src/bin/lockin-internal.rs

@@ -101,12 +101,13 @@ const APP: () = {
             .zip(Accu::new(sample_phase, sample_frequency))
             // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
             .map(|(&sample, phase)| {
-                let s = (sample as i16 as i32) << (15 + 1);
+                let s = (sample as i16 as i32) << 16;
                 lockin.update(s, phase, time_constant)
             })
             // Decimate
             .last()
-            .unwrap();
+            .unwrap()
+            * 2; // Full scale assuming the 2f component is gone.
 
         for value in dac_samples[1].iter_mut() {
             *value = (output.arg() >> 16) as u16 ^ 0x8000;