kai/pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

dsp: implement Complex as type alias for tuple

Browse Source
This commit is contained in:
Matt Huszagh 2020-11-28 16:21:08 -08:00
parent d1b7efad48
commit 260206e4f0
4 changed files with 72 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
dsp/src/complex.rs
View File

@ -1,21 +0,0 @@
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())
}
}

2
dsp/src/lib.rs
View File

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

140
dsp/src/lockin.rs
View File

@ -52,8 +52,8 @@
//! 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 super::Complex;
use core::f32::consts::PI;
/// The number of ADC samples in one batch.
@ -155,7 +155,7 @@ impl Lockin {
&mut self,
adc_samples: &[i16],
timestamps: &[u16],
) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> {
) -> Result<[Complex<f32>; 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 {
@ -187,7 +187,7 @@ impl Lockin {
// compute ADC sample phases, sines/cosines and demodulate
let reference_period =
self.timestamps[0].unwrap() - self.timestamps[1].unwrap();
let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE];
let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE];
signal
.iter_mut()
.zip(adc_samples.iter())
@ -200,8 +200,8 @@ impl Lockin {
+ 2. * PI * integer_phase as f32 / reference_period as f32;
let (sine, cosine) = libm::sincosf(phase);
let sample = *sample as f32;
s.re = sine * sample;
s.im = cosine * sample;
s.0 = sine * sample;
s.1 = cosine * sample;
});
Ok(signal)
@ -213,10 +213,10 @@ impl Lockin {
/// # Arguments
///
/// * `signal` - Complex signal to filter.
pub fn filter(&mut self, signal: &mut [Complex]) {
pub fn filter(&mut self, signal: &mut [Complex<f32>]) {
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);
s.0 = self.iir.update(&mut self.iirstate[0], s.0);
s.1 = self.iir.update(&mut self.iirstate[1], s.1);
});
}
}
@ -233,21 +233,21 @@ impl Lockin {
///
/// The decimated signal.
pub fn decimate(
signal: [Complex; ADC_SAMPLE_BUFFER_SIZE],
) -> [Complex; DECIMATED_BUFFER_SIZE] {
signal: [Complex<f32>; ADC_SAMPLE_BUFFER_SIZE],
) -> [Complex<f32>; 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 signal_decimated = [Complex::new(0., 0.); DECIMATED_BUFFER_SIZE];
let mut signal_decimated = [(0_f32, 0_f32); DECIMATED_BUFFER_SIZE];
signal_decimated
.iter_mut()
.zip(signal.iter().step_by(n_k))
.for_each(|(s_d, s)| {
s_d.re = s.re;
s_d.im = s.im;
s_d.0 = s.0;
s_d.1 = s.1;
});
signal_decimated
@ -259,12 +259,12 @@ pub fn decimate(
/// # Arguments
///
/// * `signal` - Complex signal to decimate.
pub fn magnitude_phase(signal: &mut [Complex]) {
pub fn magnitude_phase(signal: &mut [Complex<f32>]) {
signal.iter_mut().for_each(|s| {
let new_i = s.abs();
let new_q = s.arg();
s.re = new_i;
s.im = new_q;
let new_i = libm::sqrtf([s.0, s.1].iter().map(|i| i * i).sum());
let new_q = libm::atan2f(s.1, s.0);
s.0 = new_i;
s.1 = new_q;
});
}
@ -276,11 +276,11 @@ mod tests {
(a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON
}
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_is_close(a: Complex<f32>, b: Complex<f32>) -> bool {
f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1)
}
fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool {
fn complex_array_is_close(a: &[Complex<f32>], b: &[Complex<f32>]) -> bool {
let mut result: bool = true;
a.iter().zip(b.iter()).for_each(|(i, j)| {
result &= complex_is_close(*i, *j);
@ -299,18 +299,18 @@ mod tests {
}
fn complex_within_tolerance(
a: Complex,
b: Complex,
a: Complex<f32>,
b: Complex<f32>,
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)
within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance)
&& within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance)
}
fn complex_array_within_tolerance(
a: &[Complex],
b: &[Complex],
a: &[Complex<f32>],
b: &[Complex<f32>],
relative_tolerance: f32,
fixed_tolerance: f32,
) -> bool {
@ -339,93 +339,81 @@ mod tests {
#[test]
fn magnitude_phase_length_1_quadrant_1() {
let mut signal: [Complex; 1] = [Complex::new(1., 1.)];
let mut signal: [Complex<f32>; 1] = [(1., 1.)];
magnitude_phase(&mut signal);
assert!(complex_array_is_close(
&signal,
&[Complex::new(2_f32.sqrt(), PI / 4.)]
));
assert!(complex_array_is_close(&signal, &[(2_f32.sqrt(), PI / 4.)]));
signal = [Complex::new(3_f32.sqrt() / 2., 1. / 2.)];
signal = [(3_f32.sqrt() / 2., 1. / 2.)];
magnitude_phase(&mut signal);
assert!(complex_array_is_close(
&signal,
&[Complex::new(1., PI / 6.)]
));
assert!(complex_array_is_close(&signal, &[(1., PI / 6.)]));
}
#[test]
fn magnitude_phase_length_1_quadrant_2() {
let mut signal = [Complex::new(-1., 1.)];
let mut signal = [(-1., 1.)];
magnitude_phase(&mut signal);
assert!(complex_array_is_close(
&signal,
&[Complex::new(2_f32.sqrt(), 3. * PI / 4.)]
&[(2_f32.sqrt(), 3. * PI / 4.)]
));
signal = [Complex::new(-1. / 2., 3_f32.sqrt() / 2.)];
signal = [(-1. / 2., 3_f32.sqrt() / 2.)];
magnitude_phase(&mut signal);
assert!(complex_array_is_close(
&signal,
&[Complex::new(1_f32, 2. * PI / 3.)]
));
assert!(complex_array_is_close(&signal, &[(1_f32, 2. * PI / 3.)]));
}
#[test]
fn magnitude_phase_length_1_quadrant_3() {
let mut signal = [Complex::new(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())];
let mut signal = [(-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.)]
&[(1_f32.sqrt(), -3. * PI / 4.)]
));
signal = [Complex::new(-1. / 2., -2_f32.sqrt())];
signal = [(-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)]
&[((3. / 2.) as f32, -1.91063323625 as f32)]
));
}
#[test]
fn magnitude_phase_length_1_quadrant_4() {
let mut signal = [Complex::new(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())];
let mut signal = [(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.)]
&[(1_f32.sqrt(), -1. * PI / 4.)]
));
signal = [Complex::new(3_f32.sqrt() / 2., -1. / 2.)];
signal = [(3_f32.sqrt() / 2., -1. / 2.)];
magnitude_phase(&mut signal);
assert!(complex_array_is_close(
&signal,
&[Complex::new(1_f32, -PI / 6.)]
));
assert!(complex_array_is_close(&signal, &[(1_f32, -PI / 6.)]));
}
#[test]
fn decimate_sample_16_decimated_1() {
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 signal: [Complex<f32>; ADC_SAMPLE_BUFFER_SIZE] = [
(0.0, 1.6),
(0.1, 1.7),
(0.2, 1.8),
(0.3, 1.9),
(0.4, 2.0),
(0.5, 2.1),
(0.6, 2.2),
(0.7, 2.3),
(0.8, 2.4),
(0.9, 2.5),
(1.0, 2.6),
(1.1, 2.7),
(1.2, 2.8),
(1.3, 2.9),
(1.4, 3.0),
(1.5, 3.1),
];
assert_eq!(decimate(signal), [Complex::new(0.0, 1.6)]);
assert_eq!(decimate(signal), [(0.0, 1.6)]);
}
#[test]
@ -492,13 +480,13 @@ 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 signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE];
let mut signal = [(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();
s.re = sine * adc_samples[n] as f32;
s.im = cosine * adc_samples[n] as f32;
s.0 = sine * adc_samples[n] as f32;
s.1 = cosine * adc_samples[n] as f32;
}
let result = lockin.demodulate(&adc_samples, timestamps).unwrap();
assert!(

14
dsp/tests/lockin_low_pass.rs
View File

@ -1,9 +1,9 @@
use dsp::complex::Complex;
use dsp::iir::IIR;
use dsp::lockin::{
decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE,
DECIMATED_BUFFER_SIZE,
};
use dsp::Complex;
use std::f64::consts::PI;
use std::vec::Vec;
@ -598,7 +598,7 @@ fn lowpass_test(
internal_frequency,
);
let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE];
let mut signal: [Complex<f32>; ADC_SAMPLE_BUFFER_SIZE];
match lockin.demodulate(&adc_signal, timestamps) {
Ok(s) => {
signal = s;
@ -622,7 +622,7 @@ fn lowpass_test(
if n >= samples {
for k in 0..DECIMATED_BUFFER_SIZE {
let amplitude_normalized: f32 =
magnitude_phase_decimated[k].re / ADC_MAX_COUNT as f32;
magnitude_phase_decimated[k].0 / 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}",
@ -635,13 +635,13 @@ fn lowpass_test(
assert!(
tolerance_check(
effective_phase_offset as f32,
magnitude_phase_decimated[k].im,
magnitude_phase_decimated[k].1,
total_phase_noise as f32,
tolerance
),
"phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}",
effective_phase_offset as f32,
magnitude_phase_decimated[k].im,
magnitude_phase_decimated[k].1,
max_error(
effective_phase_offset as f32,
total_phase_noise as f32,
@ -650,9 +650,9 @@ fn lowpass_test(
);
let in_phase_normalized: f32 =
signal_decimated[k].re / ADC_MAX_COUNT as f32;
signal_decimated[k].0 / ADC_MAX_COUNT as f32;
let quadrature_normalized: f32 =
signal_decimated[k].im / ADC_MAX_COUNT as f32;
signal_decimated[k].1 / ADC_MAX_COUNT as f32;
assert!(
tolerance_check(
in_phase_actual as f32,