Feature/iir tweaks

.cargo/config

@@ -1,6 +1,16 @@
 [target.'cfg(all(target_arch = "arm", target_os = "none"))']
 runner = "gdb-multiarch -q -x openocd.gdb"
-rustflags = ["-C", "link-arg=-Tlink.x"]
+rustflags = [
+    "-C", "link-arg=-Tlink.x",
+# The target (below) defaults to cortex-m4
+# There currently are two different options to go beyond that:
+# 1. cortex-m7 has the right flags and instructions (FPU) but no instruction schedule yet
+    "-C", "target-cpu=cortex-m7",
+# 2. cortex-m4 with the additional fpv5 instructions and a potentially
+# better-than-nothing instruction schedule
+    "-C", "target-feature=+fp-armv8d16",
+# When combined they are equivalent to (1) alone
+]
 
 [build]
 target = "thumbv7em-none-eabihf"

Cargo.toml

@@ -62,7 +62,7 @@ branch = "dma"
 [features]
 semihosting = ["panic-semihosting", "cortex-m-log/semihosting"]
 bkpt = [ ]
-nightly = ["cortex-m/inline-asm"]
+nightly = ["cortex-m/inline-asm", "dsp/nightly"]
 
 [profile.dev]
 codegen-units = 1

dsp/Cargo.toml

@@ -6,3 +6,6 @@ edition = "2018"
 
 [dependencies]
 serde = { version = "1.0", features = ["derive"], default-features = false }
+
+[features]
+nightly = []

dsp/src/iir.rs

@@ -1,4 +1,4 @@
-use core::ops::{Add, Mul};
+use core::ops::{Add, Mul, Neg};
 use serde::{Deserialize, Serialize};
 
 use core::f32;
@@ -8,38 +8,64 @@ use core::f32;
 // `compiler-intrinsics`/llvm should have better (robust, universal, and
 // faster) implementations.
 
-fn abs(x: f32) -> f32 {
-    if x >= 0. {
+fn abs<T>(x: T) -> T
+where
+    T: PartialOrd + Default + Neg<Output = T>,
+{
+    if x >= T::default() {
         x
     } else {
         -x
     }
 }
 
-fn copysign(x: f32, y: f32) -> f32 {
-    if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) {
+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
     }
 }
 
-fn max(x: f32, y: f32) -> f32 {
+#[cfg(not(feature = "nightly"))]
+fn max<T>(x: T, y: T) -> T
+where
+    T: PartialOrd,
+{
     if x > y {
         x
     } else {
         y
     }
 }
 
-fn min(x: f32, y: f32) -> f32 {
+#[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.
@@ -50,18 +76,18 @@ where
 {
     x.iter()
         .zip(a)
-        .map(|(&x, &a)| x * a)
+        .map(|(x, a)| *x * *a)
         .fold(y0, |y, xa| y + xa)
 }
 
 /// IIR state and coefficients type.
 ///
 /// To represent the IIR state (input and output memory) during the filter update
 /// this contains the three inputs (x0, x1, x2) and the two outputs (y1, y2)
-/// concatenated.
+/// concatenated. Lower indices correspond to more recent samples.
 /// To represent the IIR coefficients, this contains the feed-forward
-/// coefficients (b0, b1, b2) followd by the feed-back coefficients (a1, a2),
-/// all normalized such that a0 = 1.
+/// coefficients (b0, b1, b2) followd by the negated feed-back coefficients
+/// (-a1, -a2), all five normalized such that a0 = 1.
 pub type IIRState = [f32; 5];
 
 /// IIR configuration.
@@ -159,18 +185,21 @@ impl IIR {
     /// * `xy` - Current filter state.
     /// * `x0` - New input.
     pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
+        let n = self.ba.len();
+        debug_assert!(xy.len() == n);
         // `xy` contains       x0 x1 y0 y1 y2
         // Increment time      x1 x2 y1 y2 y3
-        // Rotate              y3 x1 x2 y1 y2
-        xy.rotate_right(1);
+        // Shift               x1 x1 x2 y1 y2
+        // This unrolls better than xy.rotate_right(1)
+        xy.copy_within(0..n - 1, 1);
         // Store x0            x0 x1 x2 y1 y2
         xy[0] = x0;
         // Compute y0 by multiply-accumulate
         let y0 = macc(self.y_offset, xy, &self.ba);
         // Limit y0
         let y0 = max(self.y_min, min(self.y_max, y0));
         // Store y0            x0 x1 y0 y1 y2
-        xy[xy.len() / 2] = y0;
+        xy[n / 2] = y0;
         y0
     }
 }

dsp/src/lib.rs

@@ -1,3 +1,4 @@
 #![no_std]
+#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))]
 
 pub mod iir;

src/main.rs

@@ -13,6 +13,9 @@
 fn panic(_info: &core::panic::PanicInfo) -> ! {
     let gpiod = unsafe { &*hal::stm32::GPIOD::ptr() };
     gpiod.odr.modify(|_, w| w.odr6().high().odr12().high()); // FP_LED_1, FP_LED_3
+    #[cfg(feature = "nightly")]
+    core::intrinsics::abort();
+    #[cfg(not(feature = "nightly"))]
     unsafe {
         core::intrinsics::abort();
     }
@@ -760,14 +763,22 @@ const APP: () = {
                 let x0 = f32::from(*adc0 as i16);
                 let y0 = c.resources.iir_ch[0]
                     .update(&mut c.resources.iir_state[0], x0);
-                y0 as i16 as u16 ^ 0x8000
+                // Note(unsafe): The filter limits ensure that the value is in range.
+                // The truncation introduces 1/2 LSB distortion.
+                let y0 = unsafe { y0.to_int_unchecked::<i16>() };
+                // Convert to DAC code
+                y0 as u16 ^ 0x8000
             };
 
             dac1[i] = {
                 let x1 = f32::from(*adc1 as i16);
                 let y1 = c.resources.iir_ch[1]
                     .update(&mut c.resources.iir_state[1], x1);
-                y1 as i16 as u16 ^ 0x8000
+                // Note(unsafe): The filter limits ensure that the value is in range.
+                // The truncation introduces 1/2 LSB distortion.
+                let y1 = unsafe { y1.to_int_unchecked::<i16>() };
+                // Convert to DAC code
+                y1 as u16 ^ 0x8000
             };
         }