pounder_test/dsp/src/iir.rs

196 lines
5.9 KiB
Rust
Raw Normal View History

use core::ops::{Add, Mul, Neg};
2019-11-24 22:09:52 +08:00
use serde::{Deserialize, Serialize};
2019-04-16 19:27:11 +08:00
2020-12-17 08:01:50 +08:00
use super::abs;
2020-12-17 08:14:11 +08:00
use core::f32;
2019-03-28 17:02:32 +08:00
2020-11-23 05:58:17 +08:00
// 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.
2019-03-28 02:48:22 +08:00
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())
{
2019-03-30 02:33:32 +08:00
x
} else {
-x
2019-03-28 23:10:04 +08:00
}
}
2020-11-26 21:19:09 +08:00
#[cfg(not(feature = "nightly"))]
fn max<T>(x: T, y: T) -> T
where
T: PartialOrd,
{
2019-11-24 22:09:52 +08:00
if x > y {
x
} else {
y
}
2019-04-16 22:55:26 +08:00
}
2020-11-26 21:19:09 +08:00
#[cfg(not(feature = "nightly"))]
fn min<T>(x: T, y: T) -> T
where
T: PartialOrd,
{
2019-11-24 22:09:52 +08:00
if x < y {
x
} else {
y
}
2019-04-16 22:55:26 +08:00
}
2020-11-26 21:19:09 +08:00
#[cfg(feature = "nightly")]
fn max(x: f32, y: f32) -> f32 {
core::intrinsics::maxnumf32(x, y)
2020-11-26 21:19:09 +08:00
}
#[cfg(feature = "nightly")]
fn min(x: f32, y: f32) -> f32 {
core::intrinsics::minnumf32(x, y)
2020-11-26 21:19:09 +08:00
}
2020-11-23 05:58:17 +08:00
// Multiply-accumulate vectors `x` and `a`.
//
// A.k.a. dot product.
// Rust/LLVM optimize this nicely.
2019-04-16 19:27:11 +08:00
fn macc<T>(y0: T, x: &[T], a: &[T]) -> T
2019-11-24 22:09:52 +08:00
where
T: Add<Output = T> + Mul<Output = T> + Copy,
2019-04-16 19:27:11 +08:00
{
2019-11-24 22:09:52 +08:00
x.iter()
2020-11-23 05:58:17 +08:00
.zip(a)
.map(|(x, a)| *x * *a)
2019-11-24 22:09:52 +08:00
.fold(y0, |y, xa| y + xa)
2019-03-30 02:33:32 +08:00
}
2020-11-23 05:58:17 +08:00
/// 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. Lower indices correspond to more recent samples.
2020-11-23 05:58:17 +08:00
/// To represent the IIR coefficients, this contains the feed-forward
/// coefficients (b0, b1, b2) followd by the negated feed-back coefficients
/// (-a1, -a2), all five normalized such that a0 = 1.
2020-11-23 05:58:17 +08:00
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,
}
2019-03-28 02:48:22 +08:00
impl IIR {
2020-11-23 05:58:17 +08:00
/// 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.
2019-03-31 19:33:18 +08:00
pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
2019-03-30 02:33:32 +08:00
let ki = copysign(ki, kp);
let g = copysign(g, kp);
2019-11-24 22:09:52 +08:00
let (a1, b0, b1) = if abs(ki) < f32::EPSILON {
(0., kp, 0.)
} else {
let c = if abs(g) < f32::EPSILON {
1.
2019-03-30 02:33:32 +08:00
} else {
2019-11-24 22:09:52 +08:00
1. / (1. + ki / g)
2019-03-30 02:33:32 +08:00
};
2019-11-24 22:09:52 +08:00
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)
};
2020-11-23 05:58:17 +08:00
self.ba.copy_from_slice(&[b0, b1, 0., a1, 0.]);
2019-03-28 17:02:32 +08:00
Ok(())
}
2020-11-23 05:58:17 +08:00
/// 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.
2019-03-31 19:33:18 +08:00
pub fn get_x_offset(&self) -> Result<f32, &str> {
2020-11-23 05:58:17 +08:00
let k = self.get_k();
if abs(k) < f32::EPSILON {
Err("k is zero")
2019-04-16 19:27:11 +08:00
} else {
2020-11-23 05:58:17 +08:00
Ok(self.y_offset / k)
2019-03-31 19:33:18 +08:00
}
}
2020-11-23 05:58:17 +08:00
/// Convert input (`x`) offset to equivalent output (`y`) offset and apply.
///
/// # Arguments
/// * `xo`: Input (`x`) offset.
2019-03-30 02:33:32 +08:00
pub fn set_x_offset(&mut self, xo: f32) {
2020-11-23 05:58:17 +08:00
self.y_offset = xo * self.get_k();
2019-03-30 02:33:32 +08:00
}
2020-11-23 05:58:17 +08:00
/// 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.
2019-03-28 02:48:22 +08:00
pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
let n = self.ba.len();
debug_assert!(xy.len() == n);
2020-11-23 05:58:17 +08:00
// `xy` contains x0 x1 y0 y1 y2
// Increment time x1 x2 y1 y2 y3
2020-11-26 00:53:13 +08:00
// Shift x1 x1 x2 y1 y2
2020-11-26 01:55:07 +08:00
// This unrolls better than xy.rotate_right(1)
xy.copy_within(0..n - 1, 1);
2020-11-23 05:58:17 +08:00
// Store x0 x0 x1 x2 y1 y2
2019-03-30 02:33:32 +08:00
xy[0] = x0;
2020-11-23 05:58:17 +08:00
// Compute y0 by multiply-accumulate
2019-04-16 22:55:26 +08:00
let y0 = macc(self.y_offset, xy, &self.ba);
2020-11-23 05:58:17 +08:00
// Limit y0
2019-04-16 22:55:26 +08:00
let y0 = max(self.y_min, min(self.y_max, y0));
2020-11-23 05:58:17 +08:00
// Store y0 x0 x1 y0 y1 y2
xy[n / 2] = y0;
2019-03-28 02:48:22 +08:00
y0
}
}