dsp: replace in_phase and quadrature with Complex

This commit is contained in:
Matt Huszagh 2020-11-28 16:04:22 -08:00
parent fcdfcb0be7
commit d1b7efad48
4 changed files with 201 additions and 189 deletions

21
dsp/src/complex.rs Normal file
View File

@ -0,0 +1,21 @@
use core::cmp::PartialEq;
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct Complex {
pub re: f32,
pub im: f32,
}
impl Complex {
pub fn new(re: f32, im: f32) -> Self {
Complex { re: re, im: im }
}
pub fn arg(&self) -> f32 {
libm::atan2f(self.im, self.re)
}
pub fn abs(&self) -> f32 {
libm::sqrtf([self.re, self.im].iter().map(|i| i * i).sum())
}
}

View File

@ -1,6 +1,7 @@
#![cfg_attr(not(test), no_std)] #![cfg_attr(not(test), no_std)]
#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))]
pub mod complex;
pub mod iir; pub mod iir;
pub mod lockin; pub mod lockin;
pub mod pll; pub mod pll;

View File

@ -2,25 +2,15 @@
//! //!
//! Lock-in processing is performed through a combination of the //! Lock-in processing is performed through a combination of the
//! following modular processing blocks: demodulation, filtering, //! following modular processing blocks: demodulation, filtering,
//! decimation and computing the magnitude and phase from the in-phase //! decimation and computing the magnitude and phase from a complex
//! and quadrature signals. These processing blocks are mutually //! signal. These processing blocks are mutually independent.
//! independent.
//! //!
//! # Terminology //! # Terminology
//! //!
//! * _demodulation signal_ - A copy of the reference signal that is //! * _demodulation signal_ - A copy of the reference signal that is
//! optionally frequency scaled and phase shifted. There are two //! optionally frequency scaled and phase shifted. This is a complex
//! copies of this signal. The first copy is in-phase with the //! signal. The demodulation signals are used to demodulate the ADC
//! reference signal (before any optional phase shifting). The second
//! is 90 degrees out of phase (in quadrature) with the first
//! copy. The demodulation signals are used to demodulate the ADC
//! sampled signal. //! sampled signal.
//! * _in-phase_ and _quadrature_ - These terms are used to delineate
//! between the two components of the demodulation signal and the
//! resulting two signals at any step downstream of the demodulation
//! step. The in-phase signal is in-phase with the reference signal
//! prior to any phase shifts. The quadrature signal is 90 degrees out
//! of phase with the in-phase signal.
//! * _internal clock_ - A fast internal clock used to increment a //! * _internal clock_ - A fast internal clock used to increment a
//! counter for determining the 0-phase points of a reference signal. //! counter for determining the 0-phase points of a reference signal.
//! * _reference signal_ - A constant-frequency signal used to derive //! * _reference signal_ - A constant-frequency signal used to derive
@ -44,24 +34,25 @@
//! //!
//! * `demodulate` - Computes the phase of the demodulation signal //! * `demodulate` - Computes the phase of the demodulation signal
//! corresponding to each ADC sample, uses this phase to compute the //! corresponding to each ADC sample, uses this phase to compute the
//! in-phase and quadrature demodulation signals, and multiplies these //! demodulation signal, and multiplies this demodulation signal by
//! demodulation signals by the ADC-sampled signal. This is a method //! the ADC-sampled signal. This is a method of `Lockin` since it
//! of `Lockin` since it requires information about how to modify the //! requires information about how to modify the reference signal for
//! reference signal for demodulation. //! demodulation.
//! * `filter` - Performs IIR biquad filtering of in-phase and //! * `filter` - Performs IIR biquad filtering of a complex
//! quadrature signals. This is commonly performed on the in-phase and //! signals. This is commonly performed on the signal provided by the
//! quadrature components provided by the demodulation step, but can //! demodulation step, but can be performed at any other point in the
//! be performed at any other point in the processing chain or omitted //! processing chain or omitted entirely. `filter` is a method of
//! entirely. `filter` is a method of `Lockin` since it must hold onto //! `Lockin` since it must hold onto the filter configuration and
//! the filter configuration and state. //! state.
//! * `decimate` - This decimates the in-phase and quadrature signals //! * `decimate` - This decimates a signal to reduce the load on the
//! to reduce the load on the DAC output. It does not require any //! DAC output. It does not require any state information and is
//! state information and is therefore a normal function. //! therefore a normal function.
//! * `magnitude_phase` - Computes the magnitude and phase of the //! * `magnitude_phase` - Computes the magnitude and phase of the
//! component of the ADC-sampled signal whose frequency is equal to //! component of the ADC-sampled signal whose frequency is equal to
//! the demodulation frequency. This does not require any state //! the demodulation frequency. This does not require any state
//! information and is therefore a normal function. //! information and is therefore a normal function.
use super::complex::Complex;
use super::iir::{IIRState, IIR}; use super::iir::{IIRState, IIR};
use core::f32::consts::PI; use core::f32::consts::PI;
@ -139,8 +130,7 @@ impl Lockin {
} }
} }
/// Demodulate an input signal with in-phase and quadrature /// Demodulate an input signal with the complex reference signal.
/// reference signals.
/// ///
/// # Arguments /// # Arguments
/// ///
@ -151,9 +141,9 @@ impl Lockin {
/// ///
/// # Returns /// # Returns
/// ///
/// The demodulated in-phase and quadrature signals as an /// The demodulated complex signal as a `Result`. When there are
/// `Option`. When there are an insufficient number of timestamps /// an insufficient number of timestamps to perform processing,
/// to perform processing, `None` is returned. /// `Err` is returned.
/// ///
/// # Assumptions /// # Assumptions
/// ///
@ -165,10 +155,7 @@ impl Lockin {
&mut self, &mut self,
adc_samples: &[i16], adc_samples: &[i16],
timestamps: &[u16], timestamps: &[u16],
) -> Result< ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> {
([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE]),
&str,
> {
// update old timestamps for new ADC batch // update old timestamps for new ADC batch
let sample_period = self.sample_period as i32; let sample_period = self.sample_period as i32;
self.timestamps.iter_mut().for_each(|t| match *t { self.timestamps.iter_mut().for_each(|t| match *t {
@ -200,14 +187,12 @@ impl Lockin {
// compute ADC sample phases, sines/cosines and demodulate // compute ADC sample phases, sines/cosines and demodulate
let reference_period = let reference_period =
self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); self.timestamps[0].unwrap() - self.timestamps[1].unwrap();
let mut in_phase = [0f32; ADC_SAMPLE_BUFFER_SIZE]; let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE];
let mut quadrature = [0f32; ADC_SAMPLE_BUFFER_SIZE]; signal
in_phase
.iter_mut() .iter_mut()
.zip(quadrature.iter_mut())
.zip(adc_samples.iter()) .zip(adc_samples.iter())
.enumerate() .enumerate()
.for_each(|(n, ((i, q), sample))| { .for_each(|(n, (s, sample))| {
let integer_phase: i32 = (n as i32 * self.sample_period as i32 let integer_phase: i32 = (n as i32 * self.sample_period as i32
- self.timestamps[0].unwrap()) - self.timestamps[0].unwrap())
* self.harmonic as i32; * self.harmonic as i32;
@ -215,104 +200,90 @@ impl Lockin {
+ 2. * PI * integer_phase as f32 / reference_period as f32; + 2. * PI * integer_phase as f32 / reference_period as f32;
let (sine, cosine) = libm::sincosf(phase); let (sine, cosine) = libm::sincosf(phase);
let sample = *sample as f32; let sample = *sample as f32;
*i = sine * sample; s.re = sine * sample;
*q = cosine * sample; s.im = cosine * sample;
}); });
Ok((in_phase, quadrature)) Ok(signal)
} }
/// Filter the in-phase and quadrature signals using the supplied /// Filter the complex signal using the supplied biquad IIR. The
/// biquad IIR. The signal arrays are modified in place. /// signal array is modified in place.
/// ///
/// # Arguments /// # Arguments
/// ///
/// * `in_phase` - In-phase signal. /// * `signal` - Complex signal to filter.
/// * `quadrature` - Quadrature signal. pub fn filter(&mut self, signal: &mut [Complex]) {
pub fn filter(&mut self, in_phase: &mut [f32], quadrature: &mut [f32]) { signal.iter_mut().for_each(|s| {
in_phase s.re = self.iir.update(&mut self.iirstate[0], s.re);
.iter_mut() s.im = self.iir.update(&mut self.iirstate[1], s.im);
.zip(quadrature.iter_mut())
.for_each(|(i, q)| {
*i = self.iir.update(&mut self.iirstate[0], *i);
*q = self.iir.update(&mut self.iirstate[1], *q);
}); });
} }
} }
/// Decimate the in-phase and quadrature signals to /// Decimate the complex signal to `DECIMATED_BUFFER_SIZE`. The ratio
/// `DECIMATED_BUFFER_SIZE`. The ratio of `ADC_SAMPLE_BUFFER_SIZE` to /// of `ADC_SAMPLE_BUFFER_SIZE` to `DECIMATED_BUFFER_SIZE` must be a
/// `DECIMATED_BUFFER_SIZE` must be a power of 2. /// power of 2.
/// ///
/// # Arguments /// # Arguments
/// ///
/// * `in_phase` - In-phase signal. /// * `signal` - Complex signal to decimate.
/// * `quadrature` - Quadrature signal.
/// ///
/// # Returns /// # Returns
/// ///
/// The decimated in-phase and quadrature signals. /// The decimated signal.
pub fn decimate( pub fn decimate(
in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE], signal: [Complex; ADC_SAMPLE_BUFFER_SIZE],
quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE], ) -> [Complex; DECIMATED_BUFFER_SIZE] {
) -> ([f32; DECIMATED_BUFFER_SIZE], [f32; DECIMATED_BUFFER_SIZE]) {
let n_k = ADC_SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE; let n_k = ADC_SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE;
debug_assert!( debug_assert!(
ADC_SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0 ADC_SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0
); );
let mut in_phase_decimated = [0f32; DECIMATED_BUFFER_SIZE]; let mut signal_decimated = [Complex::new(0., 0.); DECIMATED_BUFFER_SIZE];
let mut quadrature_decimated = [0f32; DECIMATED_BUFFER_SIZE];
in_phase_decimated signal_decimated
.iter_mut() .iter_mut()
.zip(quadrature_decimated.iter_mut()) .zip(signal.iter().step_by(n_k))
.zip( .for_each(|(s_d, s)| {
in_phase s_d.re = s.re;
.iter() s_d.im = s.im;
.step_by(n_k)
.zip(quadrature.iter().step_by(n_k)),
)
.for_each(|((i_d, q_d), (i, q))| {
*i_d = *i;
*q_d = *q;
}); });
(in_phase_decimated, quadrature_decimated) signal_decimated
} }
/// Compute the magnitude and phase from the in-phase and quadrature /// Compute the magnitude and phase from the complex signal. The
/// signals. The in-phase and quadrature arrays are modified in place. /// signal array is modified in place.
/// ///
/// # Arguments /// # Arguments
/// ///
/// * `in_phase` - In-phase signal. /// * `signal` - Complex signal to decimate.
/// * `quadrature` - Quadrature signal. pub fn magnitude_phase(signal: &mut [Complex]) {
pub fn magnitude_phase(in_phase: &mut [f32], quadrature: &mut [f32]) { signal.iter_mut().for_each(|s| {
in_phase let new_i = s.abs();
.iter_mut() let new_q = s.arg();
.zip(quadrature.iter_mut()) s.re = new_i;
.for_each(|(i, q)| { s.im = new_q;
let new_i = libm::sqrtf([*i, *q].iter().map(|i| i * i).sum());
let new_q = libm::atan2f(*q, *i);
*i = new_i;
*q = new_q;
}); });
} }
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use super::*; use super::*;
extern crate std;
fn f32_is_close(a: f32, b: f32) -> bool { fn f32_is_close(a: f32, b: f32) -> bool {
(a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON
} }
fn f32_array_is_close(a: &[f32], b: &[f32]) -> bool { fn complex_is_close(a: Complex, b: Complex) -> bool {
f32_is_close(a.re, b.re) && f32_is_close(a.im, b.im)
}
fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool {
let mut result: bool = true; let mut result: bool = true;
a.iter().zip(b.iter()).for_each(|(i, j)| { a.iter().zip(b.iter()).for_each(|(i, j)| {
result &= f32_is_close(*i, *j); result &= complex_is_close(*i, *j);
}); });
result result
} }
@ -327,16 +298,30 @@ mod tests {
<= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance
} }
fn array_within_tolerance( fn complex_within_tolerance(
a: &[f32], a: Complex,
b: &[f32], b: Complex,
relative_tolerance: f32,
fixed_tolerance: f32,
) -> bool {
within_tolerance(a.re, b.re, relative_tolerance, fixed_tolerance)
&& within_tolerance(a.im, b.im, relative_tolerance, fixed_tolerance)
}
fn complex_array_within_tolerance(
a: &[Complex],
b: &[Complex],
relative_tolerance: f32, relative_tolerance: f32,
fixed_tolerance: f32, fixed_tolerance: f32,
) -> bool { ) -> bool {
let mut result: bool = true; let mut result: bool = true;
a.iter().zip(b.iter()).for_each(|(i, j)| { a.iter().zip(b.iter()).for_each(|(i, j)| {
result &= result &= complex_within_tolerance(
within_tolerance(*i, *j, relative_tolerance, fixed_tolerance); *i,
*j,
relative_tolerance,
fixed_tolerance,
);
}); });
result result
} }
@ -354,75 +339,93 @@ mod tests {
#[test] #[test]
fn magnitude_phase_length_1_quadrant_1() { fn magnitude_phase_length_1_quadrant_1() {
let mut in_phase: [f32; 1] = [1.]; let mut signal: [Complex; 1] = [Complex::new(1., 1.)];
let mut quadrature: [f32; 1] = [1.]; magnitude_phase(&mut signal);
magnitude_phase(&mut in_phase, &mut quadrature); assert!(complex_array_is_close(
assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); &signal,
assert!(f32_array_is_close(&quadrature, &[PI / 4.])); &[Complex::new(2_f32.sqrt(), PI / 4.)]
));
in_phase = [3_f32.sqrt() / 2.]; signal = [Complex::new(3_f32.sqrt() / 2., 1. / 2.)];
quadrature = [1. / 2.]; magnitude_phase(&mut signal);
magnitude_phase(&mut in_phase, &mut quadrature); assert!(complex_array_is_close(
assert!(f32_array_is_close(&in_phase, &[1_f32])); &signal,
assert!(f32_array_is_close(&quadrature, &[PI / 6.])); &[Complex::new(1., PI / 6.)]
));
} }
#[test] #[test]
fn magnitude_phase_length_1_quadrant_2() { fn magnitude_phase_length_1_quadrant_2() {
let mut in_phase: [f32; 1] = [-1.]; let mut signal = [Complex::new(-1., 1.)];
let mut quadrature: [f32; 1] = [1.]; magnitude_phase(&mut signal);
magnitude_phase(&mut in_phase, &mut quadrature); assert!(complex_array_is_close(
assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); &signal,
assert!(f32_array_is_close(&quadrature, &[3. * PI / 4.])); &[Complex::new(2_f32.sqrt(), 3. * PI / 4.)]
));
in_phase = [-1. / 2.]; signal = [Complex::new(-1. / 2., 3_f32.sqrt() / 2.)];
quadrature = [3_f32.sqrt() / 2.]; magnitude_phase(&mut signal);
magnitude_phase(&mut in_phase, &mut quadrature); assert!(complex_array_is_close(
assert!(f32_array_is_close(&in_phase, &[1_f32])); &signal,
assert!(f32_array_is_close(&quadrature, &[2. * PI / 3.])); &[Complex::new(1_f32, 2. * PI / 3.)]
));
} }
#[test] #[test]
fn magnitude_phase_length_1_quadrant_3() { fn magnitude_phase_length_1_quadrant_3() {
let mut in_phase: [f32; 1] = [-1. / 2_f32.sqrt()]; let mut signal = [Complex::new(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())];
let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; magnitude_phase(&mut signal);
magnitude_phase(&mut in_phase, &mut quadrature); assert!(complex_array_is_close(
assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); &signal,
assert!(f32_array_is_close(&quadrature, &[-3. * PI / 4.])); &[Complex::new(1_f32.sqrt(), -3. * PI / 4.)]
));
in_phase = [-1. / 2.]; signal = [Complex::new(-1. / 2., -2_f32.sqrt())];
quadrature = [-2_f32.sqrt()]; magnitude_phase(&mut signal);
magnitude_phase(&mut in_phase, &mut quadrature); assert!(complex_array_is_close(
assert!(f32_array_is_close(&in_phase, &[(3. / 2.) as f32])); &signal,
assert!(f32_array_is_close(&quadrature, &[-1.91063323625 as f32])); &[Complex::new((3. / 2.) as f32, -1.91063323625 as f32)]
));
} }
#[test] #[test]
fn magnitude_phase_length_1_quadrant_4() { fn magnitude_phase_length_1_quadrant_4() {
let mut in_phase: [f32; 1] = [1. / 2_f32.sqrt()]; let mut signal = [Complex::new(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())];
let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; magnitude_phase(&mut signal);
magnitude_phase(&mut in_phase, &mut quadrature); assert!(complex_array_is_close(
assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); &signal,
assert!(f32_array_is_close(&quadrature, &[-1. * PI / 4.])); &[Complex::new(1_f32.sqrt(), -1. * PI / 4.)]
));
in_phase = [3_f32.sqrt() / 2.]; signal = [Complex::new(3_f32.sqrt() / 2., -1. / 2.)];
quadrature = [-1. / 2.]; magnitude_phase(&mut signal);
magnitude_phase(&mut in_phase, &mut quadrature); assert!(complex_array_is_close(
assert!(f32_array_is_close(&in_phase, &[1_f32])); &signal,
assert!(f32_array_is_close(&quadrature, &[-PI / 6.])); &[Complex::new(1_f32, -PI / 6.)]
));
} }
#[test] #[test]
fn decimate_sample_16_decimated_1() { fn decimate_sample_16_decimated_1() {
let in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [
0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, Complex::new(0.0, 1.6),
1.3, 1.4, 1.5, Complex::new(0.1, 1.7),
Complex::new(0.2, 1.8),
Complex::new(0.3, 1.9),
Complex::new(0.4, 2.0),
Complex::new(0.5, 2.1),
Complex::new(0.6, 2.2),
Complex::new(0.7, 2.3),
Complex::new(0.8, 2.4),
Complex::new(0.9, 2.5),
Complex::new(1.0, 2.6),
Complex::new(1.1, 2.7),
Complex::new(1.2, 2.8),
Complex::new(1.3, 2.9),
Complex::new(1.4, 3.0),
Complex::new(1.5, 3.1),
]; ];
let quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ assert_eq!(decimate(signal), [Complex::new(0.0, 1.6)]);
1.6, 1.7, 1.8, 1.9, 2.0, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8,
2.9, 3.0, 3.1,
];
assert_eq!(decimate(in_phase, quadrature), ([0.0], [1.6]));
} }
#[test] #[test]
@ -439,7 +442,7 @@ mod tests {
}, },
); );
assert_eq!( assert_eq!(
lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[],), lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[]),
Err("insufficient timestamps") Err("insufficient timestamps")
); );
} }
@ -489,32 +492,20 @@ mod tests {
-(initial_phase_integer as f32) / reference_period as f32 * 2. * PI; -(initial_phase_integer as f32) / reference_period as f32 * 2. * PI;
let phase_increment: f32 = let phase_increment: f32 =
adc_period as f32 / reference_period as f32 * 2. * PI; adc_period as f32 / reference_period as f32 * 2. * PI;
let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE];
[0.; ADC_SAMPLE_BUFFER_SIZE]; for (n, s) in signal.iter_mut().enumerate() {
let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] =
[0.; ADC_SAMPLE_BUFFER_SIZE];
for (n, (i, q)) in
in_phase.iter_mut().zip(quadrature.iter_mut()).enumerate()
{
let adc_phase = initial_phase + n as f32 * phase_increment; let adc_phase = initial_phase + n as f32 * phase_increment;
let sine = adc_phase.sin(); let sine = adc_phase.sin();
let cosine = adc_phase.cos(); let cosine = adc_phase.cos();
*i = sine * adc_samples[n] as f32; s.re = sine * adc_samples[n] as f32;
*q = cosine * adc_samples[n] as f32; s.im = cosine * adc_samples[n] as f32;
} }
let (result_in_phase, result_quadrature) = let result = lockin.demodulate(&adc_samples, timestamps).unwrap();
lockin.demodulate(&adc_samples, timestamps).unwrap();
assert!( assert!(
array_within_tolerance(&result_in_phase, &in_phase, 0., 1e-5), complex_array_within_tolerance(&result, &signal, 0., 1e-5),
"\nin_phase computed: {:?},\nin_phase expected: {:?}", "\nsignal computed: {:?},\nsignal expected: {:?}",
result_in_phase, result,
in_phase signal
);
assert!(
array_within_tolerance(&result_quadrature, &quadrature, 0., 1e-5),
"\nquadrature computed: {:?},\nquadrature expected: {:?}",
result_quadrature,
quadrature
); );
} }
} }

