lockin: refine

This commit is contained in:
Robert Jördens 2021-01-17 22:19:14 +01:00
parent 147b0a6982
commit 20488ea3bc
6 changed files with 235 additions and 345 deletions

View File

@ -1,6 +1,7 @@
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
pub type IIRState = [i32; 5]; #[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct IIRState(pub [i32; 5]);
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
@ -18,7 +19,7 @@ fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
/// See `dsp::iir::IIR` for general implementation details. /// See `dsp::iir::IIR` for general implementation details.
/// Offset and limiting disabled to suit lowpass applications. /// Offset and limiting disabled to suit lowpass applications.
/// Coefficient scaling fixed and optimized. /// Coefficient scaling fixed and optimized.
#[derive(Copy, Clone, Deserialize, Serialize)] #[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct IIR { pub struct IIR {
pub ba: IIRState, pub ba: IIRState,
// pub y_offset: i32, // pub y_offset: i32,
@ -29,7 +30,7 @@ pub struct IIR {
impl IIR { impl IIR {
/// Coefficient fixed point: signed Q2.30. /// Coefficient fixed point: signed Q2.30.
/// Tailored to low-passes PI, II etc. /// Tailored to low-passes PI, II etc.
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
/// return the new output. Only the state `xy` is modified. /// return the new output. Only the state `xy` is modified.
@ -38,21 +39,21 @@ impl IIR {
/// * `xy` - Current filter state. /// * `xy` - Current filter state.
/// * `x0` - New input. /// * `x0` - New input.
pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 { pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 {
let n = self.ba.len(); let n = self.ba.0.len();
debug_assert!(xy.len() == n); debug_assert!(xy.0.len() == n);
// `xy` contains x0 x1 y0 y1 y2 // `xy` contains x0 x1 y0 y1 y2
// Increment time x1 x2 y1 y2 y3 // Increment time x1 x2 y1 y2 y3
// Shift x1 x1 x2 y1 y2 // Shift x1 x1 x2 y1 y2
// This unrolls better than xy.rotate_right(1) // This unrolls better than xy.rotate_right(1)
xy.copy_within(0..n - 1, 1); xy.0.copy_within(0..n - 1, 1);
// Store x0 x0 x1 x2 y1 y2 // Store x0 x0 x1 x2 y1 y2
xy[0] = x0; xy.0[0] = x0;
// Compute y0 by multiply-accumulate // Compute y0 by multiply-accumulate
let y0 = macc(0, xy, &self.ba, IIR::SHIFT); let y0 = macc(0, &xy.0, &self.ba.0, IIR::SHIFT);
// Limit y0 // Limit y0
// let y0 = y0.max(self.y_min).min(self.y_max); // let y0 = y0.max(self.y_min).min(self.y_max);
// Store y0 x0 x1 y0 y1 y2 // Store y0 x0 x1 y0 y1 y2
xy[n / 2] = y0; xy.0[n / 2] = y0;
y0 y0
} }
} }

View File

@ -2,8 +2,10 @@
#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))]
use core::ops::{Add, Mul, Neg}; use core::ops::{Add, Mul, Neg};
use serde::{Deserialize, Serialize};
pub type Complex<T> = (T, T); #[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct Complex<T>(pub T, pub T);
/// Bit shift, round up half. /// Bit shift, round up half.
/// ///
@ -17,7 +19,7 @@ pub type Complex<T> = (T, T);
/// Shifted and rounded value. /// Shifted and rounded value.
#[inline(always)] #[inline(always)]
pub fn shift_round(x: i32, shift: usize) -> i32 { pub fn shift_round(x: i32, shift: usize) -> i32 {
x.saturating_add(1 << (shift - 1)) >> shift (x + (1 << (shift - 1))) >> shift
} }
/// Integer division, round up half. /// Integer division, round up half.
@ -122,4 +124,4 @@ pub mod trig;
pub mod unwrap; pub mod unwrap;
#[cfg(test)] #[cfg(test)]
mod testing; pub mod testing;

View File

@ -1,6 +1,22 @@
#![allow(dead_code)] #![allow(dead_code)]
use super::Complex; use super::Complex;
/// Maximum acceptable error between a computed and actual value given fixed and relative
/// tolerances.
///
/// # Args
/// * `a` - First input.
/// * `b` - Second input. The relative tolerance is computed with respect to the maximum of the
/// absolute values of the first and second inputs.
/// * `rtol` - Relative tolerance.
/// * `atol` - Fixed tolerance.
///
/// # Returns
/// Maximum acceptable error.
pub fn max_error(a: f64, b: f64, rtol: f64, atol: f64) -> f64 {
rtol * a.abs().max(b.abs()) + atol
}
pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool { pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool {
(a - b).abs() <= a.abs().max(b.abs()) * rtol + atol (a - b).abs() <= a.abs().max(b.abs()) * rtol + atol
} }

View File

@ -153,7 +153,7 @@ pub fn cossin(phase: i32) -> Complex<i32> {
sin *= -1; sin *= -1;
} }
(cos, sin) Complex(cos, sin)
} }
#[cfg(test)] #[cfg(test)]
@ -210,13 +210,13 @@ mod tests {
#[test] #[test]
fn cossin_error_max_rms_all_phase() { fn cossin_error_max_rms_all_phase() {
// Constant amplitude error due to LUT data range. // Constant amplitude error due to LUT data range.
const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as f64; const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as _;
const MAX_PHASE: f64 = (1i64 << 32) as f64; const MAX_PHASE: f64 = (1i64 << 32) as _;
let mut rms_err: Complex<f64> = (0., 0.); let mut rms_err = Complex(0., 0.);
let mut sum_err: Complex<f64> = (0., 0.); let mut sum_err = Complex(0., 0.);
let mut max_err: Complex<f64> = (0., 0.); let mut max_err = Complex(0f64, 0.);
let mut sum: Complex<f64> = (0., 0.); let mut sum = Complex(0., 0.);
let mut demod: Complex<f64> = (0., 0.); let mut demod = Complex(0., 0.);
// use std::{fs::File, io::{BufWriter, prelude::*}, path::Path}; // use std::{fs::File, io::{BufWriter, prelude::*}, path::Path};
// let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap()); // let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap());

View File

@ -1,7 +1,6 @@
use dsp::{ use dsp::{
iir_int::{IIRState, IIR}, iir_int::{IIRState, IIR},
reciprocal_pll::TimestampHandler, reciprocal_pll::TimestampHandler,
shift_round,
trig::{atan2, cossin}, trig::{atan2, cossin},
Complex, Complex,
}; };
@ -9,191 +8,113 @@ use dsp::{
use std::f64::consts::PI; use std::f64::consts::PI;
use std::vec::Vec; use std::vec::Vec;
const ADC_MAX: f64 = 1.; // TODO: -> dsp/src/testing.rs
/// Maximum acceptable error between a computed and actual value given fixed and relative
/// tolerances.
///
/// # Args
/// * `a` - First input.
/// * `b` - Second input. The relative tolerance is computed with respect to the maximum of the
/// absolute values of the first and second inputs.
/// * `rtol` - Relative tolerance.
/// * `atol` - Fixed tolerance.
///
/// # Returns
/// Maximum acceptable error.
pub fn max_error(a: f64, b: f64, rtol: f64, atol: f64) -> f64 {
rtol * a.abs().max(b.abs()) + atol
}
pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool {
(a - b).abs() <= a.abs().max(b.abs()) * rtol + atol
}
const ADC_MAX_COUNT: f64 = (1 << 15) as f64; const ADC_MAX_COUNT: f64 = (1 << 15) as f64;
struct Lockin { struct Lockin {
harmonic: u32, harmonic: u32,
phase_offset: u32, phase: u32,
iir: IIR, iir: IIR,
iir_state: [IIRState; 2], iir_state: [IIRState; 2],
} }
impl Lockin { impl Lockin {
/// Construct a new `Lockin` instance. pub fn new(harmonic: u32, phase: u32, iir: IIR) -> Self {
///
/// # Args
/// * `harmonic` - Factor for harmonic demodulation. For example, a value of 1 would demodulate
/// with the reference frequency whereas a value of 2 would demodulate with the first harmonic
/// of the reference frequency.
/// * `phase_offset` - Phase offset of the scaled (see `harmonic`) demodulation signal relative
/// to the reference signal.
/// * `iir` - IIR coefficients (see `iir_int::IIR`) used for filtering the demodulated in-phase
/// and quadrature signals.
pub fn new(harmonic: u32, phase_offset: u32, iir: IIR) -> Self {
Lockin { Lockin {
harmonic, harmonic,
phase_offset, phase,
iir, iir,
iir_state: [[0; 5]; 2], iir_state: [IIRState::default(); 2],
} }
} }
/// Compute the in-phase and quadrature signals. This is intended to mimic the lock-in
/// processing routine invoked in main.rs.
///
/// # Args
/// * `adc_samples` - ADC samples.
/// * `demodulation_initial_phase` - Phase value of the demodulation signal corresponding to the
/// first ADC sample.
/// * `demodulation_frequency` - Demodulation frequency.
pub fn update( pub fn update(
&mut self, &mut self,
adc_samples: Vec<i16>, input: Vec<i16>,
demodulation_initial_phase: u32, phase: u32,
demodulation_frequency: u32, frequency: u32,
) -> Complex<i32> { ) -> Complex<i32> {
let mut signal = Vec::<Complex<i32>>::new(); let frequency = frequency.wrapping_mul(self.harmonic);
let mut phase =
self.phase.wrapping_add(phase.wrapping_mul(self.harmonic));
let mut last = Complex::default();
adc_samples.iter().enumerate().for_each(|(i, s)| { for s in input.iter() {
let sample_phase = self let m = cossin(-(phase as i32));
.harmonic phase = phase.wrapping_add(frequency);
.wrapping_mul(
(demodulation_frequency.wrapping_mul(i as u32))
.wrapping_add(demodulation_initial_phase),
)
.wrapping_add(self.phase_offset);
let (cos, sin) = cossin(sample_phase as i32);
signal.push(( last = Complex(
*s as i32 * shift_round(sin, 16), self.iir.update(
*s as i32 * shift_round(cos, 16), &mut self.iir_state[0],
)); ((*s as i64 * m.0 as i64) >> 16) as i32,
),
signal[i].0 = self.iir.update(&mut self.iir_state[0], signal[i].0); self.iir.update(
signal[i].1 = self.iir.update(&mut self.iir_state[1], signal[i].1); &mut self.iir_state[1],
}); ((*s as i64 * m.1 as i64) >> 16) as i32,
),
(signal[0].0, signal[0].1) );
}
last
} }
} }
/// Single-frequency sinusoid. /// Single-frequency sinusoid.
#[derive(Copy, Clone)] #[derive(Copy, Clone)]
struct PureSine { struct Tone {
// Frequency (in Hz). // Frequency (in Hz).
frequency: f64, frequency: f64,
// Amplitude in dBFS (decibels relative to full-scale). A 16-bit ADC has a minimum dBFS for each
// sample of -90.
amplitude_dbfs: f64,
// Phase offset (in radians). // Phase offset (in radians).
phase_offset: f64, phase: f64,
// Amplitude in dBFS (decibels relative to full-scale).
// A 16-bit ADC has a minimum dBFS for each sample of -90.
amplitude_dbfs: f64,
} }
/// Convert a dBFS voltage ratio to a linear ratio. /// Convert dBFS to a linear ratio.
///
/// # Args
/// * `dbfs` - dB ratio relative to full scale.
///
/// # Returns
/// Linear value.
fn linear(dbfs: f64) -> f64 { fn linear(dbfs: f64) -> f64 {
let base = 10_f64; 10f64.powf(dbfs / 20.)
ADC_MAX * base.powf(dbfs / 20.)
} }
/// Convert a linear voltage ratio to a dBFS ratio. /// Generate a full batch of samples starting at `time_offset`.
///
/// # Args
/// * `linear` - Linear voltage ratio.
///
/// # Returns
/// dBFS value.
fn dbfs(linear: f64) -> f64 {
20. * (linear / ADC_MAX).log10()
}
/// Convert a real ADC input value in the range `-ADC_MAX` to `+ADC_MAX` to an equivalent 16-bit ADC
/// sampled value. This models the ideal ADC transfer function.
///
/// # Args
/// * `x` - Real ADC input value.
///
/// # Returns
/// Sampled ADC value.
fn real_to_adc_sample(x: f64) -> i16 {
let max: i32 = i16::MAX as i32;
let min: i32 = i16::MIN as i32;
let xi: i32 = (x / ADC_MAX * ADC_MAX_COUNT) as i32;
// It's difficult to characterize the correct output result when the inputs are clipped, so
// panic instead.
if xi > max {
panic!("Input clipped to maximum, result is unlikely to be correct.");
} else if xi < min {
panic!("Input clipped to minimum, result is unlikely to be correct.");
}
xi as i16
}
/// Generate a full batch of ADC samples starting at `timestamp_start`.
///
/// # Args
/// * `pure_signals` - Pure sinusoidal components of the ADC-sampled signal.
/// * `timestamp_start` - Starting time of ADC-sampled signal in terms of the internal clock count.
/// * `internal_frequency` - Internal clock frequency (in Hz).
/// * `adc_frequency` - ADC sampling frequency (in Hz).
/// * `sample_buffer_size` - The number of ADC samples in one processing batch.
///
/// # Returns
/// The sampled signal at the ADC input.
fn adc_sampled_signal( fn adc_sampled_signal(
pure_signals: &Vec<PureSine>, tones: &Vec<Tone>,
timestamp_start: u64, time_offset: f64,
internal_frequency: f64, sampling_frequency: f64,
adc_frequency: f64,
sample_buffer_size: u32, sample_buffer_size: u32,
) -> Vec<i16> { ) -> Vec<i16> {
// amplitude of each pure signal
let mut amplitude: Vec<f64> = Vec::<f64>::new();
// initial phase value for each pure signal
let mut initial_phase: Vec<f64> = Vec::<f64>::new();
// phase increment at each ADC sample for each pure signal
let mut phase_increment: Vec<f64> = Vec::<f64>::new();
let adc_period = internal_frequency / adc_frequency;
// For each pure sinusoid, compute the amplitude, phase corresponding to the first ADC sample,
// and phase increment for each subsequent ADC sample.
for pure_signal in pure_signals.iter() {
let signal_period = internal_frequency / pure_signal.frequency;
let phase_offset_count =
pure_signal.phase_offset / (2. * PI) * signal_period;
let initial_phase_count =
(phase_offset_count + timestamp_start as f64) % signal_period;
amplitude.push(linear(pure_signal.amplitude_dbfs));
initial_phase.push(2. * PI * initial_phase_count / signal_period);
phase_increment.push(2. * PI * adc_period / signal_period);
}
// Compute the input signal corresponding to each ADC sample by summing the contributions from
// each pure sinusoid.
let mut signal = Vec::<i16>::new(); let mut signal = Vec::<i16>::new();
for i in 0..sample_buffer_size { for i in 0..sample_buffer_size {
signal.push(real_to_adc_sample( let time = 2. * PI * (time_offset + i as f64 / sampling_frequency);
amplitude let x: f64 = tones
.iter() .iter()
.zip(initial_phase.iter()) .map(|&t| {
.zip(phase_increment.iter()) linear(t.amplitude_dbfs) * (t.phase + t.frequency * time).cos()
.fold(0., |acc, ((a, phi), theta)| { })
acc + a * (phi + theta * i as f64).sin() .sum();
}), assert!(-1. < x && x < 1.);
)); signal.push((x * ADC_MAX_COUNT) as i16);
} }
signal signal
} }
@ -235,76 +156,36 @@ fn adc_batch_timestamps(
/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
/// ///
/// # Args /// # Args
/// * `corner_frequency` - Corner frequency, or 3dB cutoff frequency (in Hz). /// * `fc` - Corner frequency, or 3dB cutoff frequency (in Hz).
/// * `sampling_frequency` - Sampling frequency (in Hz). /// * `q` - Quality factor (1/sqrt(2) for critical).
/// * `k` - DC gain.
/// ///
/// # Returns /// # Returns
/// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1. /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1.
fn lowpass_iir_coefficients( fn lowpass_iir_coefficients(fc: f64, q: f64, k: f64) -> IIRState {
corner_frequency: f64, let f = 2. * PI * fc;
sampling_frequency: f64, let a = f.sin() / (2. * q);
) -> [i32; 5] { // IIR uses Q2.30 fixed point
let normalized_angular_frequency: f64 = let a0 = (1. + a) / (1 << IIR::SHIFT) as f64;
2. * PI * corner_frequency / sampling_frequency; let b0 = (k / 2. * (1. - f.cos()) / a0).round() as _;
let quality_factor: f64 = 1. / 2f64.sqrt(); let a1 = (2. * f.cos() / a0).round() as _;
let alpha: f64 = normalized_angular_frequency.sin() / (2. * quality_factor); let a2 = ((a - 1.) / a0).round() as _;
// All b coefficients have been multiplied by a factor of 2 in comparison with the link above in
// order to set the passband gain to 2.
let mut b0: f64 = 1. - normalized_angular_frequency.cos();
let mut b1: f64 = 2. * (1. - normalized_angular_frequency.cos());
let mut b2: f64 = b0;
let a0: f64 = 1. + alpha;
let mut a1: f64 = -2. * normalized_angular_frequency.cos();
let mut a2: f64 = 1. - alpha;
b0 /= a0;
b1 /= a0;
b2 /= a0;
a1 /= -a0;
a2 /= -a0;
// iir uses Q2.30 fixed point IIRState([b0, 2 * b0, b0, a1, a2])
[
(b0 * (1 << 30) as f64).round() as i32,
(b1 * (1 << 30) as f64).round() as i32,
(b2 * (1 << 30) as f64).round() as i32,
(a1 * (1 << 30) as f64).round() as i32,
(a2 * (1 << 30) as f64).round() as i32,
]
}
/// Maximum acceptable error between a computed and actual value given fixed and relative
/// tolerances.
///
/// # Args
/// * `a` - First input.
/// * `b` - Second input. The relative tolerance is computed with respect to the maximum of the
/// absolute values of the first and second inputs.
/// * `rtol` - Relative tolerance.
/// * `atol` - Fixed tolerance.
///
/// # Returns
/// Maximum acceptable error.
fn max_error(a: f64, b: f64, rtol: f64, atol: f64) -> f64 {
rtol * a.abs().max(b.abs()) + atol
}
// TODO this is (mostly) copied from testing.rs.
pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool {
(a - b).abs() <= max_error(a, b, rtol, atol)
} }
/// Total noise amplitude of the input signal after sampling by the ADC. This computes an upper /// Total noise amplitude of the input signal after sampling by the ADC. This computes an upper
/// bound of the total noise amplitude, rather than its actual value. /// bound of the total noise amplitude, rather than its actual value.
/// ///
/// # Args /// # Args
/// * `noise_inputs` - Noise sources at the ADC input. /// * `tones` - Noise sources at the ADC input.
/// * `demodulation_frequency` - Frequency of the demodulation signal (in Hz). /// * `demodulation_frequency` - Frequency of the demodulation signal (in Hz).
/// * `corner_frequency` - Low-pass filter 3dB corner (cutoff) frequency. /// * `corner_frequency` - Low-pass filter 3dB corner (cutoff) frequency.
/// ///
/// # Returns /// # Returns
/// Upper bound of the total amplitude of all noise sources. /// Upper bound of the total amplitude of all noise sources.
fn sampled_noise_amplitude( fn sampled_noise_amplitude(
noise_inputs: &Vec<PureSine>, tones: &Vec<Tone>,
demodulation_frequency: f64, demodulation_frequency: f64,
corner_frequency: f64, corner_frequency: f64,
) -> f64 { ) -> f64 {
@ -315,7 +196,7 @@ fn sampled_noise_amplitude(
// amplitudes of the individual noise sources. We treat this as an upper bound, and use it as an // amplitudes of the individual noise sources. We treat this as an upper bound, and use it as an
// approximation of the actual amplitude. // approximation of the actual amplitude.
let mut noise: f64 = noise_inputs let mut noise: f64 = tones
.iter() .iter()
.map(|n| { .map(|n| {
// Noise inputs create an oscillation at the output, where the oscillation magnitude is // Noise inputs create an oscillation at the output, where the oscillation magnitude is
@ -325,7 +206,7 @@ fn sampled_noise_amplitude(
/ corner_frequency) / corner_frequency)
.log2(); .log2();
// 2nd-order filter. Approximately 12dB/octave rolloff. // 2nd-order filter. Approximately 12dB/octave rolloff.
let attenuation = -2. * 20. * 2_f64.log10() * octaves; let attenuation = -2. * 20. * 2f64.log10() * octaves;
linear(n.amplitude_dbfs + attenuation) linear(n.amplitude_dbfs + attenuation)
}) })
.sum(); .sum();
@ -498,8 +379,8 @@ fn phase_noise(
/// * `pll_shift_frequency` - See `pll::update()`. /// * `pll_shift_frequency` - See `pll::update()`.
/// * `pll_shift_phase` - See `pll::update()`. /// * `pll_shift_phase` - See `pll::update()`.
/// * `corner_frequency` - Lowpass filter 3dB cutoff frequency. /// * `corner_frequency` - Lowpass filter 3dB cutoff frequency.
/// * `desired_input` - `PureSine` giving the frequency, amplitude and phase of the desired result. /// * `desired_input` - `Tone` giving the frequency, amplitude and phase of the desired result.
/// * `noise_inputs` - Vector of `PureSine` for any noise inputs on top of `desired_input`. /// * `noise_inputs` - Vector of `Tone` for any noise inputs on top of `desired_input`.
/// * `time_constant_factor` - Number of time constants after which the output is considered valid. /// * `time_constant_factor` - Number of time constants after which the output is considered valid.
/// * `tolerance` - Acceptable relative tolerance for the magnitude and angle outputs. This is added /// * `tolerance` - Acceptable relative tolerance for the magnitude and angle outputs. This is added
/// to fixed tolerance values computed inside this function. The outputs must remain within this /// to fixed tolerance values computed inside this function. The outputs must remain within this
@ -514,8 +395,8 @@ fn lowpass_test(
pll_shift_frequency: u8, pll_shift_frequency: u8,
pll_shift_phase: u8, pll_shift_phase: u8,
corner_frequency: f64, corner_frequency: f64,
desired_input: PureSine, desired_input: Tone,
noise_inputs: &mut Vec<PureSine>, tones: &mut Vec<Tone>,
time_constant_factor: f64, time_constant_factor: f64,
tolerance: f64, tolerance: f64,
) { ) {
@ -543,7 +424,11 @@ fn lowpass_test(
(demodulation_phase_offset / (2. * PI) * (1_u64 << 32) as f64).round() (demodulation_phase_offset / (2. * PI) * (1_u64 << 32) as f64).round()
as u32, as u32,
IIR { IIR {
ba: lowpass_iir_coefficients(corner_frequency, adc_frequency), ba: lowpass_iir_coefficients(
corner_frequency / adc_frequency,
1. / 2f64.sqrt(),
2.,
),
}, },
); );
let mut timestamp_handler = TimestampHandler::new( let mut timestamp_handler = TimestampHandler::new(
@ -569,14 +454,14 @@ fn lowpass_test(
1_u64 << (adc_sample_ticks_log2 + sample_buffer_size_log2); 1_u64 << (adc_sample_ticks_log2 + sample_buffer_size_log2);
let effective_phase_offset = let effective_phase_offset =
desired_input.phase_offset - demodulation_phase_offset; desired_input.phase - demodulation_phase_offset;
let in_phase_actual = let in_phase_actual =
linear(desired_input.amplitude_dbfs) * effective_phase_offset.cos(); linear(desired_input.amplitude_dbfs) * effective_phase_offset.cos();
let quadrature_actual = let quadrature_actual =
linear(desired_input.amplitude_dbfs) * effective_phase_offset.sin(); linear(desired_input.amplitude_dbfs) * effective_phase_offset.sin();
let total_noise_amplitude = sampled_noise_amplitude( let total_noise_amplitude = sampled_noise_amplitude(
noise_inputs, tones,
reference_frequency * harmonic as f64, reference_frequency * harmonic as f64,
corner_frequency, corner_frequency,
); );
@ -593,14 +478,12 @@ fn lowpass_test(
phase_noise(total_noise_amplitude, in_phase_actual, quadrature_actual) phase_noise(total_noise_amplitude, in_phase_actual, quadrature_actual)
+ 1e-2 * 2. * PI; + 1e-2 * 2. * PI;
let pure_signals = noise_inputs; tones.push(desired_input);
pure_signals.push(desired_input);
for n in 0..(samples + extra_samples) { for n in 0..(samples + extra_samples) {
let adc_signal = adc_sampled_signal( let adc_signal = adc_sampled_signal(
&pure_signals, &tones,
timestamp_start, timestamp_start as f64 / internal_frequency,
internal_frequency,
adc_frequency, adc_frequency,
1 << sample_buffer_size_log2, 1 << sample_buffer_size_log2,
); );
@ -610,19 +493,19 @@ fn lowpass_test(
timestamp_start + batch_sample_count - 1, timestamp_start + batch_sample_count - 1,
internal_frequency, internal_frequency,
); );
timestamp_start += batch_sample_count;
let (demodulation_initial_phase, demodulation_frequency) = let (demodulation_initial_phase, demodulation_frequency) =
timestamp_handler.update(timestamp); timestamp_handler.update(timestamp);
let output = lockin.update(
let (in_phase, quadrature) = lockin.update(
adc_signal, adc_signal,
demodulation_initial_phase, demodulation_initial_phase,
demodulation_frequency, demodulation_frequency,
); );
let magnitude = (((output.0 as i64) * (output.0 as i64)
let magnitude = shift_round(in_phase, 16) * shift_round(in_phase, 16) + (output.1 as i64) * (output.1 as i64))
+ shift_round(quadrature, 16) * shift_round(quadrature, 16); >> 32) as i32;
let phase = atan2(quadrature, in_phase); let phase = atan2(output.1, output.0);
// Ensure stable within tolerance for 1 time constant after `time_constant_factor`. // Ensure stable within tolerance for 1 time constant after `time_constant_factor`.
if n >= samples { if n >= samples {
@ -638,7 +521,7 @@ fn lowpass_test(
linear(desired_input.amplitude_dbfs), linear(desired_input.amplitude_dbfs),
desired_input.amplitude_dbfs, desired_input.amplitude_dbfs,
amplitude_normalized, amplitude_normalized,
dbfs(amplitude_normalized), 20.*amplitude_normalized.log10(),
max_error(linear(desired_input.amplitude_dbfs), amplitude_normalized, tolerance, total_magnitude_noise), max_error(linear(desired_input.amplitude_dbfs), amplitude_normalized, tolerance, total_magnitude_noise),
); );
let phase_normalized = let phase_normalized =
@ -661,8 +544,8 @@ fn lowpass_test(
), ),
); );
let in_phase_normalized = in_phase as f64 / (1 << 30) as f64; let in_phase_normalized = output.0 as f64 / (1 << 30) as f64;
let quadrature_normalized = quadrature as f64 / (1 << 30) as f64; let quadrature_normalized = output.1 as f64 / (1 << 30) as f64;
assert!( assert!(
isclose( isclose(
@ -699,8 +582,6 @@ fn lowpass_test(
), ),
); );
} }
timestamp_start += batch_sample_count;
} }
} }
@ -729,21 +610,21 @@ fn lowpass() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: 0., phase: 0.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.1 * demodulation_frequency, frequency: 1.1 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.9 * demodulation_frequency, frequency: 0.9 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -776,21 +657,21 @@ fn lowpass_demodulation_phase_offset_pi_2() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: 0., phase: 0.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.1 * demodulation_frequency, frequency: 1.1 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.9 * demodulation_frequency, frequency: 0.9 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -823,21 +704,21 @@ fn lowpass_phase_offset_pi_2() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: PI / 2., phase: PI / 2.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.1 * demodulation_frequency, frequency: 1.1 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.9 * demodulation_frequency, frequency: 0.9 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -870,21 +751,21 @@ fn lowpass_fundamental_111e3_phase_offset_pi_4() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: PI / 4., phase: PI / 4.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.1 * demodulation_frequency, frequency: 1.1 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.9 * demodulation_frequency, frequency: 0.9 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -917,21 +798,21 @@ fn lowpass_first_harmonic() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: 0., phase: 0.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.2 * demodulation_frequency, frequency: 1.2 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.8 * demodulation_frequency, frequency: 0.8 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -964,21 +845,21 @@ fn lowpass_second_harmonic() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: 0., phase: 0.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.2 * demodulation_frequency, frequency: 1.2 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.8 * demodulation_frequency, frequency: 0.8 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -1011,21 +892,21 @@ fn lowpass_third_harmonic() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: 0., phase: 0.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.2 * demodulation_frequency, frequency: 1.2 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.8 * demodulation_frequency, frequency: 0.8 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -1058,21 +939,21 @@ fn lowpass_first_harmonic_phase_shift() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: PI / 4., phase: PI / 4.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.2 * demodulation_frequency, frequency: 1.2 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.8 * demodulation_frequency, frequency: 0.8 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -1105,21 +986,21 @@ fn lowpass_adc_frequency_1e6() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: 0., phase: 0.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.2 * demodulation_frequency, frequency: 1.2 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.8 * demodulation_frequency, frequency: 0.8 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -1152,21 +1033,21 @@ fn lowpass_internal_frequency_125e6() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: 0., phase: 0.,
}, },
&mut vec![ &mut vec![
PureSine { Tone {
frequency: 1.2 * demodulation_frequency, frequency: 1.2 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
PureSine { Tone {
frequency: 0.8 * demodulation_frequency, frequency: 0.8 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}, },
], ],
time_constant_factor, time_constant_factor,
@ -1199,15 +1080,15 @@ fn lowpass_low_signal_frequency() {
pll_shift_frequency, pll_shift_frequency,
pll_shift_phase, pll_shift_phase,
corner_frequency, corner_frequency,
PureSine { Tone {
frequency: demodulation_frequency, frequency: demodulation_frequency,
amplitude_dbfs: -30., amplitude_dbfs: -30.,
phase_offset: 0., phase: 0.,
}, },
&mut vec![PureSine { &mut vec![Tone {
frequency: 1.1 * demodulation_frequency, frequency: 1.1 * demodulation_frequency,
amplitude_dbfs: -20., amplitude_dbfs: -20.,
phase_offset: 0., phase: 0.,
}], }],
time_constant_factor, time_constant_factor,
tolerance, tolerance,

View File

@ -94,8 +94,8 @@ use dac::{Dac0Output, Dac1Output};
use dsp::{ use dsp::{
iir, iir_int, iir, iir_int,
reciprocal_pll::TimestampHandler, reciprocal_pll::TimestampHandler,
shift_round,
trig::{atan2, cossin}, trig::{atan2, cossin},
Complex,
}; };
use pounder::DdsOutput; use pounder::DdsOutput;
@ -948,10 +948,8 @@ const APP: () = {
ADC_SAMPLE_TICKS_LOG2 as usize, ADC_SAMPLE_TICKS_LOG2 as usize,
SAMPLE_BUFFER_SIZE_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
); );
let iir_lockin = iir_int::IIR { let iir_lockin = iir_int::IIR::default();
ba: [1, 0, 0, 0, 0], let iir_state_lockin = [iir_int::IIRState::default(); 2];
};
let iir_state_lockin = [[0; 5]; 2];
// Start sampling ADCs. // Start sampling ADCs.
sampling_timer.start(); sampling_timer.start();
@ -995,47 +993,39 @@ const APP: () = {
c.resources.dacs.1.acquire_buffer(), c.resources.dacs.1.acquire_buffer(),
]; ];
let (demodulation_initial_phase, demodulation_frequency) = c
.resources
.timestamp_handler
.update(c.resources.input_stamper.latest_timestamp());
let [dac0, dac1] = dac_samples; let [dac0, dac1] = dac_samples;
let iir_lockin = c.resources.iir_lockin; let iir_lockin = c.resources.iir_lockin;
let iir_state_lockin = c.resources.iir_state_lockin; let iir_state_lockin = c.resources.iir_state_lockin;
let iir_ch = c.resources.iir_ch; let iir_ch = c.resources.iir_ch;
let iir_state = c.resources.iir_state; let iir_state = c.resources.iir_state;
let (pll_phase, pll_frequency) = c
.resources
.timestamp_handler
.update(c.resources.input_stamper.latest_timestamp());
let frequency = pll_frequency.wrapping_mul(HARMONIC);
let mut phase =
PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC));
dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each(
|(i, (d0, d1))| { |(i, (d0, d1))| {
let sample_phase = (HARMONIC.wrapping_mul( let m = cossin(-(phase as i32));
(demodulation_frequency.wrapping_mul(i as u32)) phase = phase.wrapping_add(frequency);
.wrapping_add(demodulation_initial_phase),
))
.wrapping_add(PHASE_OFFSET);
let (cos, sin) = cossin(sample_phase as i32);
let mut signal = (0_i32, 0_i32); let signal = Complex(
iir_lockin.update(
&mut iir_state_lockin[0],
((adc_samples[0][i] as i64 * m.0 as i64) >> 16) as i32,
),
iir_lockin.update(
&mut iir_state_lockin[1],
((adc_samples[0][i] as i64 * m.1 as i64) >> 16) as i32,
),
);
// shift cos/sin before multiplying to avoid i64 multiplication let mut magnitude =
signal.0 = (signal.0 * signal.0 + signal.1 * signal.1) as f32;
adc_samples[0][i] as i16 as i32 * shift_round(sin, 16); let mut phase = atan2(signal.1, signal.0) as f32;
signal.1 =
adc_samples[0][i] as i16 as i32 * shift_round(cos, 16);
signal.0 =
iir_lockin.update(&mut iir_state_lockin[0], signal.0);
signal.1 =
iir_lockin.update(&mut iir_state_lockin[1], signal.1);
let mut magnitude = f32::from(shift_round(
signal.0 * signal.0 + signal.1 * signal.1,
16,
) as i16);
let mut phase = f32::from(shift_round(
atan2(signal.1, signal.0),
16,
) as i16);
for j in 0..iir_state[0].len() { for j in 0..iir_state[0].len() {
magnitude = magnitude =