diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 75c7b97..7fc1e85 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,35 +1,30 @@ use super::{lowpass::Lowpass, Complex}; -use generic_array::typenum::U3; +use generic_array::typenum::U4; #[derive(Clone, Default)] pub struct Lockin { - state: [Lowpass; 2], - k: u8, + state: [Lowpass; 2], } impl Lockin { /// Create a new Lockin with given IIR coefficients. - pub fn new(k: u8) -> Self { + pub fn new() -> Self { let lp = Lowpass::default(); Self { state: [lp.clone(), lp], - k, } } /// Update the lockin with a sample taken at a given phase. - pub fn update(&mut self, sample: i32, phase: i32) -> Complex { + pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex { // Get the LO signal for demodulation. let lo = Complex::from_angle(phase); // Mix with the LO signal, filter with the IIR lowpass, // return IQ (in-phase and quadrature) data. - // Note: 32x32 -> 64 bit multiplications are pretty much free. Complex( - 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), + self.state[0].update((sample as i32 * (lo.0 >> 16)) >> 16, k), + self.state[1].update((sample as i32 * (lo.1 >> 16)) >> 16, k), ) } } diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index fb2eedf..5004923 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -1,38 +1,35 @@ use generic_array::{ArrayLength, GenericArray}; /// 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)] pub struct Lowpass> { // IIR state storage - xy: GenericArray, + y: GenericArray, } impl> Lowpass { /// Update the filter with a new sample. /// /// # Args - /// * `x`: Input data - /// * `k`: Log2 time constant, 1..32 + /// * `x`: Input data, needs k bits headroom + /// * `k`: Log2 time constant, 0..31 /// /// # Return /// Filtered output y 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 // 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; + // Note DF-II and the zero(s) at Nyquist. + let mut x = x; + for y in self.y.iter_mut() { + let dy = x - (*y >> k); + *y += dy; + x = (*y - (dy >> 1)) >> k; } - x0 + x } } diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index fad27ec..5f7451e 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(10); + let lockin = Lockin::new(); // Enable ADC/DAC events stabilizer.adcs.0.start(); @@ -102,6 +102,9 @@ const APP: () = { // Demodulation LO phase offset let phase_offset: i32 = 0; // TODO: expose + // Log2 lowpass time constant + let time_constant: u8 = 8; // TODO: expose + let sample_frequency = ((pll_frequency // .wrapping_add(1 << design_parameters::SAMPLE_BUFFER_SIZE_LOG2 - 1) // half-up rounding bias >> design_parameters::SAMPLE_BUFFER_SIZE_LOG2) @@ -115,7 +118,7 @@ const APP: () = { .zip(Accu::new(sample_phase, sample_frequency)) // Convert to signed, MSB align the ADC sample. .map(|(&sample, phase)| { - lockin.update((sample as i16 as i32) << 16, phase) + lockin.update(sample as i16, phase, time_constant) }) .last() .unwrap(); @@ -125,7 +128,7 @@ const APP: () = { // Convert from IQ to power and phase. "power_phase" => [output.abs_sqr(), output.arg()], "frequency_discriminator" => [pll_frequency as i32, output.arg()], - _ => [output.0, output.1], + _ => [output.0 << 16, output.1 << 16], }; // Convert to DAC data. diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index 287815a..cfa3115 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(10); // TODO: expose + let lockin = Lockin::new(); // Enable ADC/DAC events stabilizer.adcs.1.start(); @@ -85,10 +85,14 @@ const APP: () = { 1i32 << (32 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2); // 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 - 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_phase = phase_offset .wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); @@ -99,24 +103,14 @@ const APP: () = { .zip(Accu::new(sample_phase, sample_frequency)) // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) .map(|(&sample, phase)| { - lockin.update((sample as i16 as i32) << 16, phase) + lockin.update(sample as i16, phase, time_constant) }) // Decimate .last() .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() { - *value = (output[1] >> 16) as u16 ^ 0x8000; + *value = (output.arg() >> 16) as u16 ^ 0x8000; } }