View File

@ -1,3 +1,4 @@
use dsp::complex::Complex;
use dsp::iir::IIR; use dsp::iir::IIR;
use dsp::lockin::{ use dsp::lockin::{
decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE,
@ -582,7 +583,7 @@ fn lowpass_test(
pure_signals.push(desired_input); pure_signals.push(desired_input);
for n in 0..(samples + extra_samples) { for n in 0..(samples + extra_samples) {
let signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( let adc_signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal(
&pure_signals, &pure_signals,
timestamp_start, timestamp_start,
internal_frequency, internal_frequency,
@ -597,33 +598,31 @@ fn lowpass_test(
internal_frequency, internal_frequency,
); );
let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE]; let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE];
let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE]; match lockin.demodulate(&adc_signal, timestamps) {
match lockin.demodulate(&signal, timestamps) { Ok(s) => {
Ok((i, q)) => { signal = s;
in_phase = i;
quadrature = q;
} }
Err(_) => { Err(_) => {
continue; continue;
} }
} }
lockin.filter(&mut in_phase, &mut quadrature); lockin.filter(&mut signal);
let (in_phase_decimated, quadrature_decimated) = let signal_decimated = decimate(signal);
decimate(in_phase, quadrature);
let mut magnitude_decimated = in_phase_decimated.clone(); let mut magnitude_phase_decimated = signal.clone();
let mut phase_decimated = quadrature_decimated.clone(); // let mut magnitude_decimated = in_phase_decimated.clone();
// let mut phase_decimated = quadrature_decimated.clone();
magnitude_phase(&mut magnitude_decimated, &mut phase_decimated); magnitude_phase(&mut magnitude_phase_decimated);
// Ensure stable within tolerance for 1 time constant after // Ensure stable within tolerance for 1 time constant after
// `time_constant_factor`. // `time_constant_factor`.
if n >= samples { if n >= samples {
for k in 0..DECIMATED_BUFFER_SIZE { for k in 0..DECIMATED_BUFFER_SIZE {
let amplitude_normalized: f32 = let amplitude_normalized: f32 =
magnitude_decimated[k] / ADC_MAX_COUNT as f32; magnitude_phase_decimated[k].re / ADC_MAX_COUNT as f32;
assert!( assert!(
tolerance_check(linear(desired_input.amplitude_dbfs) as f32, amplitude_normalized, total_magnitude_noise as f32, tolerance), tolerance_check(linear(desired_input.amplitude_dbfs) as f32, amplitude_normalized, total_magnitude_noise as f32, tolerance),
"magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}", "magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}",
@ -636,13 +635,13 @@ fn lowpass_test(
assert!( assert!(
tolerance_check( tolerance_check(
effective_phase_offset as f32, effective_phase_offset as f32,
phase_decimated[k], magnitude_phase_decimated[k].im,
total_phase_noise as f32, total_phase_noise as f32,
tolerance tolerance
), ),
"phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}",
effective_phase_offset as f32, effective_phase_offset as f32,
phase_decimated[k], magnitude_phase_decimated[k].im,
max_error( max_error(
effective_phase_offset as f32, effective_phase_offset as f32,
total_phase_noise as f32, total_phase_noise as f32,
@ -651,9 +650,9 @@ fn lowpass_test(
); );
let in_phase_normalized: f32 = let in_phase_normalized: f32 =
in_phase_decimated[k] / ADC_MAX_COUNT as f32; signal_decimated[k].re / ADC_MAX_COUNT as f32;
let quadrature_normalized: f32 = let quadrature_normalized: f32 =
quadrature_decimated[k] / ADC_MAX_COUNT as f32; signal_decimated[k].im / ADC_MAX_COUNT as f32;
assert!( assert!(
tolerance_check( tolerance_check(
in_phase_actual as f32, in_phase_actual as f32,