dsp: replace in_phase and quadrature with Complex
This commit is contained in:
Matt Huszagh
parent
fcdfcb0be7
commit
d1b7efad48
21
dsp/src/complex.rs
Normal file
21
dsp/src/complex.rs
Normal 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())
|
||||
}
|
||||
}
|
||||
@ -1,6 +1,7 @@
|
||||
#![cfg_attr(not(test), no_std)]
|
||||
#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))]
|
||||
|
||||
pub mod complex;
|
||||
pub mod iir;
|
||||
pub mod lockin;
|
||||
pub mod pll;
|
||||
|
||||
@ -2,25 +2,15 @@
|
||||
//!
|
||||
//! Lock-in processing is performed through a combination of the
|
||||
//! following modular processing blocks: demodulation, filtering,
|
||||
//! decimation and computing the magnitude and phase from the in-phase
|
||||
//! and quadrature signals. These processing blocks are mutually
|
||||
//! independent.
|
||||
//! decimation and computing the magnitude and phase from a complex
|
||||
//! signal. These processing blocks are mutually independent.
|
||||
//!
|
||||
//! # Terminology
|
||||
//!
|
||||
//! * _demodulation signal_ - A copy of the reference signal that is
|
||||
//! optionally frequency scaled and phase shifted. There are two
|
||||
//! copies of this signal. The first copy is in-phase with the
|
||||
//! 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
|
||||
//! optionally frequency scaled and phase shifted. This is a complex
|
||||
//! signal. The demodulation signals are used to demodulate the ADC
|
||||
//! 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
|
||||
//! counter for determining the 0-phase points of a reference signal.
|
||||
//! * _reference signal_ - A constant-frequency signal used to derive
|
||||
@ -44,24 +34,25 @@
|
||||
//!
|
||||
//! * `demodulate` - Computes the phase of the demodulation signal
|
||||
//! corresponding to each ADC sample, uses this phase to compute the
|
||||
//! in-phase and quadrature demodulation signals, and multiplies these
|
||||
//! demodulation signals by the ADC-sampled signal. This is a method
|
||||
//! of `Lockin` since it requires information about how to modify the
|
||||
//! reference signal for demodulation.
|
||||
//! * `filter` - Performs IIR biquad filtering of in-phase and
|
||||
//! quadrature signals. This is commonly performed on the in-phase and
|
||||
//! quadrature components provided by the demodulation step, but can
|
||||
//! be performed at any other point in the processing chain or omitted
|
||||
//! entirely. `filter` is a method of `Lockin` since it must hold onto
|
||||
//! the filter configuration and state.
|
||||
//! * `decimate` - This decimates the in-phase and quadrature signals
|
||||
//! to reduce the load on the DAC output. It does not require any
|
||||
//! state information and is therefore a normal function.
|
||||
//! demodulation signal, and multiplies this demodulation signal by
|
||||
//! the ADC-sampled signal. This is a method of `Lockin` since it
|
||||
//! requires information about how to modify the reference signal for
|
||||
//! demodulation.
|
||||
//! * `filter` - Performs IIR biquad filtering of a complex
|
||||
//! signals. This is commonly performed on the signal provided by the
|
||||
//! demodulation step, but can be performed at any other point in the
|
||||
//! processing chain or omitted entirely. `filter` is a method of
|
||||
//! `Lockin` since it must hold onto the filter configuration and
|
||||
//! state.
|
||||
//! * `decimate` - This decimates a signal to reduce the load on the
|
||||
//! DAC output. It does not require any state information and is
|
||||
//! therefore a normal function.
|
||||
//! * `magnitude_phase` - Computes the magnitude and phase of the
|
||||
//! component of the ADC-sampled signal whose frequency is equal to
|
||||
//! the demodulation frequency. This does not require any state
|
||||
//! information and is therefore a normal function.
|
||||
|
||||
use super::complex::Complex;
|
||||
use super::iir::{IIRState, IIR};
|
||||
use core::f32::consts::PI;
|
||||
|
||||
@ -139,8 +130,7 @@ impl Lockin {
|
||||
}
|
||||
}
|
||||
|
||||
/// Demodulate an input signal with in-phase and quadrature
|
||||
/// reference signals.
|
||||
/// Demodulate an input signal with the complex reference signal.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
@ -151,9 +141,9 @@ impl Lockin {
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The demodulated in-phase and quadrature signals as an
|
||||
/// `Option`. When there are an insufficient number of timestamps
|
||||
/// to perform processing, `None` is returned.
|
||||
/// The demodulated complex signal as a `Result`. When there are
|
||||
/// an insufficient number of timestamps to perform processing,
|
||||
/// `Err` is returned.
|
||||
///
|
||||
/// # Assumptions
|
||||
///
|
||||
@ -165,10 +155,7 @@ impl Lockin {
|
||||
&mut self,
|
||||
adc_samples: &[i16],
|
||||
timestamps: &[u16],
|
||||
) -> Result<
|
||||
([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE]),
|
||||
&str,
|
||||
> {
|
||||
) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> {
|
||||
// update old timestamps for new ADC batch
|
||||
let sample_period = self.sample_period as i32;
|
||||
self.timestamps.iter_mut().for_each(|t| match *t {
|
||||
@ -200,14 +187,12 @@ impl Lockin {
|
||||
// compute ADC sample phases, sines/cosines and demodulate
|
||||
let reference_period =
|
||||
self.timestamps[0].unwrap() - self.timestamps[1].unwrap();
|
||||
let mut in_phase = [0f32; ADC_SAMPLE_BUFFER_SIZE];
|
||||
let mut quadrature = [0f32; ADC_SAMPLE_BUFFER_SIZE];
|
||||
in_phase
|
||||
let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE];
|
||||
signal
|
||||
.iter_mut()
|
||||
.zip(quadrature.iter_mut())
|
||||
.zip(adc_samples.iter())
|
||||
.enumerate()
|
||||
.for_each(|(n, ((i, q), sample))| {
|
||||
.for_each(|(n, (s, sample))| {
|
||||
let integer_phase: i32 = (n as i32 * self.sample_period as i32
|
||||
- self.timestamps[0].unwrap())
|
||||
* self.harmonic as i32;
|
||||
@ -215,104 +200,90 @@ impl Lockin {
|
||||
+ 2. * PI * integer_phase as f32 / reference_period as f32;
|
||||
let (sine, cosine) = libm::sincosf(phase);
|
||||
let sample = *sample as f32;
|
||||
*i = sine * sample;
|
||||
*q = cosine * sample;
|
||||
s.re = sine * sample;
|
||||
s.im = cosine * sample;
|
||||
});
|
||||
|
||||
Ok((in_phase, quadrature))
|
||||
Ok(signal)
|
||||
}
|
||||
|
||||
/// Filter the in-phase and quadrature signals using the supplied
|
||||
/// biquad IIR. The signal arrays are modified in place.
|
||||
/// Filter the complex signal using the supplied biquad IIR. The
|
||||
/// signal array is modified in place.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `in_phase` - In-phase signal.
|
||||
/// * `quadrature` - Quadrature signal.
|
||||
pub fn filter(&mut self, in_phase: &mut [f32], quadrature: &mut [f32]) {
|
||||
in_phase
|
||||
.iter_mut()
|
||||
.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);
|
||||
/// * `signal` - Complex signal to filter.
|
||||
pub fn filter(&mut self, signal: &mut [Complex]) {
|
||||
signal.iter_mut().for_each(|s| {
|
||||
s.re = self.iir.update(&mut self.iirstate[0], s.re);
|
||||
s.im = self.iir.update(&mut self.iirstate[1], s.im);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
/// Decimate the in-phase and quadrature signals to
|
||||
/// `DECIMATED_BUFFER_SIZE`. The ratio of `ADC_SAMPLE_BUFFER_SIZE` to
|
||||
/// `DECIMATED_BUFFER_SIZE` must be a power of 2.
|
||||
/// Decimate the complex signal to `DECIMATED_BUFFER_SIZE`. The ratio
|
||||
/// of `ADC_SAMPLE_BUFFER_SIZE` to `DECIMATED_BUFFER_SIZE` must be a
|
||||
/// power of 2.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `in_phase` - In-phase signal.
|
||||
/// * `quadrature` - Quadrature signal.
|
||||
/// * `signal` - Complex signal to decimate.
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The decimated in-phase and quadrature signals.
|
||||
/// The decimated signal.
|
||||
pub fn decimate(
|
||||
in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE],
|
||||
quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE],
|
||||
) -> ([f32; DECIMATED_BUFFER_SIZE], [f32; DECIMATED_BUFFER_SIZE]) {
|
||||
signal: [Complex; ADC_SAMPLE_BUFFER_SIZE],
|
||||
) -> [Complex; DECIMATED_BUFFER_SIZE] {
|
||||
let n_k = ADC_SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE;
|
||||
debug_assert!(
|
||||
ADC_SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0
|
||||
);
|
||||
|
||||
let mut in_phase_decimated = [0f32; DECIMATED_BUFFER_SIZE];
|
||||
let mut quadrature_decimated = [0f32; DECIMATED_BUFFER_SIZE];
|
||||
let mut signal_decimated = [Complex::new(0., 0.); DECIMATED_BUFFER_SIZE];
|
||||
|
||||
in_phase_decimated
|
||||
signal_decimated
|
||||
.iter_mut()
|
||||
.zip(quadrature_decimated.iter_mut())
|
||||
.zip(
|
||||
in_phase
|
||||
.iter()
|
||||
.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;
|
||||
.zip(signal.iter().step_by(n_k))
|
||||
.for_each(|(s_d, s)| {
|
||||
s_d.re = s.re;
|
||||
s_d.im = s.im;
|
||||
});
|
||||
|
||||
(in_phase_decimated, quadrature_decimated)
|
||||
signal_decimated
|
||||
}
|
||||
|
||||
/// Compute the magnitude and phase from the in-phase and quadrature
|
||||
/// signals. The in-phase and quadrature arrays are modified in place.
|
||||
/// Compute the magnitude and phase from the complex signal. The
|
||||
/// signal array is modified in place.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `in_phase` - In-phase signal.
|
||||
/// * `quadrature` - Quadrature signal.
|
||||
pub fn magnitude_phase(in_phase: &mut [f32], quadrature: &mut [f32]) {
|
||||
in_phase
|
||||
.iter_mut()
|
||||
.zip(quadrature.iter_mut())
|
||||
.for_each(|(i, 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;
|
||||
/// * `signal` - Complex signal to decimate.
|
||||
pub fn magnitude_phase(signal: &mut [Complex]) {
|
||||
signal.iter_mut().for_each(|s| {
|
||||
let new_i = s.abs();
|
||||
let new_q = s.arg();
|
||||
s.re = new_i;
|
||||
s.im = new_q;
|
||||
});
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
extern crate std;
|
||||
|
||||
fn f32_is_close(a: f32, b: f32) -> bool {
|
||||
(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;
|
||||
a.iter().zip(b.iter()).for_each(|(i, j)| {
|
||||
result &= f32_is_close(*i, *j);
|
||||
result &= complex_is_close(*i, *j);
|
||||
});
|
||||
result
|
||||
}
|
||||
@ -327,16 +298,30 @@ mod tests {
|
||||
<= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance
|
||||
}
|
||||
|
||||
fn array_within_tolerance(
|
||||
a: &[f32],
|
||||
b: &[f32],
|
||||
fn complex_within_tolerance(
|
||||
a: Complex,
|
||||
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,
|
||||
fixed_tolerance: f32,
|
||||
) -> bool {
|
||||
let mut result: bool = true;
|
||||
a.iter().zip(b.iter()).for_each(|(i, j)| {
|
||||
result &=
|
||||
within_tolerance(*i, *j, relative_tolerance, fixed_tolerance);
|
||||
result &= complex_within_tolerance(
|
||||
*i,
|
||||
*j,
|
||||
relative_tolerance,
|
||||
fixed_tolerance,
|
||||
);
|
||||
});
|
||||
result
|
||||
}
|
||||
@ -354,75 +339,93 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn magnitude_phase_length_1_quadrant_1() {
|
||||
let mut in_phase: [f32; 1] = [1.];
|
||||
let mut quadrature: [f32; 1] = [1.];
|
||||
magnitude_phase(&mut in_phase, &mut quadrature);
|
||||
assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()]));
|
||||
assert!(f32_array_is_close(&quadrature, &[PI / 4.]));
|
||||
let mut signal: [Complex; 1] = [Complex::new(1., 1.)];
|
||||
magnitude_phase(&mut signal);
|
||||
assert!(complex_array_is_close(
|
||||
&signal,
|
||||
&[Complex::new(2_f32.sqrt(), PI / 4.)]
|
||||
));
|
||||
|
||||
in_phase = [3_f32.sqrt() / 2.];
|
||||
quadrature = [1. / 2.];
|
||||
magnitude_phase(&mut in_phase, &mut quadrature);
|
||||
assert!(f32_array_is_close(&in_phase, &[1_f32]));
|
||||
assert!(f32_array_is_close(&quadrature, &[PI / 6.]));
|
||||
signal = [Complex::new(3_f32.sqrt() / 2., 1. / 2.)];
|
||||
magnitude_phase(&mut signal);
|
||||
assert!(complex_array_is_close(
|
||||
&signal,
|
||||
&[Complex::new(1., PI / 6.)]
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn magnitude_phase_length_1_quadrant_2() {
|
||||
let mut in_phase: [f32; 1] = [-1.];
|
||||
let mut quadrature: [f32; 1] = [1.];
|
||||
magnitude_phase(&mut in_phase, &mut quadrature);
|
||||
assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()]));
|
||||
assert!(f32_array_is_close(&quadrature, &[3. * PI / 4.]));
|
||||
let mut signal = [Complex::new(-1., 1.)];
|
||||
magnitude_phase(&mut signal);
|
||||
assert!(complex_array_is_close(
|
||||
&signal,
|
||||
&[Complex::new(2_f32.sqrt(), 3. * PI / 4.)]
|
||||
));
|
||||
|
||||
in_phase = [-1. / 2.];
|
||||
quadrature = [3_f32.sqrt() / 2.];
|
||||
magnitude_phase(&mut in_phase, &mut quadrature);
|
||||
assert!(f32_array_is_close(&in_phase, &[1_f32]));
|
||||
assert!(f32_array_is_close(&quadrature, &[2. * PI / 3.]));
|
||||
signal = [Complex::new(-1. / 2., 3_f32.sqrt() / 2.)];
|
||||
magnitude_phase(&mut signal);
|
||||
assert!(complex_array_is_close(
|
||||
&signal,
|
||||
&[Complex::new(1_f32, 2. * PI / 3.)]
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn magnitude_phase_length_1_quadrant_3() {
|
||||
let mut in_phase: [f32; 1] = [-1. / 2_f32.sqrt()];
|
||||
let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()];
|
||||
magnitude_phase(&mut in_phase, &mut quadrature);
|
||||
assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()]));
|
||||
assert!(f32_array_is_close(&quadrature, &[-3. * PI / 4.]));
|
||||
let mut signal = [Complex::new(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())];
|
||||
magnitude_phase(&mut signal);
|
||||
assert!(complex_array_is_close(
|
||||
&signal,
|
||||
&[Complex::new(1_f32.sqrt(), -3. * PI / 4.)]
|
||||
));
|
||||
|
||||
in_phase = [-1. / 2.];
|
||||
quadrature = [-2_f32.sqrt()];
|
||||
magnitude_phase(&mut in_phase, &mut quadrature);
|
||||
assert!(f32_array_is_close(&in_phase, &[(3. / 2.) as f32]));
|
||||
assert!(f32_array_is_close(&quadrature, &[-1.91063323625 as f32]));
|
||||
signal = [Complex::new(-1. / 2., -2_f32.sqrt())];
|
||||
magnitude_phase(&mut signal);
|
||||
assert!(complex_array_is_close(
|
||||
&signal,
|
||||
&[Complex::new((3. / 2.) as f32, -1.91063323625 as f32)]
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn magnitude_phase_length_1_quadrant_4() {
|
||||
let mut in_phase: [f32; 1] = [1. / 2_f32.sqrt()];
|
||||
let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()];
|
||||
magnitude_phase(&mut in_phase, &mut quadrature);
|
||||
assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()]));
|
||||
assert!(f32_array_is_close(&quadrature, &[-1. * PI / 4.]));
|
||||
let mut signal = [Complex::new(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())];
|
||||
magnitude_phase(&mut signal);
|
||||
assert!(complex_array_is_close(
|
||||
&signal,
|
||||
&[Complex::new(1_f32.sqrt(), -1. * PI / 4.)]
|
||||
));
|
||||
|
||||
in_phase = [3_f32.sqrt() / 2.];
|
||||
quadrature = [-1. / 2.];
|
||||
magnitude_phase(&mut in_phase, &mut quadrature);
|
||||
assert!(f32_array_is_close(&in_phase, &[1_f32]));
|
||||
assert!(f32_array_is_close(&quadrature, &[-PI / 6.]));
|
||||
signal = [Complex::new(3_f32.sqrt() / 2., -1. / 2.)];
|
||||
magnitude_phase(&mut signal);
|
||||
assert!(complex_array_is_close(
|
||||
&signal,
|
||||
&[Complex::new(1_f32, -PI / 6.)]
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decimate_sample_16_decimated_1() {
|
||||
let in_phase: [f32; 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,
|
||||
1.3, 1.4, 1.5,
|
||||
let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [
|
||||
Complex::new(0.0, 1.6),
|
||||
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] = [
|
||||
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]));
|
||||
assert_eq!(decimate(signal), [Complex::new(0.0, 1.6)]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
@ -439,7 +442,7 @@ mod tests {
|
||||
},
|
||||
);
|
||||
assert_eq!(
|
||||
lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[],),
|
||||
lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[]),
|
||||
Err("insufficient timestamps")
|
||||
);
|
||||
}
|
||||
@ -489,32 +492,20 @@ mod tests {
|
||||
-(initial_phase_integer as f32) / reference_period as f32 * 2. * PI;
|
||||
let phase_increment: f32 =
|
||||
adc_period as f32 / reference_period as f32 * 2. * PI;
|
||||
let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] =
|
||||
[0.; ADC_SAMPLE_BUFFER_SIZE];
|
||||
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 mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE];
|
||||
for (n, s) in signal.iter_mut().enumerate() {
|
||||
let adc_phase = initial_phase + n as f32 * phase_increment;
|
||||
let sine = adc_phase.sin();
|
||||
let cosine = adc_phase.cos();
|
||||
*i = sine * adc_samples[n] as f32;
|
||||
*q = cosine * adc_samples[n] as f32;
|
||||
s.re = sine * adc_samples[n] as f32;
|
||||
s.im = cosine * adc_samples[n] as f32;
|
||||
}
|
||||
let (result_in_phase, result_quadrature) =
|
||||
lockin.demodulate(&adc_samples, timestamps).unwrap();
|
||||
let result = lockin.demodulate(&adc_samples, timestamps).unwrap();
|
||||
assert!(
|
||||
array_within_tolerance(&result_in_phase, &in_phase, 0., 1e-5),
|
||||
"\nin_phase computed: {:?},\nin_phase expected: {:?}",
|
||||
result_in_phase,
|
||||
in_phase
|
||||
);
|
||||
assert!(
|
||||
array_within_tolerance(&result_quadrature, &quadrature, 0., 1e-5),
|
||||
"\nquadrature computed: {:?},\nquadrature expected: {:?}",
|
||||
result_quadrature,
|
||||
quadrature
|
||||
complex_array_within_tolerance(&result, &signal, 0., 1e-5),
|
||||
"\nsignal computed: {:?},\nsignal expected: {:?}",
|
||||
result,
|
||||
signal
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,3 +1,4 @@
|
||||
use dsp::complex::Complex;
|
||||
use dsp::iir::IIR;
|
||||
use dsp::lockin::{
|
||||
decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE,
|
||||
@ -582,7 +583,7 @@ fn lowpass_test(
|
||||
pure_signals.push(desired_input);
|
||||
|
||||
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,
|
||||
timestamp_start,
|
||||
internal_frequency,
|
||||
@ -597,33 +598,31 @@ fn lowpass_test(
|
||||
internal_frequency,
|
||||
);
|
||||
|
||||
let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE];
|
||||
let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE];
|
||||
match lockin.demodulate(&signal, timestamps) {
|
||||
Ok((i, q)) => {
|
||||
in_phase = i;
|
||||
quadrature = q;
|
||||
let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE];
|
||||
match lockin.demodulate(&adc_signal, timestamps) {
|
||||
Ok(s) => {
|
||||
signal = s;
|
||||
}
|
||||
Err(_) => {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
lockin.filter(&mut in_phase, &mut quadrature);
|
||||
let (in_phase_decimated, quadrature_decimated) =
|
||||
decimate(in_phase, quadrature);
|
||||
lockin.filter(&mut signal);
|
||||
let signal_decimated = decimate(signal);
|
||||
|
||||
let mut magnitude_decimated = in_phase_decimated.clone();
|
||||
let mut phase_decimated = quadrature_decimated.clone();
|
||||
let mut magnitude_phase_decimated = signal.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
|
||||
// `time_constant_factor`.
|
||||
if n >= samples {
|
||||
for k in 0..DECIMATED_BUFFER_SIZE {
|
||||
let amplitude_normalized: f32 =
|
||||
magnitude_decimated[k] / ADC_MAX_COUNT as f32;
|
||||
magnitude_phase_decimated[k].re / ADC_MAX_COUNT as f32;
|
||||
assert!(
|
||||
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}",
|
||||
@ -636,13 +635,13 @@ fn lowpass_test(
|
||||
assert!(
|
||||
tolerance_check(
|
||||
effective_phase_offset as f32,
|
||||
phase_decimated[k],
|
||||
magnitude_phase_decimated[k].im,
|
||||
total_phase_noise as f32,
|
||||
tolerance
|
||||
),
|
||||
"phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}",
|
||||
effective_phase_offset as f32,
|
||||
phase_decimated[k],
|
||||
magnitude_phase_decimated[k].im,
|
||||
max_error(
|
||||
effective_phase_offset as f32,
|
||||
total_phase_noise as f32,
|
||||
@ -651,9 +650,9 @@ fn lowpass_test(
|
||||
);
|
||||
|
||||
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 =
|
||||
quadrature_decimated[k] / ADC_MAX_COUNT as f32;
|
||||
signal_decimated[k].im / ADC_MAX_COUNT as f32;
|
||||
assert!(
|
||||
tolerance_check(
|
||||
in_phase_actual as f32,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user