From 07b7751dbc947530ae33bd0556e43788c99293cf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Thu, 18 Feb 2021 14:06:01 +0100
Subject: [PATCH] move lowpass gain outside lowpass/lockin

---
 dsp/src/lockin.rs          |  3 +--
 dsp/src/lowpass.rs         | 10 +++++-----
 src/bin/lockin-external.rs | 15 +++++++++------
 src/bin/lockin-internal.rs | 11 ++++++-----
 4 files changed, 21 insertions(+), 18 deletions(-)

diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index d3ebc73..f0a2767 100644
--- a/dsp/src/lockin.rs
+++ b/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);
 
diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs
index 91fae2f..6ed23e0 100644
--- a/dsp/src/lowpass.rs
+++ b/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;
diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index 15b88c7..b3b9d4f 100644
--- a/src/bin/lockin-external.rs
+++ b/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 {
diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs
index bba0569..6913f0a 100644
--- a/src/bin/lockin-internal.rs
+++ b/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;