From a6d4099ed3f77aae7b1ccda321856329b5e71887 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Fri, 12 Feb 2021 11:06:59 +0100
Subject: [PATCH] lowpass: expose natural gain, add bias

---
 dsp/src/lowpass.rs         | 12 ++++++------
 src/bin/lockin-external.rs |  4 ++--
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs
index c9d3ef6..91fae2f 100644
--- a/dsp/src/lowpass.rs
+++ b/dsp/src/lowpass.rs
@@ -14,21 +14,21 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
     /// Update the filter with a new sample.
     ///
     /// # Args
-    /// * `x`: Input data
-    /// * `k`: Log2 time constant, 0..31
+    /// * `x`: Input data, needs `k` bits headroom.
+    /// * `k`: Log2 time constant, 0..31.
     ///
     /// # Return
-    /// Filtered output y, needs `k` bits headroom
+    /// Filtered output y, with gain of `1 << k`.
     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;
+        let mut x = x << k;
         for y in self.y.iter_mut() {
-            let dy = x - (*y >> k);
+            let dy = (x - *y + (1 << (k - 1))) >> k;
             *y += dy;
-            x = (*y - (dy >> 1)) >> k;
+            x = *y - (dy >> 1);
         }
         x
     }
diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs
index cbda83c..c0e6cae 100644
--- a/src/bin/lockin-external.rs
+++ b/src/bin/lockin-external.rs
@@ -103,7 +103,7 @@ const APP: () = {
         let phase_offset: i32 = 0; // TODO: expose
 
         // Log2 lowpass time constant
-        let time_constant: u8 = 8; // TODO: expose
+        let time_constant: u8 = 6; // TODO: expose
 
         let sample_frequency = ((pll_frequency
             // .wrapping_add(1 << design_parameters::SAMPLE_BUFFER_SIZE_LOG2 - 1)  // half-up rounding bias
@@ -128,7 +128,7 @@ const APP: () = {
             // Convert from IQ to power and phase.
             "power_phase" => [(output.log2() << 24) as _, output.arg()],
             "frequency_discriminator" => [pll_frequency as _, output.arg()],
-            _ => [output.0 << 16, output.1 << 16],
+            _ => [output.0, output.1],
         };
 
         // Convert to DAC data.