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..75c7b97 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: 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: [Vec5::default(); 2], + state: [lp.clone(), lp], + 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..fb2eedf --- /dev/null +++ b/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> { + // IIR state storage + xy: GenericArray, +} + +impl> Lowpass { + /// 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 + } +} diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index 74ca836..fad27ec 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(10); // Enable ADC/DAC events stabilizer.adcs.0.start(); diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index cadf189..287815a 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(10); // 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(),