diff --git a/Cargo.lock b/Cargo.lock index 47e2086..b4bac53 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-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", diff --git a/Cargo.toml b/Cargo.toml index 44f2d02..301956c 100644 --- a/Cargo.toml +++ b/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" diff --git a/README.md b/README.md index d111f63..cf231c0 100644 --- a/README.md +++ b/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 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..625d0f0 --- /dev/null +++ b/dsp/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "dsp" +version = "0.1.0" +authors = ["Robert Jördens "] +edition = "2018" + +[dependencies] +serde = { version = "1.0", features = ["derive"], default-features = false } diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs new file mode 100644 index 0000000..fac1c4c --- /dev/null +++ b/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(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) +} + +/// 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 { + 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 + } +} 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/iir.rs b/src/iir.rs deleted file mode 100644 index 0c34306..0000000 --- a/src/iir.rs +++ /dev/null @@ -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(y0: T, x: &[T], a: &[T]) -> T -where - T: Add + Mul + 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 { - 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 - } -} diff --git a/src/main.rs b/src/main.rs index 6060d4f..89908f5 100644 --- a/src/main.rs +++ b/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() {}