Merge branch 'master' into feature/dma-updates

Cargo.lock

@@ -185,6 +185,13 @@ dependencies = [
  "cortex-m",
 ]
 
+[[package]]
+name = "dsp"
+version = "0.1.0"
+dependencies = [
+ "serde",
+]
+
 [[package]]
 name = "embedded-dma"
 version = "0.1.2"
@@ -466,6 +473,7 @@ dependencies = [
  "cortex-m-log",
  "cortex-m-rt",
  "cortex-m-rtic",
+ "dsp",
  "embedded-hal",
  "enum-iterator",
  "heapless",

Cargo.toml

@@ -41,6 +41,7 @@ nb = "1.0.0"
 asm-delay = "0.9.0"
 enum-iterator = "0.6.0"
 paste = "1"
+dsp = { path = "dsp" }
 
 [dependencies.mcp23017]
 git = "https://github.com/mrd0ll4r/mcp23017.git"

README.md

@@ -1,4 +1,4 @@
-![Continuous Integration](https://github.com/quartiq/stabilizer/workflows/Continuous%20Integration/badge.svg)
+[![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org)](https://matrix.to/#/#quartiq:matrix.org)
 
 # Stabilizer Firmware
 

dsp/Cargo.lock

@@ -0,0 +1,63 @@
+# This file is automatically @generated by Cargo.
+# It is not intended for manual editing.
+[[package]]
+name = "dsp"
+version = "0.1.0"
+dependencies = [
+ "serde",
+]
+
+[[package]]
+name = "proc-macro2"
+version = "1.0.24"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1e0704ee1a7e00d7bb417d0770ea303c1bccbabf0ef1667dae92b5967f5f8a71"
+dependencies = [
+ "unicode-xid",
+]
+
+[[package]]
+name = "quote"
+version = "1.0.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "aa563d17ecb180e500da1cfd2b028310ac758de548efdd203e18f283af693f37"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "serde"
+version = "1.0.117"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b88fa983de7720629c9387e9f517353ed404164b1e482c970a90c1a4aaf7dc1a"
+dependencies = [
+ "serde_derive",
+]
+
+[[package]]
+name = "serde_derive"
+version = "1.0.117"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cbd1ae72adb44aab48f325a02444a5fc079349a8d804c1fc922aed3f7454c74e"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "syn"
+version = "1.0.50"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "443b4178719c5a851e1bde36ce12da21d74a0e60b4d982ec3385a933c812f0f6"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-xid",
+]
+
+[[package]]
+name = "unicode-xid"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f7fe0bb3479651439c9112f72b6c505038574c9fbb575ed1bf3b797fa39dd564"

dsp/Cargo.toml

@@ -0,0 +1,8 @@
+[package]
+name = "dsp"
+version = "0.1.0"
+authors = ["Robert Jördens <rj@quartiq.de>"]
+edition = "2018"
+
+[dependencies]
+serde = { version = "1.0", features = ["derive"], default-features = false }

dsp/src/iir.rs

@@ -0,0 +1,176 @@
+use core::ops::{Add, Mul};
+use serde::{Deserialize, Serialize};
+
+use core::f32;
+
+// 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 abs(x: f32) -> f32 {
+    if x >= 0. {
+        x
+    } else {
+        -x
+    }
+}
+
+fn copysign(x: f32, y: f32) -> f32 {
+    if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) {
+        x
+    } else {
+        -x
+    }
+}
+
+fn max(x: f32, y: f32) -> f32 {
+    if x > y {
+        x
+    } else {
+        y
+    }
+}
+
+fn min(x: f32, y: f32) -> f32 {
+    if x < y {
+        x
+    } else {
+        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)
+}
+
+/// 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.
+/// 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.
+pub type IIRState = [f32; 5];
+
+/// IIR configuration.
+///
+/// Contains the coeeficients `ba`, the output offset `y_offset`, and the
+/// output limits `y_min` and `y_max`.
+///
+/// This implementation achieves several important properties:
+///
+/// * Its transfer function is universal in the sense that any biquadratic
+///   transfer function can be implemented (high-passes, gain limits, second
+///   order integrators with inherent anti-windup, notches etc) without code
+///   changes preserving all features.
+/// * It inherits a universal implementation of "integrator anti-windup", also
+///   and especially in the presence of set-point changes and in the presence
+///   of proportional or derivative gain without any back-off that would reduce
+///   steady-state output range.
+/// * It has universal derivative-kick (undesired, unlimited, and un-physical
+///   amplification of set-point changes by the derivative term) avoidance.
+/// * An offset at the input of an IIR filter (a.k.a. "set-point") is
+///   equivalent to an offset at the output. They are related by the
+///   overall (DC feed-forward) gain of the filter.
+/// * It stores only previous outputs and inputs. These have direct and
+///   invariant interpretation (independent of gains and offsets).
+///   Therefore it can trivially implement bump-less transfer.
+/// * Cascading multiple IIR filters allows stable and robust
+///   implementation of transfer functions beyond bequadratic terms.
+#[derive(Copy, Clone, Deserialize, Serialize)]
+pub struct IIR {
+    pub ba: IIRState,
+    pub y_offset: f32,
+    pub y_min: f32,
+    pub y_max: f32,
+}
+
+impl IIR {
+    /// Configures IIR filter coefficients for proportional-integral behavior
+    /// with gain limit.
+    ///
+    /// # Arguments
+    ///
+    /// * `kp` - Proportional gain. Also defines gain sign.
+    /// * `ki` - Integral gain at Nyquist. Sign taken from `kp`.
+    /// * `g` - Gain limit.
+    pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
+        let ki = copysign(ki, kp);
+        let g = copysign(g, kp);
+        let (a1, b0, b1) = if abs(ki) < f32::EPSILON {
+            (0., kp, 0.)
+        } else {
+            let c = if abs(g) < f32::EPSILON {
+                1.
+            } else {
+                1. / (1. + ki / g)
+            };
+            let a1 = 2. * c - 1.;
+            let b0 = ki * c + kp;
+            let b1 = ki * c - a1 * kp;
+            if abs(b0 + b1) < f32::EPSILON {
+                return Err("low integrator gain and/or gain limit");
+            }
+            (a1, b0, b1)
+        };
+        self.ba.copy_from_slice(&[b0, b1, 0., a1, 0.]);
+        Ok(())
+    }
+
+    /// Compute the overall (DC feed-forward) gain.
+    pub fn get_k(&self) -> f32 {
+        self.ba[..3].iter().sum()
+    }
+
+    /// Compute input-referred (`x`) offset from output (`y`) offset.
+    pub fn get_x_offset(&self) -> Result<f32, &str> {
+        let k = self.get_k();
+        if abs(k) < f32::EPSILON {
+            Err("k is zero")
+        } else {
+            Ok(self.y_offset / k)
+        }
+    }
+
+    /// Convert input (`x`) offset to equivalent output (`y`) offset and apply.
+    ///
+    /// # Arguments
+    /// * `xo`: Input (`x`) offset.
+    pub fn set_x_offset(&mut self, xo: f32) {
+        self.y_offset = xo * self.get_k();
+    }
+
+    /// Feed a new input value into the filter, update the filter state, and
+    /// return the new output. Only the state `xy` is modified.
+    ///
+    /// # Arguments
+    /// * `xy` - Current filter state.
+    /// * `x0` - New input.
+    pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
+        // `xy` contains       x0 x1 y0 y1 y2
+        // Increment time      x1 x2 y1 y2 y3
+        // Rotate              y3 x1 x2 y1 y2
+        xy.rotate_right(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;
+        y0
+    }
+}

