From 9983fad0410aafb3efe087eaf6d3e225dc49f16e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 18 Feb 2021 13:17:24 +0100 Subject: [PATCH] dsp: use num --- Cargo.lock | 36 ++++++++++ dsp/Cargo.toml | 1 + dsp/benches/micro.rs | 4 +- dsp/src/complex.rs | 132 +++++++++++++++++++------------------ dsp/src/cossin.rs | 11 ++-- dsp/src/lib.rs | 26 ++++---- dsp/src/lockin.rs | 17 ++--- dsp/src/testing.rs | 2 +- src/bin/lockin-external.rs | 4 +- src/bin/lockin-internal.rs | 2 +- 10 files changed, 138 insertions(+), 97 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 64c84cd..d5ebc04 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -356,6 +356,7 @@ dependencies = [ "generic-array 0.14.4", "libm", "ndarray", + "num", "rand", "serde", ] @@ -623,6 +624,19 @@ dependencies = [ "rawpointer", ] +[[package]] +name = "num" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" +dependencies = [ + "num-complex", + "num-integer", + "num-iter", + "num-rational", + "num-traits", +] + [[package]] name = "num-complex" version = "0.3.1" @@ -642,6 +656,28 @@ dependencies = [ "num-traits", ] +[[package]] +name = "num-iter" +version = "0.1.42" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b2021c8337a54d21aca0d59a92577a029af9431cb59b909b03252b9c164fad59" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "12ac428b1cb17fce6f731001d307d351ec70a6d202fc2e60f7d4c5e42d8f4f07" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + [[package]] name = "num-traits" version = "0.2.14" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index 8e07060..516b6e2 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -8,6 +8,7 @@ edition = "2018" libm = "0.2.1" serde = { version = "1.0", features = ["derive"], default-features = false } generic-array = "0.14" +num = { version = "0.3.1", default-features = false } [dev-dependencies] criterion = "0.3" diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs index 56b9149..6ecc090 100644 --- a/dsp/benches/micro.rs +++ b/dsp/benches/micro.rs @@ -1,8 +1,6 @@ use core::f32::consts::PI; use criterion::{black_box, criterion_group, criterion_main, Criterion}; -use dsp::{atan2, cossin}; -use dsp::{iir, iir_int}; -use dsp::{pll::PLL, rpll::RPLL}; +use dsp::{atan2, cossin, iir, iir_int, PLL, RPLL}; fn atan2_bench(c: &mut Criterion) { let xi = (10 << 16) as i32; diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs index 6353f9a..42d736b 100644 --- a/dsp/src/complex.rs +++ b/dsp/src/complex.rs @@ -1,33 +1,41 @@ -use core::ops::Mul; +pub use num::Complex; use super::{atan2, cossin}; -#[derive(Copy, Clone, Default, PartialEq, Debug)] -pub struct Complex(pub T, pub T); +pub trait Map { + fn map(&self, func: F) -> Self; +} -impl Complex { - pub fn map(&self, func: F) -> Self - where - F: Fn(T) -> T, - { - Complex(func(self.0), func(self.1)) +impl T, T: Copy> Map for Complex { + fn map(&self, func: F) -> Self { + Complex { + re: func(self.re), + im: func(self.im), + } } } -impl Complex { +pub trait FastInt { + fn from_angle(angle: T) -> Self; + fn abs_sqr(&self) -> U; + fn log2(&self) -> T; + fn arg(&self) -> T; +} + +impl FastInt for Complex { /// Return a Complex on the unit circle given an angle. /// /// Example: /// /// ``` - /// use dsp::Complex; + /// use dsp::{Complex, FastInt}; /// Complex::::from_angle(0); /// Complex::::from_angle(1 << 30); // pi/2 /// Complex::::from_angle(-1 << 30); // -pi/2 /// ``` - pub fn from_angle(angle: i32) -> Self { + fn from_angle(angle: i32) -> Self { let (c, s) = cossin(angle); - Self(c, s) + Self { re: c, im: s } } /// Return the absolute square (the squared magnitude). @@ -39,13 +47,13 @@ impl Complex { /// Example: /// /// ``` - /// use dsp::Complex; - /// assert_eq!(Complex(i32::MIN, 0).abs_sqr(), 1 << 31); - /// assert_eq!(Complex(i32::MAX, i32::MAX).abs_sqr(), u32::MAX - 3); + /// use dsp::{Complex, FastInt}; + /// assert_eq!(Complex::new(i32::MIN, 0).abs_sqr(), 1 << 31); + /// assert_eq!(Complex::new(i32::MAX, i32::MAX).abs_sqr(), u32::MAX - 3); /// ``` - pub fn abs_sqr(&self) -> u32 { - (((self.0 as i64) * (self.0 as i64) - + (self.1 as i64) * (self.1 as i64)) + fn abs_sqr(&self) -> u32 { + (((self.re as i64) * (self.re as i64) + + (self.im as i64) * (self.im as i64)) >> 31) as u32 } @@ -59,15 +67,15 @@ impl Complex { /// Example: /// /// ``` - /// use dsp::Complex; - /// assert_eq!(Complex(i32::MAX, i32::MAX).log2(), -1); - /// assert_eq!(Complex(i32::MAX, 0).log2(), -2); - /// assert_eq!(Complex(1, 0).log2(), -63); - /// assert_eq!(Complex(0, 0).log2(), -64); + /// use dsp::{Complex, FastInt}; + /// assert_eq!(Complex::new(i32::MAX, i32::MAX).log2(), -1); + /// assert_eq!(Complex::new(i32::MAX, 0).log2(), -2); + /// assert_eq!(Complex::new(1, 0).log2(), -63); + /// assert_eq!(Complex::new(0, 0).log2(), -64); /// ``` - pub fn log2(&self) -> i32 { - let a = (self.0 as i64) * (self.0 as i64) - + (self.1 as i64) * (self.1 as i64); + fn log2(&self) -> i32 { + let a = (self.re as i64) * (self.re as i64) + + (self.im as i64) * (self.im as i64); -(a.leading_zeros() as i32) } @@ -78,52 +86,50 @@ impl Complex { /// Example: /// /// ``` - /// use dsp::Complex; - /// assert_eq!(Complex(1, 0).arg(), 0); - /// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX); - /// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX); - /// assert_eq!(Complex(0, -1).arg(), -i32::MAX >> 1); - /// assert_eq!(Complex(0, 1).arg(), (i32::MAX >> 1) + 1); - /// assert_eq!(Complex(1, 1).arg(), (i32::MAX >> 2) + 1); + /// use dsp::{Complex, FastInt}; + /// assert_eq!(Complex::new(1, 0).arg(), 0); + /// assert_eq!(Complex::new(-i32::MAX, 1).arg(), i32::MAX); + /// assert_eq!(Complex::new(-i32::MAX, -1).arg(), -i32::MAX); + /// assert_eq!(Complex::new(0, -1).arg(), -i32::MAX >> 1); + /// assert_eq!(Complex::new(0, 1).arg(), (i32::MAX >> 1) + 1); + /// assert_eq!(Complex::new(1, 1).arg(), (i32::MAX >> 2) + 1); /// ``` - pub fn arg(&self) -> i32 { - atan2(self.1, self.0) + fn arg(&self) -> i32 { + atan2(self.im, self.re) } } -impl Mul for Complex { - type Output = Self; +pub trait MulScaled { + fn mul_scaled(self, other: T) -> Self; +} - fn mul(self, other: Self) -> Self { - let a = self.0 as i64; - let b = self.1 as i64; - let c = other.0 as i64; - let d = other.1 as i64; - Complex( - ((a * c - b * d + (1 << 31)) >> 32) as i32, - ((b * c + a * d + (1 << 31)) >> 32) as i32, - ) +impl MulScaled> for Complex { + fn mul_scaled(self, other: Self) -> Self { + let a = self.re as i64; + let b = self.im as i64; + let c = other.re as i64; + let d = other.im as i64; + Complex { + re: ((a * c - b * d + (1 << 31)) >> 32) as i32, + im: ((b * c + a * d + (1 << 31)) >> 32) as i32, + } } } -impl Mul for Complex { - type Output = Self; - - fn mul(self, other: i32) -> Self { - Complex( - ((other as i64 * self.0 as i64 + (1 << 31)) >> 32) as i32, - ((other as i64 * self.1 as i64 + (1 << 31)) >> 32) as i32, - ) +impl MulScaled for Complex { + fn mul_scaled(self, other: i32) -> Self { + Complex { + re: ((other as i64 * self.re as i64 + (1 << 31)) >> 32) as i32, + im: ((other as i64 * self.im as i64 + (1 << 31)) >> 32) as i32, + } } } -impl Mul for Complex { - type Output = Self; - - fn mul(self, other: i16) -> Self { - Complex( - (other as i32 * (self.0 >> 16) + (1 << 15)) >> 16, - (other as i32 * (self.1 >> 16) + (1 << 15)) >> 16, - ) +impl MulScaled for Complex { + fn mul_scaled(self, other: i16) -> Self { + Complex { + re: (other as i32 * (self.re >> 16) + (1 << 15)) >> 16, + im: (other as i32 * (self.im >> 16) + (1 << 15)) >> 16, + } } } diff --git a/dsp/src/cossin.rs b/dsp/src/cossin.rs index e5746a3..4a5cccd 100644 --- a/dsp/src/cossin.rs +++ b/dsp/src/cossin.rs @@ -74,7 +74,6 @@ pub fn cossin(phase: i32) -> (i32, i32) { #[cfg(test)] mod tests { use super::*; - use crate::Complex; use core::f64::consts::PI; #[test] @@ -82,11 +81,11 @@ mod tests { // Constant amplitude error due to LUT data range. const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as _; const MAX_PHASE: f64 = (1i64 << 32) as _; - let mut rms_err = Complex(0f64, 0f64); - let mut sum_err = Complex(0f64, 0f64); - let mut max_err = Complex(0f64, 0f64); - let mut sum = Complex(0f64, 0f64); - let mut demod = Complex(0f64, 0f64); + let mut rms_err = (0f64, 0f64); + let mut sum_err = (0f64, 0f64); + let mut max_err = (0f64, 0f64); + let mut sum = (0f64, 0f64); + let mut demod = (0f64, 0f64); // use std::{fs::File, io::{BufWriter, prelude::*}, path::Path}; // let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap()); diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 31ff680..359eee1 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -80,22 +80,26 @@ where .fold(y0, |y, xa| y + xa) } -pub mod accu; mod atan2; +pub use atan2::*; +mod accu; +pub use accu::*; mod complex; +pub use complex::*; mod cossin; +pub use cossin::*; pub mod iir; pub mod iir_int; -pub mod lockin; -pub mod lowpass; -pub mod pll; -pub mod rpll; -pub mod unwrap; - -pub use accu::Accu; -pub use atan2::atan2; -pub use complex::Complex; -pub use cossin::cossin; +mod lockin; +pub use lockin::*; +mod lowpass; +pub use lowpass::*; +mod pll; +pub use pll::*; +mod rpll; +pub use rpll::*; +mod unwrap; +pub use unwrap::*; #[cfg(test)] pub mod testing; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 6ebf4cb..d3ebc73 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,4 +1,4 @@ -use super::{lowpass::Lowpass, Complex}; +use super::{Complex, FastInt, Lowpass, MulScaled}; use generic_array::typenum::U2; #[derive(Clone, Default)] @@ -10,17 +10,14 @@ impl Lockin { /// Update the lockin with a sample taken at a given phase. /// The lowpass has a gain of `1 << k`. pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex { - // Get the LO signal for demodulation. - let lo = Complex::from_angle(phase); - - // Mix with the LO signal - let mix = lo * sample; + // Get the LO signal for demodulation and mix the sample; + let mix = Complex::from_angle(phase).mul_scaled(sample); // Filter with the IIR lowpass, // return IQ (in-phase and quadrature) data. - Complex( - self.state[0].update(mix.0, k), - self.state[1].update(mix.1, k), - ) + Complex { + re: self.state[0].update(mix.re, k), + im: self.state[1].update(mix.im, k), + } } } diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs index f8e753d..1654b62 100644 --- a/dsp/src/testing.rs +++ b/dsp/src/testing.rs @@ -31,7 +31,7 @@ pub fn complex_isclose( rtol: f32, atol: f32, ) -> bool { - isclosef(a.0, b.0, rtol, atol) && isclosef(a.1, b.1, rtol, atol) + isclosef(a.re, b.re, rtol, atol) && isclosef(a.im, b.im, rtol, atol) } pub fn complex_allclose( diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index a454e19..15b88c7 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin-external.rs @@ -6,7 +6,7 @@ use stm32h7xx_hal as hal; use stabilizer::{hardware, hardware::design_parameters}; -use dsp::{lockin::Lockin, rpll::RPLL, Accu}; +use dsp::{Accu, FastInt, Lockin, RPLL}; use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, }; @@ -127,7 +127,7 @@ const APP: () = { // Convert from IQ to power and phase. "power_phase" => [(output.log2() << 24) as _, output.arg()], "frequency_discriminator" => [pll_frequency as _, output.arg()], - _ => [output.0, output.1], + _ => [output.re, output.im], }; // Convert to DAC data. diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index c4d5bd6..bba0569 100644 --- a/src/bin/lockin-internal.rs +++ b/src/bin/lockin-internal.rs @@ -2,7 +2,7 @@ #![no_std] #![no_main] -use dsp::{lockin::Lockin, Accu}; +use dsp::{Accu, FastInt, Lockin}; use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; use stabilizer::{hardware, hardware::design_parameters};