lowpass: reimplement better

dsp/src/lockin.rs

@@ -1,35 +1,30 @@
 use super::{lowpass::Lowpass, Complex};
-use generic_array::typenum::U3;
+use generic_array::typenum::U4;
 
 #[derive(Clone, Default)]
 pub struct Lockin {
-    state: [Lowpass<U3>; 2],
+    state: [Lowpass<U4>; 2],
-    k: u8,
 }
 
 impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
-    pub fn new(k: u8) -> Self {
+    pub fn new() -> Self {
         let lp = Lowpass::default();
         Self {
             state: [lp.clone(), lp],
-            k,
         }
     }
 
     /// Update the lockin with a sample taken at a given phase.
-    pub fn update(&mut self, sample: i32, phase: i32) -> Complex<i32> {
+    pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex<i32> {
         // Get the LO signal for demodulation.
         let lo = Complex::from_angle(phase);
 
         // Mix with the LO signal, filter with the IIR lowpass,
         // return IQ (in-phase and quadrature) data.
-        // Note: 32x32 -> 64 bit multiplications are pretty much free.
         Complex(
-            self.state[0]
+            self.state[0].update((sample as i32 * (lo.0 >> 16)) >> 16, k),
-                .update(((sample as i64 * lo.0 as i64) >> 32) as _, self.k),
+            self.state[1].update((sample as i32 * (lo.1 >> 16)) >> 16, k),
-            self.state[1]
-                .update(((sample as i64 * lo.1 as i64) >> 32) as _, self.k),
         )
     }
 }

dsp/src/lowpass.rs

@@ -1,38 +1,35 @@
 use generic_array::{ArrayLength, GenericArray};
 
 /// Arbitrary order, high dynamic range, wide coefficient range,
-/// lowpass filter implementation.
+/// lowpass filter implementation. DC gain is 1.
 ///
-/// Type argument N is the filter order + 1.
+/// Type argument N is the filter order.
 #[derive(Clone, Default)]
 pub struct Lowpass<N: ArrayLength<i32>> {
     // IIR state storage
-    xy: GenericArray<i32, N>,
+    y: GenericArray<i32, N>,
 }
 
 impl<N: ArrayLength<i32>> Lowpass<N> {
     /// Update the filter with a new sample.
     ///
     /// # Args
-    /// * `x`: Input data
+    /// * `x`: Input data, needs k bits headroom
-    /// * `k`: Log2 time constant, 1..32
+    /// * `k`: Log2 time constant, 0..31
     ///
     /// # Return
     /// Filtered output y
     pub fn update(&mut self, x: i32, k: u8) -> i32 {
-        debug_assert!((1..32).contains(&k));
+        debug_assert!(k & 31 == k);
         // This is an unrolled and optimized first-order IIR loop
         // that works for all possible time constants.
-        // Note the zero(s) at Nyquist.
+        // Note DF-II and the zero(s) at Nyquist.
-        let mut x0 = x;
+        let mut x = x;
-        let mut x1 = self.xy[0];
+        for y in self.y.iter_mut() {
-        self.xy[0] = x0;
+            let dy = x - (*y >> k);
-        for y1 in self.xy[1..].iter_mut() {
+            *y += dy;
-            x0 = *y1
+            x = (*y - (dy >> 1)) >> k;
-                + (((x0 >> 2) + (x1 >> 2) - (*y1 >> 1) + (1 << (k - 1))) >> k);
-            x1 = *y1;
-            *y1 = x0;
         }
-        x0
+        x
     }
 }

src/bin/lockin-external.rs

@@ -34,7 +34,7 @@ const APP: () = {
                 + design_parameters::SAMPLE_BUFFER_SIZE_LOG2,
         );
 
-        let lockin = Lockin::new(10);
+        let lockin = Lockin::new();
 
         // Enable ADC/DAC events
         stabilizer.adcs.0.start();
@@ -102,6 +102,9 @@ const APP: () = {
         // Demodulation LO phase offset
         let phase_offset: i32 = 0; // TODO: expose
 
+        // Log2 lowpass time constant
+        let time_constant: u8 = 8; // TODO: expose
+
         let sample_frequency = ((pll_frequency
             // .wrapping_add(1 << design_parameters::SAMPLE_BUFFER_SIZE_LOG2 - 1)  // half-up rounding bias
             >> design_parameters::SAMPLE_BUFFER_SIZE_LOG2)
@@ -115,7 +118,7 @@ const APP: () = {
             .zip(Accu::new(sample_phase, sample_frequency))
             // Convert to signed, MSB align the ADC sample.
             .map(|(&sample, phase)| {
-                lockin.update((sample as i16 as i32) << 16, phase)
+                lockin.update(sample as i16, phase, time_constant)
             })
             .last()
             .unwrap();
@@ -125,7 +128,7 @@ const APP: () = {
             // Convert from IQ to power and phase.
             "power_phase" => [output.abs_sqr(), output.arg()],
             "frequency_discriminator" => [pll_frequency as i32, output.arg()],
-            _ => [output.0, output.1],
+            _ => [output.0 << 16, output.1 << 16],
         };
 
         // Convert to DAC data.

src/bin/lockin-internal.rs

@@ -28,7 +28,7 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let lockin = Lockin::new(10); // TODO: expose
+        let lockin = Lockin::new();
 
         // Enable ADC/DAC events
         stabilizer.adcs.1.start();
@@ -85,10 +85,14 @@ const APP: () = {
             1i32 << (32 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2);
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
-        let harmonic: i32 = -1;
+        let harmonic: i32 = -1; // TODO: expose
 
         // Demodulation LO phase offset
-        let phase_offset: i32 = (0.25 * i32::MAX as f32) as i32;
+        let phase_offset: i32 = (0.25 * i32::MAX as f32) as i32; // TODO: expose
+
+        // Log2 lowpass time constant.
+        let time_constant: u8 = 8;
+
         let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
@@ -99,24 +103,14 @@ 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)| {
-                lockin.update((sample as i16 as i32) << 16, phase)
+                lockin.update(sample as i16, phase, time_constant)
             })
             // Decimate
             .last()
             .unwrap();
 
-        // convert i/q to power/phase,
-        let power_phase = true; // TODO: expose
-
-        let output = if power_phase {
-            // Convert from IQ to power and phase.
-            [output.abs_sqr(), output.arg()]
-        } else {
-            [output.0, output.1]
-        };
-
         for value in dac_samples[1].iter_mut() {
-            *value = (output[1] >> 16) as u16 ^ 0x8000;
+            *value = (output.arg() >> 16) as u16 ^ 0x8000;
         }
     }