Merge pull request #266 from quartiq/rj/lowpass

new lowpass

Cargo.lock

@@ -353,6 +353,7 @@ name = "dsp"
 version = "0.1.0"
 dependencies = [
  "criterion",
+ "generic-array 0.14.4",
  "libm",
  "ndarray",
  "rand",

dsp/Cargo.toml

@@ -7,6 +7,7 @@ edition = "2018"
 [dependencies]
 libm = "0.2.1"
 serde = { version = "1.0", features = ["derive"], default-features = false }
+generic-array = "0.14"
 
 [dev-dependencies]
 criterion = "0.3"

dsp/src/lib.rs

@@ -87,6 +87,7 @@ mod cossin;
 pub mod iir;
 pub mod iir_int;
 pub mod lockin;
+pub mod lowpass;
 pub mod pll;
 pub mod rpll;
 pub mod unwrap;

dsp/src/lockin.rs

@@ -1,21 +1,19 @@
-use super::{
+use super::{lowpass::Lowpass, Complex};
-    iir_int::{Vec5, IIR},
+use generic_array::typenum::U3;
-    Complex,
-};
-use serde::{Deserialize, Serialize};
 
-#[derive(Copy, Clone, Default, Deserialize, Serialize)]
+#[derive(Clone, Default)]
 pub struct Lockin {
-    iir: IIR,
+    state: [Lowpass<U3>; 2],
-    state: [Vec5; 2],
+    k: u8,
 }
 
 impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
-    pub fn new(ba: Vec5) -> Self {
+    pub fn new(k: u8) -> Self {
+        let lp = Lowpass::default();
         Self {
-            iir: IIR { ba },
+            state: [lp.clone(), lp],
-            state: [Vec5::default(); 2],
+            k,
         }
     }
 
@@ -28,14 +26,10 @@ impl Lockin {
         // return IQ (in-phase and quadrature) data.
         // Note: 32x32 -> 64 bit multiplications are pretty much free.
         Complex(
-            self.iir.update(
+            self.state[0]
-                &mut self.state[0],
+                .update(((sample as i64 * lo.0 as i64) >> 32) as _, self.k),
-                ((sample as i64 * lo.0 as i64) >> 32) as _,
+            self.state[1]
-            ),
+                .update(((sample as i64 * lo.1 as i64) >> 32) as _, self.k),
-            self.iir.update(
-                &mut self.state[1],
-                ((sample as i64 * lo.1 as i64) >> 32) as _,
-            ),
         )
     }
 }

dsp/src/lowpass.rs

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

src/bin/lockin-external.rs

@@ -6,7 +6,7 @@ use stm32h7xx_hal as hal;
 
 use stabilizer::{hardware, hardware::design_parameters};
 
-use dsp::{iir_int, lockin::Lockin, rpll::RPLL, Accu};
+use dsp::{lockin::Lockin, rpll::RPLL, Accu};
 use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
@@ -34,9 +34,7 @@ const APP: () = {
                 + design_parameters::SAMPLE_BUFFER_SIZE_LOG2,
         );
 
-        let lockin = Lockin::new(
+        let lockin = Lockin::new(10);
-            iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose
-        );
 
         // Enable ADC/DAC events
         stabilizer.adcs.0.start();

src/bin/lockin-internal.rs

@@ -2,7 +2,7 @@
 #![no_std]
 #![no_main]
 
-use dsp::{iir_int, lockin::Lockin, Accu};
+use dsp::{lockin::Lockin, Accu};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 use stabilizer::{hardware, hardware::design_parameters};
 
@@ -17,7 +17,7 @@ const DAC_SEQUENCE: [i16; design_parameters::SAMPLE_BUFFER_SIZE] =
 const APP: () = {
     struct Resources {
         afes: (AFE0, AFE1),
-        adc1: Adc1Input,
+        adc: Adc1Input,
         dacs: (Dac0Output, Dac1Output),
 
         lockin: Lockin,
@@ -28,9 +28,7 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let lockin = Lockin::new(
+        let lockin = Lockin::new(10); // TODO: expose
-            iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose
-        );
 
         // Enable ADC/DAC events
         stabilizer.adcs.1.start();
@@ -43,7 +41,7 @@ const APP: () = {
         init::LateResources {
             lockin,
             afes: stabilizer.afes,
-            adc1: stabilizer.adcs.1,
+            adc: stabilizer.adcs.1,
             dacs: stabilizer.dacs,
         }
     }
@@ -66,10 +64,10 @@ const APP: () = {
     /// the same time bounds, meeting one also means the other is also met.
     ///
     /// TODO: Document
-    #[task(binds=DMA1_STR4, resources=[adc1, dacs, lockin], priority=2)]
+    #[task(binds=DMA1_STR4, resources=[adc, dacs, lockin], priority=2)]
     fn process(c: process::Context) {
         let lockin = c.resources.lockin;
-        let adc_samples = c.resources.adc1.acquire_buffer();
+        let adc_samples = c.resources.adc.acquire_buffer();
         let dac_samples = [
             c.resources.dacs.0.acquire_buffer(),
             c.resources.dacs.1.acquire_buffer(),