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 1/9] 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}; From 07b7751dbc947530ae33bd0556e43788c99293cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 18 Feb 2021 14:06:01 +0100 Subject: [PATCH 2/9] move lowpass gain outside lowpass/lockin --- dsp/src/lockin.rs | 3 +-- dsp/src/lowpass.rs | 10 +++++----- src/bin/lockin-external.rs | 15 +++++++++------ src/bin/lockin-internal.rs | 11 ++++++----- 4 files changed, 21 insertions(+), 18 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index d3ebc73..f0a2767 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -8,8 +8,7 @@ pub struct Lockin { 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 { + pub fn update(&mut self, sample: i32, phase: i32, k: u8) -> Complex { // Get the LO signal for demodulation and mix the sample; let mix = Complex::from_angle(phase).mul_scaled(sample); diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index 91fae2f..6ed23e0 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -14,17 +14,17 @@ impl> Lowpass { /// Update the filter with a new sample. /// /// # Args - /// * `x`: Input data, needs `k` bits headroom. - /// * `k`: Log2 time constant, 0..31. + /// * `x`: Input data. Needs 1 bit headroom. + /// * `k`: Log2 time constant, 0..=31. /// /// # Return - /// Filtered output y, with gain of `1 << k`. + /// Filtered output y. pub fn update(&mut self, x: i32, k: u8) -> i32 { debug_assert!(k & 31 == k); // This is an unrolled and optimized first-order IIR loop // that works for all possible time constants. - // Note DF-II and the zeros at Nyquist. - let mut x = x << k; + // Note T-DF-I and the zeros at Nyquist. + let mut x = x; for y in self.y.iter_mut() { let dy = (x - *y + (1 << (k - 1))) >> k; *y += dy; diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index 15b88c7..b3b9d4f 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::{Accu, FastInt, Lockin, RPLL}; +use dsp::{Accu, Complex, FastInt, Lockin, RPLL}; use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, }; @@ -112,15 +112,18 @@ const APP: () = { let sample_phase = phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); - let output = adc_samples[0] + let output: Complex = adc_samples[0] .iter() + // Zip in the LO phase. .zip(Accu::new(sample_phase, sample_frequency)) - // Convert to signed, MSB align the ADC sample. + // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) .map(|(&sample, phase)| { - lockin.update(sample as i16, phase, time_constant) + let s = (sample as i16 as i32) + << (15 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2 + 1); + lockin.update(s, phase, time_constant) }) - .last() - .unwrap(); + // Decimate + .sum(); let conf = "frequency_discriminator"; let output = match conf { diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index bba0569..6913f0a 100644 --- a/src/bin/lockin-internal.rs +++ b/src/bin/lockin-internal.rs @@ -2,7 +2,7 @@ #![no_std] #![no_main] -use dsp::{Accu, FastInt, Lockin}; +use dsp::{Accu, Complex, FastInt, Lockin}; use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; use stabilizer::{hardware, hardware::design_parameters}; @@ -95,17 +95,18 @@ const APP: () = { let sample_phase = phase_offset .wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); - let output = adc_samples + let output: Complex = adc_samples .iter() // Zip in the LO phase. .zip(Accu::new(sample_phase, sample_frequency)) // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) .map(|(&sample, phase)| { - lockin.update(sample as i16, phase, time_constant) + let s = (sample as i16 as i32) + << (15 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2 + 1); + lockin.update(s, phase, time_constant) }) // Decimate - .last() - .unwrap(); + .sum(); for value in dac_samples[1].iter_mut() { *value = (output.arg() >> 16) as u16 ^ 0x8000; From f050ba8e9f8a8cd2b20f31fdbdad76fddea23fea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 18 Feb 2021 14:29:47 +0100 Subject: [PATCH 3/9] lockin: let the lowpass do all filtering --- src/bin/lockin-external.rs | 6 +++--- src/bin/lockin-internal.rs | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index b3b9d4f..f24f251 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin-external.rs @@ -118,12 +118,12 @@ const APP: () = { .zip(Accu::new(sample_phase, sample_frequency)) // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) .map(|(&sample, phase)| { - let s = (sample as i16 as i32) - << (15 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2 + 1); + let s = (sample as i16 as i32) << (15 + 1); lockin.update(s, phase, time_constant) }) // Decimate - .sum(); + .last() + .unwrap(); let conf = "frequency_discriminator"; let output = match conf { diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index 6913f0a..393130e 100644 --- a/src/bin/lockin-internal.rs +++ b/src/bin/lockin-internal.rs @@ -101,12 +101,12 @@ const APP: () = { .zip(Accu::new(sample_phase, sample_frequency)) // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) .map(|(&sample, phase)| { - let s = (sample as i16 as i32) - << (15 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2 + 1); + let s = (sample as i16 as i32) << (15 + 1); lockin.update(s, phase, time_constant) }) // Decimate - .sum(); + .last() + .unwrap(); for value in dac_samples[1].iter_mut() { *value = (output.arg() >> 16) as u16 ^ 0x8000; From c0457787bb23f2866bac1c36aa1f4862d2de6cd5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 18 Feb 2021 18:43:45 +0100 Subject: [PATCH 4/9] lockin-external: use enum --- src/bin/lockin-external.rs | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index f24f251..54b312e 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin-external.rs @@ -125,12 +125,19 @@ const APP: () = { .last() .unwrap(); - let conf = "frequency_discriminator"; + #[allow(dead_code)] + enum Conf { + PowerPhase, + FrequencyDiscriminator, + Quadrature, + } + + let conf = Conf::FrequencyDiscriminator; // TODO: expose let output = match conf { // Convert from IQ to power and phase. - "power_phase" => [(output.log2() << 24) as _, output.arg()], - "frequency_discriminator" => [pll_frequency as _, output.arg()], - _ => [output.re, output.im], + Conf::PowerPhase => [(output.log2() << 24) as _, output.arg()], + Conf::FrequencyDiscriminator => [pll_frequency as _, output.arg()], + Conf::Quadrature => [output.re, output.im], }; // Convert to DAC data. From 33b9b414057d094db23e81c1091fc9de63037db7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 18 Feb 2021 18:50:31 +0100 Subject: [PATCH 5/9] lowpass: saturating math since it's free --- dsp/src/lowpass.rs | 7 ++++--- src/bin/lockin-external.rs | 5 +++-- src/bin/lockin-internal.rs | 5 +++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index 6ed23e0..5ab803d 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -14,19 +14,20 @@ impl> Lowpass { /// Update the filter with a new sample. /// /// # Args - /// * `x`: Input data. Needs 1 bit headroom. - /// * `k`: Log2 time constant, 0..=31. + /// * `x`: Input data. Needs 1 bit headroom but will saturate cleanly beyond that. + /// * `k`: Log2 time constant, 1..=31. /// /// # Return /// Filtered output y. pub fn update(&mut self, x: i32, k: u8) -> i32 { debug_assert!(k & 31 == k); + debug_assert!((k - 1) & 31 == k - 1); // This is an unrolled and optimized first-order IIR loop // that works for all possible time constants. // Note T-DF-I and the zeros at Nyquist. let mut x = x; for y in self.y.iter_mut() { - let dy = (x - *y + (1 << (k - 1))) >> k; + let dy = x.saturating_sub(*y).saturating_add(1 << (k - 1)) >> k; *y += dy; x = *y - (dy >> 1); } diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index 54b312e..b40037f 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin-external.rs @@ -118,12 +118,13 @@ const APP: () = { .zip(Accu::new(sample_phase, sample_frequency)) // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) .map(|(&sample, phase)| { - let s = (sample as i16 as i32) << (15 + 1); + let s = (sample as i16 as i32) << 16; lockin.update(s, phase, time_constant) }) // Decimate .last() - .unwrap(); + .unwrap() + * 2; // Full scale assuming the 2f component is gone. #[allow(dead_code)] enum Conf { diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index 393130e..3539993 100644 --- a/src/bin/lockin-internal.rs +++ b/src/bin/lockin-internal.rs @@ -101,12 +101,13 @@ const APP: () = { .zip(Accu::new(sample_phase, sample_frequency)) // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) .map(|(&sample, phase)| { - let s = (sample as i16 as i32) << (15 + 1); + let s = (sample as i16 as i32) << 16; lockin.update(s, phase, time_constant) }) // Decimate .last() - .unwrap(); + .unwrap() + * 2; // Full scale assuming the 2f component is gone. for value in dac_samples[1].iter_mut() { *value = (output.arg() >> 16) as u16 ^ 0x8000; From a97829baf54e2ddd6fc182f114bd5abc3a4b8e79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 18 Feb 2021 22:14:21 +0100 Subject: [PATCH 6/9] add saturating_scale --- dsp/src/lib.rs | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 359eee1..f3af4d4 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -80,6 +80,23 @@ where .fold(y0, |y, xa| y + xa) } +/// Combine high and low i32 into a single downscaled i32, saturating the type. +pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 { + debug_assert!(shift & 31 == shift); + + let shift_hi = 31 - shift; + debug_assert!(shift_hi & 31 == shift_hi); + + let over = hi >> shift; + if over < -1 { + i32::MIN + } else if over > 0 { + i32::MAX + } else { + (lo >> shift) + (hi << shift_hi) + } +} + mod atan2; pub use atan2::*; mod accu; From c87cb3eb9c54c3237179aa9dbd40ca1165aad901 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 19 Feb 2021 09:20:42 +0100 Subject: [PATCH 7/9] dsp: factor out tools --- dsp/src/lib.rs | 99 +----------------------------------------------- dsp/src/tools.rs | 97 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 99 insertions(+), 97 deletions(-) create mode 100644 dsp/src/tools.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index f3af4d4..581f585 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,102 +1,7 @@ #![cfg_attr(not(test), no_std)] -#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] - -use core::ops::{Add, Mul, Neg}; - -fn abs(x: T) -> T -where - T: PartialOrd + Default + Neg, -{ - if x >= T::default() { - x - } else { - -x - } -} - -// These are implemented here because core::f32 doesn't have them (yet). -// They are naive and don't handle inf/nan. -// `compiler-intrinsics`/llvm should have better (robust, universal, and -// faster) implementations. - -fn copysign(x: T, y: T) -> T -where - T: PartialOrd + Default + Neg, -{ - if (x >= T::default() && y >= T::default()) - || (x <= T::default() && y <= T::default()) - { - x - } else { - -x - } -} - -#[cfg(not(feature = "nightly"))] -fn max(x: T, y: T) -> T -where - T: PartialOrd, -{ - if x > y { - x - } else { - y - } -} - -#[cfg(not(feature = "nightly"))] -fn min(x: T, y: T) -> T -where - T: PartialOrd, -{ - if x < y { - x - } else { - y - } -} - -#[cfg(feature = "nightly")] -fn max(x: f32, y: f32) -> f32 { - core::intrinsics::maxnumf32(x, y) -} - -#[cfg(feature = "nightly")] -fn min(x: f32, y: f32) -> f32 { - core::intrinsics::minnumf32(x, y) -} - -// Multiply-accumulate vectors `x` and `a`. -// -// A.k.a. dot product. -// Rust/LLVM optimize this nicely. -fn macc(y0: T, x: &[T], a: &[T]) -> T -where - T: Add + Mul + Copy, -{ - x.iter() - .zip(a) - .map(|(x, a)| *x * *a) - .fold(y0, |y, xa| y + xa) -} - -/// Combine high and low i32 into a single downscaled i32, saturating the type. -pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 { - debug_assert!(shift & 31 == shift); - - let shift_hi = 31 - shift; - debug_assert!(shift_hi & 31 == shift_hi); - - let over = hi >> shift; - if over < -1 { - i32::MIN - } else if over > 0 { - i32::MAX - } else { - (lo >> shift) + (hi << shift_hi) - } -} +mod tools; +pub use tools::*; mod atan2; pub use atan2::*; mod accu; diff --git a/dsp/src/tools.rs b/dsp/src/tools.rs new file mode 100644 index 0000000..45dd66f --- /dev/null +++ b/dsp/src/tools.rs @@ -0,0 +1,97 @@ +#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] + +use core::ops::{Add, Mul, Neg}; + +pub fn abs(x: T) -> T +where + T: PartialOrd + Default + Neg, +{ + if x >= T::default() { + x + } else { + -x + } +} + +// These are implemented here because core::f32 doesn't have them (yet). +// They are naive and don't handle inf/nan. +// `compiler-intrinsics`/llvm should have better (robust, universal, and +// faster) implementations. + +pub fn copysign(x: T, y: T) -> T +where + T: PartialOrd + Default + Neg, +{ + if (x >= T::default() && y >= T::default()) + || (x <= T::default() && y <= T::default()) + { + x + } else { + -x + } +} + +#[cfg(not(feature = "nightly"))] +pub fn max(x: T, y: T) -> T +where + T: PartialOrd, +{ + if x > y { + x + } else { + y + } +} + +#[cfg(not(feature = "nightly"))] +pub fn min(x: T, y: T) -> T +where + T: PartialOrd, +{ + if x < y { + x + } else { + y + } +} + +#[cfg(feature = "nightly")] +pub fn max(x: f32, y: f32) -> f32 { + core::intrinsics::maxnumf32(x, y) +} + +#[cfg(feature = "nightly")] +pub fn min(x: f32, y: f32) -> f32 { + core::intrinsics::minnumf32(x, y) +} + +// Multiply-accumulate vectors `x` and `a`. +// +// A.k.a. dot product. +// Rust/LLVM optimize this nicely. +pub fn macc(y0: T, x: &[T], a: &[T]) -> T +where + T: Add + Mul + Copy, +{ + x.iter() + .zip(a) + .map(|(x, a)| *x * *a) + .fold(y0, |y, xa| y + xa) +} + +/// Combine high and low i32 into a single downscaled i32, saturating the type. +pub fn saturating_scale(lo: i32, hi: i32, shift: u32) -> i32 { + debug_assert!(shift & 31 == shift); + + let shift_hi = 31 - shift; + debug_assert!(shift_hi & 31 == shift_hi); + + let over = hi >> shift; + if over < -1 { + i32::MIN + } else if over > 0 { + i32::MAX + } else { + (lo >> shift) + (hi << shift_hi) + } +} From ccec9d7fed72a262912a12097969b14980d1b5ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 19 Feb 2021 09:29:38 +0100 Subject: [PATCH 8/9] complex: rename extension trait, fix MulScaled --- dsp/src/complex.rs | 39 ++++++++++++++------------------------ dsp/src/lockin.rs | 2 +- src/bin/lockin-external.rs | 2 +- src/bin/lockin-internal.rs | 2 +- 4 files changed, 17 insertions(+), 28 deletions(-) diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs index 42d736b..3349460 100644 --- a/dsp/src/complex.rs +++ b/dsp/src/complex.rs @@ -2,33 +2,21 @@ pub use num::Complex; use super::{atan2, cossin}; -pub trait Map { - fn map(&self, func: F) -> Self; -} - -impl T, T: Copy> Map for Complex { - fn map(&self, func: F) -> Self { - Complex { - re: func(self.re), - im: func(self.im), - } - } -} - -pub trait FastInt { +/// Complex extension trait offering DSP (fast, good accuracy) functionality. +pub trait ComplexExt { fn from_angle(angle: T) -> Self; fn abs_sqr(&self) -> U; fn log2(&self) -> T; fn arg(&self) -> T; } -impl FastInt for Complex { +impl ComplexExt for Complex { /// Return a Complex on the unit circle given an angle. /// /// Example: /// /// ``` - /// use dsp::{Complex, FastInt}; + /// use dsp::{Complex, ComplexExt}; /// Complex::::from_angle(0); /// Complex::::from_angle(1 << 30); // pi/2 /// Complex::::from_angle(-1 << 30); // -pi/2 @@ -47,7 +35,7 @@ impl FastInt for Complex { /// Example: /// /// ``` - /// use dsp::{Complex, FastInt}; + /// use dsp::{Complex, ComplexExt}; /// assert_eq!(Complex::new(i32::MIN, 0).abs_sqr(), 1 << 31); /// assert_eq!(Complex::new(i32::MAX, i32::MAX).abs_sqr(), u32::MAX - 3); /// ``` @@ -67,7 +55,7 @@ impl FastInt for Complex { /// Example: /// /// ``` - /// use dsp::{Complex, FastInt}; + /// use dsp::{Complex, ComplexExt}; /// 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); @@ -86,7 +74,7 @@ impl FastInt for Complex { /// Example: /// /// ``` - /// use dsp::{Complex, FastInt}; + /// use dsp::{Complex, ComplexExt}; /// 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); @@ -99,6 +87,7 @@ impl FastInt for Complex { } } +/// Full scale fixed point multiplication. pub trait MulScaled { fn mul_scaled(self, other: T) -> Self; } @@ -110,8 +99,8 @@ impl MulScaled> for Complex { 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, + re: ((a * c - b * d + (1 << 30)) >> 31) as i32, + im: ((b * c + a * d + (1 << 30)) >> 31) as i32, } } } @@ -119,8 +108,8 @@ impl MulScaled> for Complex { 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, + re: ((other as i64 * self.re as i64 + (1 << 30)) >> 31) as i32, + im: ((other as i64 * self.im as i64 + (1 << 30)) >> 31) as i32, } } } @@ -128,8 +117,8 @@ impl MulScaled for Complex { 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, + re: (other as i32 * (self.re >> 16) + (1 << 14)) >> 15, + im: (other as i32 * (self.im >> 16) + (1 << 14)) >> 15, } } } diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index f0a2767..6a977b4 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -1,4 +1,4 @@ -use super::{Complex, FastInt, Lowpass, MulScaled}; +use super::{Complex, ComplexExt, Lowpass, MulScaled}; use generic_array::typenum::U2; #[derive(Clone, Default)] diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index b40037f..3d7ff0b 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::{Accu, Complex, FastInt, Lockin, RPLL}; +use dsp::{Accu, Complex, ComplexExt, Lockin, RPLL}; use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, }; diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index 3539993..1f9df6e 100644 --- a/src/bin/lockin-internal.rs +++ b/src/bin/lockin-internal.rs @@ -2,7 +2,7 @@ #![no_std] #![no_main] -use dsp::{Accu, Complex, FastInt, Lockin}; +use dsp::{Accu, Complex, ComplexExt, Lockin}; use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; use stabilizer::{hardware, hardware::design_parameters}; From fcb7bb0025903bb19f66607ba5bb31752c8ea830 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 19 Feb 2021 09:50:38 +0100 Subject: [PATCH 9/9] dsp: fix nightly --- dsp/src/lib.rs | 1 + dsp/src/tools.rs | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 581f585..ac91124 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,4 +1,5 @@ #![cfg_attr(not(test), no_std)] +#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] mod tools; pub use tools::*; diff --git a/dsp/src/tools.rs b/dsp/src/tools.rs index 45dd66f..378734f 100644 --- a/dsp/src/tools.rs +++ b/dsp/src/tools.rs @@ -1,5 +1,3 @@ -#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] - use core::ops::{Add, Mul, Neg}; pub fn abs(x: T) -> T