move lowpass gain outside lowpass/lockin

This commit is contained in:
Robert Jördens 2021-02-18 14:06:01 +01:00
parent 9983fad041
commit 07b7751dbc
4 changed files with 21 additions and 18 deletions

View File

@ -8,8 +8,7 @@ pub struct Lockin {
impl Lockin { impl Lockin {
/// Update the lockin with a sample taken at a given phase. /// Update the lockin with a sample taken at a given phase.
/// The lowpass has a gain of `1 << k`. pub fn update(&mut self, sample: i32, phase: i32, k: u8) -> Complex<i32> {
pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex<i32> {
// Get the LO signal for demodulation and mix the sample; // Get the LO signal for demodulation and mix the sample;
let mix = Complex::from_angle(phase).mul_scaled(sample); let mix = Complex::from_angle(phase).mul_scaled(sample);

View File

@ -14,17 +14,17 @@ 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, needs `k` bits headroom. /// * `x`: Input data. Needs 1 bit headroom.
/// * `k`: Log2 time constant, 0..31. /// * `k`: Log2 time constant, 0..=31.
/// ///
/// # Return /// # Return
/// Filtered output y, with gain of `1 << k`. /// 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!(k & 31 == 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 DF-II and the zeros at Nyquist. // Note T-DF-I and the zeros at Nyquist.
let mut x = x << k; let mut x = x;
for y in self.y.iter_mut() { for y in self.y.iter_mut() {
let dy = (x - *y + (1 << (k - 1))) >> k; let dy = (x - *y + (1 << (k - 1))) >> k;
*y += dy; *y += dy;

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::{Accu, FastInt, Lockin, RPLL}; use dsp::{Accu, Complex, FastInt, Lockin, RPLL};
use hardware::{ use hardware::{
Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
}; };
@ -112,15 +112,18 @@ const APP: () = {
let sample_phase = let sample_phase =
phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
let output = adc_samples[0] let output: Complex<i32> = adc_samples[0]
.iter() .iter()
// Zip in the LO phase.
.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, update the Lockin (demodulate, filter)
.map(|(&sample, phase)| { .map(|(&sample, phase)| {
lockin.update(sample as i16, phase, time_constant) let s = (sample as i16 as i32)
<< (15 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2 + 1);
lockin.update(s, phase, time_constant)
}) })
.last() // Decimate
.unwrap(); .sum();
let conf = "frequency_discriminator"; let conf = "frequency_discriminator";
let output = match conf { let output = match conf {

View File

@ -2,7 +2,7 @@
#![no_std] #![no_std]
#![no_main] #![no_main]
use dsp::{Accu, FastInt, Lockin}; use dsp::{Accu, Complex, FastInt, Lockin};
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};
@ -95,17 +95,18 @@ const APP: () = {
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));
let output = adc_samples let output: Complex<i32> = adc_samples
.iter() .iter()
// Zip in the LO phase. // Zip in the LO phase.
.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, phase, time_constant) let s = (sample as i16 as i32)
<< (15 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2 + 1);
lockin.update(s, phase, time_constant)
}) })
// Decimate // Decimate
.last() .sum();
.unwrap();
for value in dac_samples[1].iter_mut() { for value in dac_samples[1].iter_mut() {
*value = (output.arg() >> 16) as u16 ^ 0x8000; *value = (output.arg() >> 16) as u16 ^ 0x8000;