lowpass: reimplement better

This commit is contained in:
Robert Jördens 2021-02-11 14:30:05 +01:00
parent c89f348f5e
commit 3ae0b710bc
4 changed files with 34 additions and 45 deletions

View File

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

View File

@ -1,38 +1,35 @@
use generic_array::{ArrayLength, GenericArray}; use generic_array::{ArrayLength, GenericArray};
/// Arbitrary order, high dynamic range, wide coefficient range, /// 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)] #[derive(Clone, Default)]
pub struct Lowpass<N: ArrayLength<i32>> { pub struct Lowpass<N: ArrayLength<i32>> {
// IIR state storage // IIR state storage
xy: GenericArray<i32, N>, y: GenericArray<i32, N>,
} }
impl<N: ArrayLength<i32>> Lowpass<N> { impl<N: ArrayLength<i32>> Lowpass<N> {
/// Update the filter with a new sample. /// Update the filter with a new sample.
/// ///
/// # Args /// # Args
/// * `x`: Input data /// * `x`: Input data, needs k bits headroom
/// * `k`: Log2 time constant, 1..32 /// * `k`: Log2 time constant, 0..31
/// ///
/// # Return /// # Return
/// Filtered output y /// Filtered output y
pub fn update(&mut self, x: i32, k: u8) -> i32 { 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 // This is an unrolled and optimized first-order IIR loop
// that works for all possible time constants. // 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
} }
} }

View File

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

View File

@ -28,7 +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(10); // TODO: expose let lockin = Lockin::new();
// Enable ADC/DAC events // Enable ADC/DAC events
stabilizer.adcs.1.start(); stabilizer.adcs.1.start();
@ -85,10 +85,14 @@ const APP: () = {
1i32 << (32 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2); 1i32 << (32 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2);
// Harmonic index of the LO: -1 to _de_modulate the fundamental // 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 // 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_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
let sample_phase = phase_offset let sample_phase = phase_offset
.wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
@ -99,24 +103,14 @@ const APP: () = {
.zip(Accu::new(sample_phase, sample_frequency)) .zip(Accu::new(sample_phase, sample_frequency))
// Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
.map(|(&sample, phase)| { .map(|(&sample, phase)| {
lockin.update((sample as i16 as i32) << 16, phase) lockin.update(sample as i16, phase, time_constant)
}) })
// Decimate // Decimate
.last() .last()
.unwrap(); .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() { for value in dac_samples[1].iter_mut() {
*value = (output[1] >> 16) as u16 ^ 0x8000; *value = (output.arg() >> 16) as u16 ^ 0x8000;
} }
} }