dsp/src/lib.rs

@@ -0,0 +1,3 @@
+#![no_std]
+
+pub mod iir;

src/iir.rs

@@ -1,108 +0,0 @@
-use core::ops::{Add, Mul};
-use serde::{Deserialize, Serialize};
-
-use core::f32;
-
-pub type IIRState = [f32; 5];
-
-#[derive(Copy, Clone, Deserialize, Serialize)]
-pub struct IIR {
-    pub ba: IIRState,
-    pub y_offset: f32,
-    pub y_min: f32,
-    pub y_max: f32,
-}
-
-fn abs(x: f32) -> f32 {
-    if x >= 0. {
-        x
-    } else {
-        -x
-    }
-}
-
-fn copysign(x: f32, y: f32) -> f32 {
-    if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) {
-        x
-    } else {
-        -x
-    }
-}
-
-fn max(x: f32, y: f32) -> f32 {
-    if x > y {
-        x
-    } else {
-        y
-    }
-}
-
-fn min(x: f32, y: f32) -> f32 {
-    if x < y {
-        x
-    } else {
-        y
-    }
-}
-
-fn macc<T>(y0: T, x: &[T], a: &[T]) -> T
-where
-    T: Add<Output = T> + Mul<Output = T> + Copy,
-{
-    x.iter()
-        .zip(a.iter())
-        .map(|(&i, &j)| i * j)
-        .fold(y0, |y, xa| y + xa)
-}
-
-impl IIR {
-    pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
-        let ki = copysign(ki, kp);
-        let g = copysign(g, kp);
-        let (a1, b0, b1) = if abs(ki) < f32::EPSILON {
-            (0., kp, 0.)
-        } else {
-            let c = if abs(g) < f32::EPSILON {
-                1.
-            } else {
-                1. / (1. + ki / g)
-            };
-            let a1 = 2. * c - 1.;
-            let b0 = ki * c + kp;
-            let b1 = ki * c - a1 * kp;
-            if abs(b0 + b1) < f32::EPSILON {
-                return Err("low integrator gain and/or gain limit");
-            }
-            (a1, b0, b1)
-        };
-        self.ba[0] = b0;
-        self.ba[1] = b1;
-        self.ba[2] = 0.;
-        self.ba[3] = a1;
-        self.ba[4] = 0.;
-        Ok(())
-    }
-
-    pub fn get_x_offset(&self) -> Result<f32, &str> {
-        let b: f32 = self.ba[..3].iter().sum();
-        if abs(b) < f32::EPSILON {
-            Err("b is zero")
-        } else {
-            Ok(self.y_offset / b)
-        }
-    }
-
-    pub fn set_x_offset(&mut self, xo: f32) {
-        let b: f32 = self.ba[..3].iter().sum();
-        self.y_offset = xo * b;
-    }
-
-    pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
-        xy.rotate_right(1);
-        xy[0] = x0;
-        let y0 = macc(self.y_offset, xy, &self.ba);
-        let y0 = max(self.y_min, min(self.y_max, y0));
-        xy[xy.len() / 2] = y0;
-        y0
-    }
-}

src/main.rs

@@ -64,13 +64,13 @@ mod adc;
 mod afe;
 mod dac;
 mod eeprom;
-mod iir;
 mod pounder;
 mod sampling_timer;
 mod server;
 
 use adc::{Adc0Input, Adc1Input, AdcInputs};
 use dac::{Dac0Output, Dac1Output, DacOutputs};
+use dsp::iir;
 
 #[cfg(not(feature = "semihosting"))]
 fn init_log() {}