lockin: refactor to use common lockin processing

This commit is contained in:
Robert Jördens 2021-01-22 15:11:16 +01:00
parent eea5033d36
commit d0d2c6352d
4 changed files with 54 additions and 85 deletions

View File

@ -3,20 +3,6 @@ use serde::{Deserialize, Serialize};
#[derive(Copy, Clone, Default, Deserialize, Serialize)] #[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct IIRState(pub [i32; 5]); pub struct IIRState(pub [i32; 5]);
impl IIRState {
#[inline(always)]
pub fn get_x(&self, index: usize) -> i32 {
// x0 is at index 0 in a biquad between updates
self.0[index]
}
#[inline(always)]
pub fn get_y(&self, index: usize) -> i32 {
// y0 is at index 2 in a biquad between updates
self.0[2 + index]
}
}
fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
// Rounding bias, half up // Rounding bias, half up
let y0 = ((y0 as i64) << shift) + (1 << (shift - 1)); let y0 = ((y0 as i64) << shift) + (1 << (shift - 1));
@ -42,8 +28,8 @@ pub struct IIR {
} }
impl IIR { impl IIR {
/// Coefficient fixed point: signed Q2.30. /// Coefficient fixed point format: signed Q2.30.
/// Tailored to low-passes PI, II etc. /// Tailored to low-passes, PI, II etc.
pub const SHIFT: u32 = 30; pub const SHIFT: u32 = 30;
/// Feed a new input value into the filter, update the filter state, and /// Feed a new input value into the filter, update the filter state, and

View File

@ -8,16 +8,18 @@ pub struct Lockin {
} }
impl Lockin { impl Lockin {
pub fn new(_corner: u8) -> Self { pub fn new(ba: &iir_int::IIRState) -> Self {
let mut iir = iir_int::IIR::default();
iir.ba.0.copy_from_slice(&ba.0);
Lockin { Lockin {
iir: iir_int::IIR::default(), // TODO: lowpass coefficients from corner iir,
iir_state: [iir_int::IIRState::default(); 2], iir_state: [iir_int::IIRState::default(); 2],
} }
} }
pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> { pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> {
// Get the LO signal for demodulation. // Get the LO signal for demodulation.
let m = cossin(phase.wrapping_neg()); let m = cossin(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.

View File

@ -1,7 +1,8 @@
use dsp::{ use dsp::{
atan2, cossin, atan2,
iir_int::{IIRState, IIR}, iir_int::{IIRState, IIR},
reciprocal_pll::TimestampHandler, reciprocal_pll::TimestampHandler,
lockin::Lockin,
Complex, Complex,
}; };
@ -30,51 +31,39 @@ pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool {
} }
/// ADC full scale in machine units (16 bit signed). /// ADC full scale in machine units (16 bit signed).
const ADC_SCALE: f64 = ((1 << 15) - 1) as f64; const ADC_SCALE: f64 = ((1 << 15) - 1) as _;
struct Lockin { struct PllLockin {
harmonic: u32, harmonic: i32,
phase: u32, phase: i32,
iir: IIR, lockin: Lockin,
iir_state: [IIRState; 2],
} }
impl Lockin { impl PllLockin {
pub fn new(harmonic: u32, phase: u32, iir: IIR) -> Self { pub fn new(harmonic: i32, phase: i32, iir: &IIRState) -> Self {
Lockin { PllLockin {
harmonic, harmonic,
phase, phase,
iir, lockin: Lockin::new(iir)
iir_state: [IIRState::default(); 2],
} }
} }
pub fn update( pub fn update(
&mut self, &mut self,
input: Vec<i16>, input: Vec<i16>,
phase: u32, phase: i32,
frequency: u32, frequency: i32,
) -> Complex<i32> { ) -> Complex<i32> {
let frequency = frequency.wrapping_mul(self.harmonic); let sample_frequency = frequency.wrapping_mul(self.harmonic);
let mut phase = let mut sample_phase =
self.phase.wrapping_add(phase.wrapping_mul(self.harmonic)); self.phase.wrapping_add(phase.wrapping_mul(self.harmonic));
input input
.iter() .iter()
.map(|&s| { .map(|&s| {
let m = cossin((phase as i32).wrapping_neg()); let input = (s as i32) << 16;
phase = phase.wrapping_add(frequency); let signal = self.lockin.update(input, sample_phase.wrapping_neg());
sample_phase = sample_phase.wrapping_add(sample_frequency);
let signal = (s as i32) << 16; signal
Complex(
self.iir.update(
&mut self.iir_state[0],
((signal as i64 * m.0 as i64) >> 32) as _,
),
self.iir.update(
&mut self.iir_state[1],
((signal as i64 * m.1 as i64) >> 32) as _,
),
)
}) })
.last() .last()
.unwrap_or(Complex::default()) .unwrap_or(Complex::default())
@ -138,10 +127,9 @@ fn sampled_noise_amplitude(
tones tones
.iter() .iter()
.map(|t| { .map(|t| {
let decades = let df = (t.frequency - frequency) / corner;
((t.frequency - frequency) / corner).abs().max(1.).log10(); // Assuming a 2nd order lowpass filter: 40dB/decade.
// 2nd-order filter: 40dB/decade rolloff. linear(t.amplitude_dbfs - 40. * df.abs().max(1.).log10())
linear(t.amplitude_dbfs - 40. * decades)
}) })
.sum::<f64>() .sum::<f64>()
.max(1. / ADC_SCALE / 2.) // 1/2 LSB from quantization .max(1. / ADC_SCALE / 2.) // 1/2 LSB from quantization
@ -152,21 +140,18 @@ fn sampled_noise_amplitude(
/// only if one occurred during the batch. /// only if one occurred during the batch.
/// ///
/// # Args /// # Args
/// * `reference_frequency` - External reference signal frequency (in Hz). /// * `reference_period` - External reference signal period in units of the internal clock period.
/// * `timestamp_start` - Start time in terms of the internal clock count. This is the start time of /// * `timestamp_start` - Start time in terms of the internal clock count. This is the start time of
/// the current processing sequence. /// the current processing sequence.
/// * `timestamp_stop` - Stop time in terms of the internal clock count. /// * `timestamp_stop` - Stop time in terms of the internal clock count.
/// * `internal_frequency` - Internal clock frequency (in Hz).
/// ///
/// # Returns /// # Returns
/// An Option, containing a timestamp if one occurred during the current batch period. /// An Option, containing a timestamp if one occurred during the current batch period.
fn adc_batch_timestamps( fn adc_batch_timestamps(
reference_frequency: f64, reference_period: f64,
timestamp_start: u64, timestamp_start: u64,
timestamp_stop: u64, timestamp_stop: u64,
internal_frequency: f64,
) -> Option<u32> { ) -> Option<u32> {
let reference_period = internal_frequency / reference_frequency;
let start_count = timestamp_start as f64 % reference_period; let start_count = timestamp_start as f64 % reference_period;
let timestamp = (reference_period - start_count) % reference_period; let timestamp = (reference_period - start_count) % reference_period;
@ -374,7 +359,7 @@ fn lowpass_test(
internal_frequency: f64, internal_frequency: f64,
reference_frequency: f64, reference_frequency: f64,
demodulation_phase_offset: f64, demodulation_phase_offset: f64,
harmonic: u32, harmonic: i32,
sample_buffer_size_log2: usize, sample_buffer_size_log2: usize,
pll_shift_frequency: u8, pll_shift_frequency: u8,
pll_shift_phase: u8, pll_shift_phase: u8,
@ -402,17 +387,14 @@ fn lowpass_test(
"The base-2 log of the number of ADC ticks in a sampling period plus the base-2 log of the sample buffer size must be less than 32." "The base-2 log of the number of ADC ticks in a sampling period plus the base-2 log of the sample buffer size must be less than 32."
); );
let mut lockin = Lockin::new( let mut lockin = PllLockin::new(
harmonic, harmonic,
(demodulation_phase_offset / (2. * PI) * (1_u64 << 32) as f64).round() (demodulation_phase_offset / (2. * PI) * (1i64 << 32) as f64).round()
as u32, as i32,
IIR { &lowpass_iir_coefficients(
ba: lowpass_iir_coefficients(
corner_frequency, corner_frequency,
1. / 2f64.sqrt(), 1. / 2f64.sqrt(), // critical q
2., 2.) // DC gain to get to full scale with the image filtered out
),
},
); );
let mut timestamp_handler = TimestampHandler::new( let mut timestamp_handler = TimestampHandler::new(
pll_shift_frequency, pll_shift_frequency,
@ -470,10 +452,9 @@ fn lowpass_test(
1 << sample_buffer_size_log2, 1 << sample_buffer_size_log2,
); );
let timestamp = adc_batch_timestamps( let timestamp = adc_batch_timestamps(
reference_frequency, internal_frequency/reference_frequency,
timestamp_start, timestamp_start,
timestamp_start + batch_sample_count - 1, timestamp_start + batch_sample_count - 1,
internal_frequency,
); );
timestamp_start += batch_sample_count; timestamp_start += batch_sample_count;
@ -481,8 +462,8 @@ fn lowpass_test(
timestamp_handler.update(timestamp); timestamp_handler.update(timestamp);
let output = lockin.update( let output = lockin.update(
adc_signal, adc_signal,
demodulation_initial_phase, demodulation_initial_phase as i32,
demodulation_frequency, demodulation_frequency as i32,
); );
let magnitude = (((output.0 as i64) * (output.0 as i64) let magnitude = (((output.0 as i64) * (output.0 as i64)
+ (output.1 as i64) * (output.1 as i64)) + (output.1 as i64) * (output.1 as i64))
@ -571,7 +552,7 @@ fn lowpass_test(
fn lowpass() { fn lowpass() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 64e-3; let signal_frequency: f64 = 64e-3;
let harmonic: u32 = 1; let harmonic: i32 = 1;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 3; let pll_shift_frequency: u8 = 3;
let pll_shift_phase: u8 = 2; let pll_shift_phase: u8 = 2;
@ -616,7 +597,7 @@ fn lowpass() {
fn lowpass_demodulation_phase_offset_pi_2() { fn lowpass_demodulation_phase_offset_pi_2() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 64e-3; let signal_frequency: f64 = 64e-3;
let harmonic: u32 = 1; let harmonic: i32 = 1;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 3; let pll_shift_frequency: u8 = 3;
let pll_shift_phase: u8 = 2; let pll_shift_phase: u8 = 2;
@ -661,7 +642,7 @@ fn lowpass_demodulation_phase_offset_pi_2() {
fn lowpass_phase_offset_pi_2() { fn lowpass_phase_offset_pi_2() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 64e-3; let signal_frequency: f64 = 64e-3;
let harmonic: u32 = 1; let harmonic: i32 = 1;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 3; let pll_shift_frequency: u8 = 3;
let pll_shift_phase: u8 = 2; let pll_shift_phase: u8 = 2;
@ -706,7 +687,7 @@ fn lowpass_phase_offset_pi_2() {
fn lowpass_fundamental_71e_3_phase_offset_pi_4() { fn lowpass_fundamental_71e_3_phase_offset_pi_4() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 71e-3; let signal_frequency: f64 = 71e-3;
let harmonic: u32 = 1; let harmonic: i32 = 1;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 3; let pll_shift_frequency: u8 = 3;
let pll_shift_phase: u8 = 2; let pll_shift_phase: u8 = 2;
@ -751,7 +732,7 @@ fn lowpass_fundamental_71e_3_phase_offset_pi_4() {
fn lowpass_first_harmonic() { fn lowpass_first_harmonic() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 50e-3; let signal_frequency: f64 = 50e-3;
let harmonic: u32 = 2; let harmonic: i32 = 2;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 2; let pll_shift_frequency: u8 = 2;
let pll_shift_phase: u8 = 1; let pll_shift_phase: u8 = 1;
@ -796,7 +777,7 @@ fn lowpass_first_harmonic() {
fn lowpass_second_harmonic() { fn lowpass_second_harmonic() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 50e-3; let signal_frequency: f64 = 50e-3;
let harmonic: u32 = 3; let harmonic: i32 = 3;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 2; let pll_shift_frequency: u8 = 2;
let pll_shift_phase: u8 = 1; let pll_shift_phase: u8 = 1;
@ -841,7 +822,7 @@ fn lowpass_second_harmonic() {
fn lowpass_third_harmonic() { fn lowpass_third_harmonic() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 50e-3; let signal_frequency: f64 = 50e-3;
let harmonic: u32 = 4; let harmonic: i32 = 4;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 2; let pll_shift_frequency: u8 = 2;
let pll_shift_phase: u8 = 1; let pll_shift_phase: u8 = 1;
@ -886,7 +867,7 @@ fn lowpass_third_harmonic() {
fn lowpass_first_harmonic_phase_shift() { fn lowpass_first_harmonic_phase_shift() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 50e-3; let signal_frequency: f64 = 50e-3;
let harmonic: u32 = 2; let harmonic: i32 = 2;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 2; let pll_shift_frequency: u8 = 2;
let pll_shift_phase: u8 = 1; let pll_shift_phase: u8 = 1;
@ -931,7 +912,7 @@ fn lowpass_first_harmonic_phase_shift() {
fn lowpass_adc_frequency_1e6() { fn lowpass_adc_frequency_1e6() {
let internal_frequency: f64 = 32.; let internal_frequency: f64 = 32.;
let signal_frequency: f64 = 100e-3; let signal_frequency: f64 = 100e-3;
let harmonic: u32 = 1; let harmonic: i32 = 1;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 2; let pll_shift_frequency: u8 = 2;
let pll_shift_phase: u8 = 1; let pll_shift_phase: u8 = 1;
@ -976,7 +957,7 @@ fn lowpass_adc_frequency_1e6() {
fn lowpass_internal_frequency_125e6() { fn lowpass_internal_frequency_125e6() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 100e-3; let signal_frequency: f64 = 100e-3;
let harmonic: u32 = 1; let harmonic: i32 = 1;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 2; let pll_shift_frequency: u8 = 2;
let pll_shift_phase: u8 = 1; let pll_shift_phase: u8 = 1;
@ -1021,7 +1002,7 @@ fn lowpass_internal_frequency_125e6() {
fn lowpass_low_signal_frequency() { fn lowpass_low_signal_frequency() {
let internal_frequency: f64 = 64.; let internal_frequency: f64 = 64.;
let signal_frequency: f64 = 10e-3; let signal_frequency: f64 = 10e-3;
let harmonic: u32 = 1; let harmonic: i32 = 1;
let sample_buffer_size_log2: usize = 2; let sample_buffer_size_log2: usize = 2;
let pll_shift_frequency: u8 = 2; let pll_shift_frequency: u8 = 2;
let pll_shift_phase: u8 = 1; let pll_shift_phase: u8 = 1;

View File

@ -16,7 +16,7 @@ use stabilizer::{
hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2, hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
}; };
use dsp::{iir, lockin::Lockin, reciprocal_pll::TimestampHandler}; use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler};
use hardware::{ use hardware::{
Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
}; };
@ -61,7 +61,7 @@ const APP: () = {
); );
let lockin = Lockin::new( let lockin = Lockin::new(
10, // relative Locking lowpass filter bandwidth, TODO: expose &iir_int::IIRState::default(), // TODO: lowpass, expose
); );
// Enable ADC/DAC events // Enable ADC/DAC events