From 04a0462aad49ac5e0e6393516d26e4ebc87fb1c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 16 Nov 2020 17:33:14 +0100 Subject: [PATCH 1/5] README: add matrix badge --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index d111f63..cafbb78 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ ![Continuous Integration](https://github.com/quartiq/stabilizer/workflows/Continuous%20Integration/badge.svg) +![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org?style=plastic) # Stabilizer Firmware From a7b6b5c796c09c15927e98c8553d0e80c2593d3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 16 Nov 2020 17:36:16 +0100 Subject: [PATCH 2/5] README: remove CI badge, add matrix link --- README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/README.md b/README.md index cafbb78..cf231c0 100644 --- a/README.md +++ b/README.md @@ -1,5 +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?style=plastic) +[![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org)](https://matrix.to/#/#quartiq:matrix.org) # Stabilizer Firmware From 3eb43c6b99107486349acf2ad35e1a6a9ee79267 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 07:45:32 -0800 Subject: [PATCH 3/5] move iir to new dsp crate --- .gitignore | 2 +- Cargo.lock | 8 ++++++ Cargo.toml | 1 + dsp/Cargo.lock | 63 +++++++++++++++++++++++++++++++++++++++++ dsp/Cargo.toml | 10 +++++++ {src => dsp/src}/iir.rs | 0 dsp/src/lib.rs | 3 ++ src/main.rs | 3 +- 8 files changed, 88 insertions(+), 2 deletions(-) create mode 100644 dsp/Cargo.lock create mode 100644 dsp/Cargo.toml rename {src => dsp/src}/iir.rs (100%) create mode 100644 dsp/src/lib.rs diff --git a/.gitignore b/.gitignore index 265b7f5..98097d7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,2 @@ -/target +**/target .gdb_history diff --git a/Cargo.lock b/Cargo.lock index 014b427..f3ace36 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -185,6 +185,13 @@ dependencies = [ "cortex-m", ] +[[package]] +name = "dsp" +version = "0.1.0" +dependencies = [ + "serde", +] + [[package]] name = "embedded-hal" version = "0.2.4" @@ -457,6 +464,7 @@ dependencies = [ "cortex-m-log", "cortex-m-rt", "cortex-m-rtic", + "dsp", "embedded-hal", "enum-iterator", "heapless", diff --git a/Cargo.toml b/Cargo.toml index a57745e..886f041 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,6 +40,7 @@ embedded-hal = "0.2.4" nb = "1.0.0" asm-delay = "0.9.0" enum-iterator = "0.6.0" +dsp = { path = "dsp", version = "0.1.0" } [dependencies.mcp23017] git = "https://github.com/mrd0ll4r/mcp23017.git" diff --git a/dsp/Cargo.lock b/dsp/Cargo.lock new file mode 100644 index 0000000..afad0c4 --- /dev/null +++ b/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" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml new file mode 100644 index 0000000..a9b7fe5 --- /dev/null +++ b/dsp/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "dsp" +version = "0.1.0" +authors = ["Robert Jördens "] +edition = "2018" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +serde = { version = "1.0", features = ["derive"], default-features = false } diff --git a/src/iir.rs b/dsp/src/iir.rs similarity index 100% rename from src/iir.rs rename to dsp/src/iir.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs new file mode 100644 index 0000000..3c44bbc --- /dev/null +++ b/dsp/src/lib.rs @@ -0,0 +1,3 @@ +#![no_std] + +pub mod iir; diff --git a/src/main.rs b/src/main.rs index a7d3d73..4c945e5 100644 --- a/src/main.rs +++ b/src/main.rs @@ -54,10 +54,11 @@ static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); mod afe; mod eeprom; -mod iir; mod pounder; mod server; +use dsp::iir; + #[cfg(not(feature = "semihosting"))] fn init_log() {} From d24dfb302e2770022d99a273af6c4862df2c0feb Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 10:32:40 -0800 Subject: [PATCH 4/5] dsp crate: drop version dependency and remove boilerplate from cargo --- .gitignore | 2 +- Cargo.toml | 2 +- dsp/Cargo.toml | 2 -- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.gitignore b/.gitignore index 98097d7..265b7f5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,2 @@ -**/target +/target .gdb_history diff --git a/Cargo.toml b/Cargo.toml index 886f041..7aad310 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,7 +40,7 @@ embedded-hal = "0.2.4" nb = "1.0.0" asm-delay = "0.9.0" enum-iterator = "0.6.0" -dsp = { path = "dsp", version = "0.1.0" } +dsp = { path = "dsp" } [dependencies.mcp23017] git = "https://github.com/mrd0ll4r/mcp23017.git" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index a9b7fe5..625d0f0 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -4,7 +4,5 @@ version = "0.1.0" authors = ["Robert Jördens "] edition = "2018" -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - [dependencies] serde = { version = "1.0", features = ["derive"], default-features = false } From 6808d32e0fcb3cc02c92813bfa74d09dcfe8f2e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 22 Nov 2020 22:58:17 +0100 Subject: [PATCH 5/5] iir: document --- dsp/src/iir.rs | 112 +++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 90 insertions(+), 22 deletions(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 0c34306..fac1c4c 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -3,15 +3,10 @@ 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, -} +// 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. { @@ -45,17 +40,72 @@ fn min(x: f32, y: f32) -> f32 { } } +// 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.iter()) - .map(|(&i, &j)| i * j) + .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); @@ -75,33 +125,51 @@ impl IIR { } (a1, b0, b1) }; - self.ba[0] = b0; - self.ba[1] = b1; - self.ba[2] = 0.; - self.ba[3] = a1; - self.ba[4] = 0.; + 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 { - let b: f32 = self.ba[..3].iter().sum(); - if abs(b) < f32::EPSILON { - Err("b is zero") + let k = self.get_k(); + if abs(k) < f32::EPSILON { + Err("k is zero") } else { - Ok(self.y_offset / b) + 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) { - let b: f32 = self.ba[..3].iter().sum(); - self.y_offset = xo * b; + 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 }