Merge #275

275: use num crate r=jordens a=jordens

* Use `num` crate
* Clean up lowpass gain handling

* [x] test on hardware

Co-authored-by: Robert Jördens <rj@quartiq.de>

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"

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"

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::{atan2, cossin, iir, iir_int, PLL, RPLL};
-use dsp::{iir, iir_int};
-use dsp::{pll::PLL, rpll::RPLL};
 
 fn atan2_bench(c: &mut Criterion) {
     let xi = (10 << 16) as i32;

dsp/src/complex.rs

@@ -1,33 +1,29 @@
-use core::ops::Mul;
+pub use num::Complex;
 
 use super::{atan2, cossin};
 
-#[derive(Copy, Clone, Default, PartialEq, Debug)]
+/// Complex extension trait offering DSP (fast, good accuracy) functionality.
-pub struct Complex<T>(pub T, pub T);
+pub trait ComplexExt<T, U> {
-
+    fn from_angle(angle: T) -> Self;
-impl<T: Copy> Complex<T> {
+    fn abs_sqr(&self) -> U;
-    pub fn map<F>(&self, func: F) -> Self
+    fn log2(&self) -> T;
-    where
+    fn arg(&self) -> T;
-        F: Fn(T) -> T,
-    {
-        Complex(func(self.0), func(self.1))
-    }
 }
 
-impl Complex<i32> {
+impl ComplexExt<i32, u32> for Complex<i32> {
     /// Return a Complex on the unit circle given an angle.
     ///
     /// Example:
     ///
     /// ```
-    /// use dsp::Complex;
+    /// use dsp::{Complex, ComplexExt};
     /// 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 +35,13 @@ impl Complex<i32> {
     /// Example:
     ///
     /// ```
-    /// use dsp::Complex;
+    /// use dsp::{Complex, ComplexExt};
-    /// assert_eq!(Complex(i32::MIN, 0).abs_sqr(), 1 << 31);
+    /// assert_eq!(Complex::new(i32::MIN, 0).abs_sqr(), 1 << 31);
-    /// assert_eq!(Complex(i32::MAX, i32::MAX).abs_sqr(), u32::MAX - 3);
+    /// assert_eq!(Complex::new(i32::MAX, i32::MAX).abs_sqr(), u32::MAX - 3);
     /// ```
-    pub fn abs_sqr(&self) -> u32 {
+    fn abs_sqr(&self) -> u32 {
-        (((self.0 as i64) * (self.0 as i64)
+        (((self.re as i64) * (self.re as i64)
-            + (self.1 as i64) * (self.1 as i64))
+            + (self.im as i64) * (self.im as i64))
             >> 31) as u32
     }
 
@@ -59,15 +55,15 @@ impl Complex<i32> {
     /// Example:
     ///
     /// ```
-    /// use dsp::Complex;
+    /// use dsp::{Complex, ComplexExt};
-    /// assert_eq!(Complex(i32::MAX, i32::MAX).log2(), -1);
+    /// assert_eq!(Complex::new(i32::MAX, i32::MAX).log2(), -1);
-    /// assert_eq!(Complex(i32::MAX, 0).log2(), -2);
+    /// assert_eq!(Complex::new(i32::MAX, 0).log2(), -2);
-    /// assert_eq!(Complex(1, 0).log2(), -63);
+    /// assert_eq!(Complex::new(1, 0).log2(), -63);
-    /// assert_eq!(Complex(0, 0).log2(), -64);
+    /// assert_eq!(Complex::new(0, 0).log2(), -64);
     /// ```
-    pub fn log2(&self) -> i32 {
+    fn log2(&self) -> i32 {
-        let a = (self.0 as i64) * (self.0 as i64)
+        let a = (self.re as i64) * (self.re as i64)
-            + (self.1 as i64) * (self.1 as i64);
+            + (self.im as i64) * (self.im as i64);
         -(a.leading_zeros() as i32)
     }
 
@@ -78,52 +74,51 @@ impl Complex<i32> {
     /// Example:
     ///
     /// ```
-    /// use dsp::Complex;
+    /// use dsp::{Complex, ComplexExt};
-    /// assert_eq!(Complex(1, 0).arg(), 0);
+    /// assert_eq!(Complex::new(1, 0).arg(), 0);
-    /// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX);
+    /// assert_eq!(Complex::new(-i32::MAX, 1).arg(), i32::MAX);
-    /// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX);
+    /// assert_eq!(Complex::new(-i32::MAX, -1).arg(), -i32::MAX);
-    /// assert_eq!(Complex(0, -1).arg(), -i32::MAX >> 1);
+    /// assert_eq!(Complex::new(0, -1).arg(), -i32::MAX >> 1);
-    /// assert_eq!(Complex(0, 1).arg(), (i32::MAX >> 1) + 1);
+    /// assert_eq!(Complex::new(0, 1).arg(), (i32::MAX >> 1) + 1);
-    /// assert_eq!(Complex(1, 1).arg(), (i32::MAX >> 2) + 1);
+    /// assert_eq!(Complex::new(1, 1).arg(), (i32::MAX >> 2) + 1);
     /// ```
-    pub fn arg(&self) -> i32 {
+    fn arg(&self) -> i32 {
-        atan2(self.1, self.0)
+        atan2(self.im, self.re)
     }
 }
 
-impl Mul for Complex<i32> {
+/// Full scale fixed point multiplication.
-    type Output = Self;
+pub trait MulScaled<T> {
+    fn mul_scaled(self, other: T) -> Self;
+}
 
-    fn mul(self, other: Self) -> Self {
+impl MulScaled<Complex<i32>> for Complex<i32> {
-        let a = self.0 as i64;
+    fn mul_scaled(self, other: Self) -> Self {
-        let b = self.1 as i64;
+        let a = self.re as i64;
-        let c = other.0 as i64;
+        let b = self.im as i64;
-        let d = other.1 as i64;
+        let c = other.re as i64;
-        Complex(
+        let d = other.im as i64;
-            ((a * c - b * d + (1 << 31)) >> 32) as i32,
+        Complex {
-            ((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,
+        }
     }
 }
 
-impl Mul<i32> for Complex<i32> {
+impl MulScaled<i32> for Complex<i32> {
-    type Output = Self;
+    fn mul_scaled(self, other: i32) -> Self {
-
+        Complex {
-    fn mul(self, other: i32) -> Self {
+            re: ((other as i64 * self.re as i64 + (1 << 30)) >> 31) as i32,
-        Complex(
+            im: ((other as i64 * self.im as i64 + (1 << 30)) >> 31) as i32,
-            ((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 Mul<i16> for Complex<i32> {
+impl MulScaled<i16> for Complex<i32> {
-    type Output = Self;
+    fn mul_scaled(self, other: i16) -> Self {
-
+        Complex {
-    fn mul(self, other: i16) -> Self {
+            re: (other as i32 * (self.re >> 16) + (1 << 14)) >> 15,
-        Complex(
+            im: (other as i32 * (self.im >> 16) + (1 << 14)) >> 15,
-            (other as i32 * (self.0 >> 16) + (1 << 15)) >> 16,
+        }
-            (other as i32 * (self.1 >> 16) + (1 << 15)) >> 16,
-        )
     }
 }

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 rms_err = (0f64, 0f64);
-        let mut sum_err = Complex(0f64, 0f64);
+        let mut sum_err = (0f64, 0f64);
-        let mut max_err = Complex(0f64, 0f64);
+        let mut max_err = (0f64, 0f64);
-        let mut sum = Complex(0f64, 0f64);
+        let mut sum = (0f64, 0f64);
-        let mut demod = Complex(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());

dsp/src/lib.rs

@@ -1,101 +1,28 @@
 #![cfg_attr(not(test), no_std)]
 #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))]
 
-use core::ops::{Add, Mul, Neg};
+mod tools;
-
+pub use tools::*;
-fn abs<T>(x: T) -> T
-where
-    T: PartialOrd + Default + Neg<Output = T>,
-{
-    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<T>(x: T, y: T) -> T
-where
-    T: PartialOrd + Default + Neg<Output = T>,
-{
-    if (x >= T::default() && y >= T::default())
-        || (x <= T::default() && y <= T::default())
-    {
-        x
-    } else {
-        -x
-    }
-}
-
-#[cfg(not(feature = "nightly"))]
-fn max<T>(x: T, y: T) -> T
-where
-    T: PartialOrd,
-{
-    if x > y {
-        x
-    } else {
-        y
-    }
-}
-
-#[cfg(not(feature = "nightly"))]
-fn min<T>(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<T>(y0: T, x: &[T], a: &[T]) -> T
-where
-    T: Add<Output = T> + Mul<Output = T> + Copy,
-{
-    x.iter()
-        .zip(a)
-        .map(|(x, a)| *x * *a)
-        .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;
+mod lockin;
-pub mod lowpass;
+pub use lockin::*;
-pub mod pll;
+mod lowpass;
-pub mod rpll;
+pub use lowpass::*;
-pub mod unwrap;
+mod pll;
-
+pub use pll::*;
-pub use accu::Accu;
+mod rpll;
-pub use atan2::atan2;
+pub use rpll::*;
-pub use complex::Complex;
+mod unwrap;
-pub use cossin::cossin;
+pub use unwrap::*;
 
 #[cfg(test)]
 pub mod testing;

dsp/src/lockin.rs

@@ -1,4 +1,4 @@
-use super::{lowpass::Lowpass, Complex};
+use super::{Complex, ComplexExt, Lowpass, MulScaled};
 use generic_array::typenum::U2;
 
 #[derive(Clone, Default)]
@@ -8,19 +8,15 @@ 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: i32, phase: i32, k: u8) -> Complex<i32> {
-    pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex<i32> {
+        // Get the LO signal for demodulation and mix the sample;
-        // Get the LO signal for demodulation.
+        let mix = Complex::from_angle(phase).mul_scaled(sample);
-        let lo = Complex::from_angle(phase);
-
-        // Mix with the LO signal
-        let mix = lo * sample;
 
         // Filter with the IIR lowpass,
         // return IQ (in-phase and quadrature) data.
-        Complex(
+        Complex {
-            self.state[0].update(mix.0, k),
+            re: self.state[0].update(mix.re, k),
-            self.state[1].update(mix.1, k),
+            im: self.state[1].update(mix.im, k),
-        )
+        }
     }
 }

dsp/src/lowpass.rs

@@ -14,19 +14,20 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
     /// Update the filter with a new sample.
     ///
     /// # Args
-    /// * `x`: Input data, needs `k` bits headroom.
+    /// * `x`: Input data. Needs 1 bit headroom but will saturate cleanly beyond that.
-    /// * `k`: Log2 time constant, 0..31.
+    /// * `k`: Log2 time constant, 1..=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);
+        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 DF-II and the zeros at Nyquist.
+        // Note T-DF-I and the zeros at Nyquist.
-        let mut x = x << k;
+        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);
         }

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(

dsp/src/tools.rs

@@ -0,0 +1,95 @@
+use core::ops::{Add, Mul, Neg};
+
+pub fn abs<T>(x: T) -> T
+where
+    T: PartialOrd + Default + Neg<Output = T>,
+{
+    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<T>(x: T, y: T) -> T
+where
+    T: PartialOrd + Default + Neg<Output = T>,
+{
+    if (x >= T::default() && y >= T::default())
+        || (x <= T::default() && y <= T::default())
+    {
+        x
+    } else {
+        -x
+    }
+}
+
+#[cfg(not(feature = "nightly"))]
+pub fn max<T>(x: T, y: T) -> T
+where
+    T: PartialOrd,
+{
+    if x > y {
+        x
+    } else {
+        y
+    }
+}
+
+#[cfg(not(feature = "nightly"))]
+pub fn min<T>(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<T>(y0: T, x: &[T], a: &[T]) -> T
+where
+    T: Add<Output = T> + Mul<Output = T> + 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)
+    }
+}

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, Complex, ComplexExt, Lockin, RPLL};
 use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
@@ -112,22 +112,33 @@ const APP: () = {
         let sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 
-        let output = adc_samples[0]
+        let output: Complex<i32> = 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) << 16;
+                lockin.update(s, phase, time_constant)
             })
+            // Decimate
             .last()
-            .unwrap();
+            .unwrap()
+            * 2; // Full scale assuming the 2f component is gone.
 
-        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()],
+            Conf::PowerPhase => [(output.log2() << 24) as _, output.arg()],
-            "frequency_discriminator" => [pll_frequency as _, output.arg()],
+            Conf::FrequencyDiscriminator => [pll_frequency as _, output.arg()],
-            _ => [output.0, output.1],
+            Conf::Quadrature => [output.re, output.im],
         };
 
         // Convert to DAC data.

src/bin/lockin-internal.rs

@@ -2,7 +2,7 @@
 #![no_std]
 #![no_main]
 
-use dsp::{lockin::Lockin, Accu};
+use dsp::{Accu, Complex, ComplexExt, Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 use stabilizer::{hardware, hardware::design_parameters};
 
@@ -95,17 +95,19 @@ const APP: () = {
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        let output = adc_samples
+        let output: Complex<i32> = 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) << 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;