From 208ba8379aff34faee818135959728308ebaaab3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 9 Feb 2021 18:30:50 +0100 Subject: [PATCH 1/6] dsp, lockin: use cascaded 1st order lowpasses --- Cargo.lock | 1 + dsp/Cargo.toml | 1 + dsp/src/lib.rs | 1 + dsp/src/lockin.rs | 32 +++++++++++---------------- dsp/src/lowpass.rs | 44 ++++++++++++++++++++++++++++++++++++++ src/bin/lockin-external.rs | 6 ++---- src/bin/lockin-internal.rs | 14 ++++++------ 7 files changed, 68 insertions(+), 31 deletions(-) create mode 100644 dsp/src/lowpass.rs diff --git a/Cargo.lock b/Cargo.lock index ed15a58..17030d2 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -353,6 +353,7 @@ name = "dsp" version = "0.1.0" dependencies = [ "criterion", + "generic-array 0.14.4", "libm", "ndarray", "rand", diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index de96d81..8e07060 100644 --- a/dsp/Cargo.toml +++ b/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" diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index f42e7c7..31ff680 100644 --- a/dsp/src/lib.rs +++ b/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; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index ea80836..6abd277 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,21 +1,19 @@ -use super::{ - iir_int::{Vec5, IIR}, - Complex, -}; -use serde::{Deserialize, Serialize}; +use super::{lowpass::Lowpass, Complex}; +use generic_array::typenum::U3; -#[derive(Copy, Clone, Default, Deserialize, Serialize)] +#[derive(Clone, Default)] pub struct Lockin { - iir: IIR, - state: [Vec5; 2], + state: [Lowpass; 2], + k: u32, } impl Lockin { /// Create a new Lockin with given IIR coefficients. - pub fn new(ba: Vec5) -> Self { + pub fn new(k: u32) -> Self { + let lp = Lowpass::default(); Self { - iir: IIR { ba }, - state: [Vec5::default(); 2], + state: [lp.clone(), lp.clone()], + 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( - &mut self.state[0], - ((sample as i64 * lo.0 as i64) >> 32) as _, - ), - self.iir.update( - &mut self.state[1], - ((sample as i64 * lo.1 as i64) >> 32) as _, - ), + self.state[0] + .update(((sample as i64 * lo.0 as i64) >> 32) as _, self.k), + self.state[1] + .update(((sample as i64 * lo.1 as i64) >> 32) as _, self.k), ) } } diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs new file mode 100644 index 0000000..50ca604 --- /dev/null +++ b/dsp/src/lowpass.rs @@ -0,0 +1,44 @@ +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> { + // IIR state storage + xy: GenericArray, +} + +impl> Lowpass { + /// Update the filter with a new sample. + /// + /// # Args + /// * `x`: Input data + /// * `k`: Cutoff, `u32::MAX` being Nyquist + /// + /// # Returns + /// Filtered output y + pub fn update(&mut self, x: i32, k: u32) -> i32 { + let mut x1 = self.xy[0]; + self.xy[0] = x; + let mut x0 = x; + + // This is an unrolled and optimized first-order IIR loop + // that works for all possible time constants. + for y1 in self.xy[1..].iter_mut() { + // Optimized first order lowpass expression + // Note the zero at Nyquist + let mut y0 = + ((x0 >> 1) as i64 + (x1 >> 1) as i64 - *y1 as i64) * k as i64; + y0 += (*y1 as i64) << 32; + y0 += 1i64 << 31; // Half-up rounding bias + + // Store and advance + x0 = (y0 >> 32) as i32; + x1 = *y1; + *y1 = x0; + } + x0 + } +} diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index 74ca836..ba576e9 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::{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( - iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose - ); + let lockin = Lockin::new(1 << 22); // Enable ADC/DAC events stabilizer.adcs.0.start(); diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index cadf189..e25c9fb 100644 --- a/src/bin/lockin-internal.rs +++ b/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( - iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose - ); + let lockin = Lockin::new(1 << 22); // 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(), From 30c2c2aac2049f319a49f88057e5a87528df84b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 10 Feb 2021 09:46:49 +0100 Subject: [PATCH 2/6] lowpass: i32, no multiplies --- dsp/src/lockin.rs | 4 ++-- dsp/src/lowpass.rs | 24 ++++++++---------------- src/bin/lockin-external.rs | 2 +- src/bin/lockin-internal.rs | 2 +- 4 files changed, 12 insertions(+), 20 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 6abd277..d78c01b 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -4,12 +4,12 @@ use generic_array::typenum::U3; #[derive(Clone, Default)] pub struct Lockin { state: [Lowpass; 2], - k: u32, + k: u8, } impl Lockin { /// Create a new Lockin with given IIR coefficients. - pub fn new(k: u32) -> Self { + pub fn new(k: u8) -> Self { let lp = Lowpass::default(); Self { state: [lp.clone(), lp.clone()], diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index 50ca604..e3a1353 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -15,27 +15,19 @@ impl> Lowpass { /// /// # Args /// * `x`: Input data - /// * `k`: Cutoff, `u32::MAX` being Nyquist + /// * `k`: Log2 time constant /// - /// # Returns + /// # Return /// Filtered output y - pub fn update(&mut self, x: i32, k: u32) -> i32 { - let mut x1 = self.xy[0]; - self.xy[0] = x; - let mut x0 = x; - + pub fn update(&mut self, x: i32, k: u8) -> i32 { // This is an unrolled and optimized first-order IIR loop // that works for all possible time constants. + // Note the zero(s) at Nyquist and the benign overflow (DF-I). + let mut x0 = x; + let mut x1 = self.xy[0]; + self.xy[0] = x; for y1 in self.xy[1..].iter_mut() { - // Optimized first order lowpass expression - // Note the zero at Nyquist - let mut y0 = - ((x0 >> 1) as i64 + (x1 >> 1) as i64 - *y1 as i64) * k as i64; - y0 += (*y1 as i64) << 32; - y0 += 1i64 << 31; // Half-up rounding bias - - // Store and advance - x0 = (y0 >> 32) as i32; + x0 = *y1 + (((x0 >> 1) + (x1 >> 1) - *y1 + (1 << k - 1)) >> k); x1 = *y1; *y1 = x0; } diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index ba576e9..fad27ec 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin-external.rs @@ -34,7 +34,7 @@ const APP: () = { + design_parameters::SAMPLE_BUFFER_SIZE_LOG2, ); - let lockin = Lockin::new(1 << 22); + let lockin = Lockin::new(10); // Enable ADC/DAC events stabilizer.adcs.0.start(); diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index e25c9fb..287815a 100644 --- a/src/bin/lockin-internal.rs +++ b/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(1 << 22); // TODO: expose + let lockin = Lockin::new(10); // TODO: expose // Enable ADC/DAC events stabilizer.adcs.1.start(); From 13b47556fd3f62e1356a4d0c64c43aebff823234 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 10 Feb 2021 13:27:56 +0100 Subject: [PATCH 3/6] lowpass: clippy --- dsp/src/lockin.rs | 2 +- dsp/src/lowpass.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index d78c01b..75c7b97 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -12,7 +12,7 @@ impl Lockin { pub fn new(k: u8) -> Self { let lp = Lowpass::default(); Self { - state: [lp.clone(), lp.clone()], + state: [lp.clone(), lp], k, } } diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index e3a1353..b85e2ed 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -27,7 +27,7 @@ impl> Lowpass { let mut x1 = self.xy[0]; self.xy[0] = x; for y1 in self.xy[1..].iter_mut() { - x0 = *y1 + (((x0 >> 1) + (x1 >> 1) - *y1 + (1 << k - 1)) >> k); + x0 = *y1 + (((x0 >> 1) + (x1 >> 1) - *y1 + (1 << (k - 1))) >> k); x1 = *y1; *y1 = x0; } From 8d68504026f33f6fb18481f7487489a541741a43 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 10 Feb 2021 13:31:41 +0100 Subject: [PATCH 4/6] lowpass: symmetric code --- dsp/src/lowpass.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index b85e2ed..24832a8 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -25,7 +25,7 @@ impl> Lowpass { // Note the zero(s) at Nyquist and the benign overflow (DF-I). let mut x0 = x; let mut x1 = self.xy[0]; - self.xy[0] = x; + self.xy[0] = x0; for y1 in self.xy[1..].iter_mut() { x0 = *y1 + (((x0 >> 1) + (x1 >> 1) - *y1 + (1 << (k - 1))) >> k); x1 = *y1; From beeb43bf8b388e7261188c4d5fe30d18c041030b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 10 Feb 2021 13:44:10 +0100 Subject: [PATCH 5/6] lowpass: robustify --- dsp/src/lowpass.rs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index 24832a8..39abffb 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -15,19 +15,20 @@ impl> Lowpass { /// /// # Args /// * `x`: Input data - /// * `k`: Log2 time constant + /// * `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 and the benign overflow (DF-I). + // 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 >> 1) + (x1 >> 1) - *y1 + (1 << (k - 1))) >> k); + x0 = *y1 + (((x0 >> 2) + (x1 >> 2) - (*y1 >> 1) + (1 << (k - 1))) >> k); x1 = *y1; *y1 = x0; } From a144c099b2e08032c318ee93eb72d526ef3852bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 10 Feb 2021 14:10:28 +0100 Subject: [PATCH 6/6] lowpass: fmt --- dsp/src/lowpass.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index 39abffb..fb2eedf 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -28,7 +28,8 @@ impl> Lowpass { 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); + x0 = *y1 + + (((x0 >> 2) + (x1 >> 2) - (*y1 >> 1) + (1 << (k - 1))) >> k); x1 = *y1; *y1 = x0; }