dsp: use num
parent
1239a29904
commit
9983fad041
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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<T>(pub T, pub T);
|
||||
pub trait Map<F> {
|
||||
fn map(&self, func: F) -> Self;
|
||||
}
|
||||
|
||||
impl<T: Copy> Complex<T> {
|
||||
pub fn map<F>(&self, func: F) -> Self
|
||||
where
|
||||
F: Fn(T) -> T,
|
||||
{
|
||||
Complex(func(self.0), func(self.1))
|
||||
impl<F: Fn(T) -> T, T: Copy> Map<F> for Complex<T> {
|
||||
fn map(&self, func: F) -> Self {
|
||||
Complex {
|
||||
re: func(self.re),
|
||||
im: func(self.im),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Complex<i32> {
|
||||
pub trait FastInt<T, U> {
|
||||
fn from_angle(angle: T) -> Self;
|
||||
fn abs_sqr(&self) -> U;
|
||||
fn log2(&self) -> T;
|
||||
fn arg(&self) -> T;
|
||||
}
|
||||
|
||||
impl FastInt<i32, u32> for Complex<i32> {
|
||||
/// Return a Complex on the unit circle given an angle.
|
||||
///
|
||||
/// Example:
|
||||
///
|
||||
/// ```
|
||||
/// use dsp::Complex;
|
||||
/// use dsp::{Complex, FastInt};
|
||||
/// Complex::<i32>::from_angle(0);
|
||||
/// Complex::<i32>::from_angle(1 << 30); // pi/2
|
||||
/// Complex::<i32>::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<i32> {
|
|||
/// 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<i32> {
|
|||
/// 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<i32> {
|
|||
/// 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<i32> {
|
||||
type Output = Self;
|
||||
pub trait MulScaled<T> {
|
||||
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<Complex<i32>> for Complex<i32> {
|
||||
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<i32> for Complex<i32> {
|
||||
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<i32> for Complex<i32> {
|
||||
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<i16> for Complex<i32> {
|
||||
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<i16> for Complex<i32> {
|
||||
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,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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<i32> {
|
||||
// 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),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue