Merge pull request #266 from quartiq/rj/lowpass

new lowpass
This commit is contained in:
Robert Jördens 2021-02-10 14:25:46 +01:00 committed by GitHub
commit c89f348f5e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 62 additions and 31 deletions

1
Cargo.lock generated
View File

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

View File

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

View File

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

View File

@ -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 { pub struct Lockin {
iir: IIR, state: [Lowpass<U3>; 2],
state: [Vec5; 2], k: u8,
} }
impl Lockin { impl Lockin {
/// Create a new Lockin with given IIR coefficients. /// 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 { 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. // return IQ (in-phase and quadrature) data.
// Note: 32x32 -> 64 bit multiplications are pretty much free. // Note: 32x32 -> 64 bit multiplications are pretty much free.
Complex( 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 _,
),
) )
} }
} }

38
dsp/src/lowpass.rs Normal file
View File

@ -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
}
}

View File

@ -6,7 +6,7 @@ use stm32h7xx_hal as hal;
use stabilizer::{hardware, hardware::design_parameters}; use stabilizer::{hardware, hardware::design_parameters};
use dsp::{iir_int, lockin::Lockin, rpll::RPLL, Accu}; use dsp::{lockin::Lockin, rpll::RPLL, Accu};
use hardware::{ use hardware::{
Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
}; };
@ -34,9 +34,7 @@ const APP: () = {
+ design_parameters::SAMPLE_BUFFER_SIZE_LOG2, + 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 // Enable ADC/DAC events
stabilizer.adcs.0.start(); stabilizer.adcs.0.start();

View File

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