pounder_test/src/iir.rs
2019-11-24 15:10:01 +01:00

109 lines
2.3 KiB
Rust

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
}
}