move lowpass gain outside lowpass/lockin

dsp/src/lockin.rs

@@ -8,8 +8,7 @@ pub struct Lockin {
 
 impl Lockin {
     /// Update the lockin with a sample taken at a given phase.
-    /// The lowpass has a gain of `1 << k`.
-    pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex<i32> {
+    pub fn update(&mut self, sample: i32, phase: i32, k: u8) -> Complex<i32> {
         // Get the LO signal for demodulation and mix the sample;
         let mix = Complex::from_angle(phase).mul_scaled(sample);
 

dsp/src/lowpass.rs

@@ -14,17 +14,17 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
     /// Update the filter with a new sample.
     ///
     /// # Args
-    /// * `x`: Input data, needs `k` bits headroom.
-    /// * `k`: Log2 time constant, 0..31.
+    /// * `x`: Input data. Needs 1 bit headroom.
+    /// * `k`: Log2 time constant, 0..=31.
     ///
     /// # Return
-    /// Filtered output y, with gain of `1 << k`.
+    /// Filtered output y.
     pub fn update(&mut self, x: i32, k: u8) -> i32 {
         debug_assert!(k & 31 == k);
         // This is an unrolled and optimized first-order IIR loop
         // that works for all possible time constants.
-        // Note DF-II and the zeros at Nyquist.
-        let mut x = x << k;
+        // 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;
             *y += dy;

src/bin/lockin-external.rs

@@ -6,7 +6,7 @@ use stm32h7xx_hal as hal;
 
 use stabilizer::{hardware, hardware::design_parameters};
 
-use dsp::{Accu, FastInt, Lockin, RPLL};
+use dsp::{Accu, Complex, FastInt, Lockin, RPLL};
 use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
@@ -112,15 +112,18 @@ const APP: () = {
         let sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 
-        let output = adc_samples[0]
+        let output: Complex<i32> = adc_samples[0]
             .iter()
+            // Zip in the LO phase.
             .zip(Accu::new(sample_phase, sample_frequency))
-            // Convert to signed, MSB align the ADC sample.
+            // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
             .map(|(&sample, phase)| {
-                lockin.update(sample as i16, phase, time_constant)
+                let s = (sample as i16 as i32)
+                    << (15 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2 + 1);
+                lockin.update(s, phase, time_constant)
             })
-            .last()
-            .unwrap();
+            // Decimate
+            .sum();
 
         let conf = "frequency_discriminator";
         let output = match conf {

src/bin/lockin-internal.rs

@@ -2,7 +2,7 @@
 #![no_std]
 #![no_main]
 
-use dsp::{Accu, FastInt, Lockin};
+use dsp::{Accu, Complex, FastInt, Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 use stabilizer::{hardware, hardware::design_parameters};
 
@@ -95,17 +95,18 @@ const APP: () = {
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        let output = adc_samples
+        let output: Complex<i32> = adc_samples
             .iter()
             // Zip in the LO phase.
             .zip(Accu::new(sample_phase, sample_frequency))
             // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
             .map(|(&sample, phase)| {
-                lockin.update(sample as i16, phase, time_constant)
+                let s = (sample as i16 as i32)
+                    << (15 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2 + 1);
+                lockin.update(s, phase, time_constant)
             })
             // Decimate
-            .last()
-            .unwrap();
+            .sum();
 
         for value in dac_samples[1].iter_mut() {
             *value = (output.arg() >> 16) as u16 ^ 0x8000;