pounder_test/src/iir.rs

77 lines
1.9 KiB
Rust
Raw Normal View History

2019-03-28 17:02:32 +08:00
use core::f32;
2019-03-28 02:48:22 +08:00
pub type IIRState = [f32; 5];
2019-03-30 02:33:32 +08:00
#[derive(Copy,Clone)]
2019-03-28 02:48:22 +08:00
pub struct IIR {
pub ba: IIRState,
2019-03-30 02:33:32 +08:00
pub y_offset: f32,
pub y_min: f32,
pub y_max: f32,
2019-03-28 02:48:22 +08:00
}
2019-03-28 23:10:04 +08:00
fn abs(x: f32) -> f32 {
if x >= 0. { x } else { -x }
}
fn copysign(x: f32, y: f32) -> f32 {
2019-03-30 02:33:32 +08:00
if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) {
x
} else {
-x
2019-03-28 23:10:04 +08:00
}
}
2019-03-30 02:33:32 +08:00
fn macc(y0: f32, x: &[f32], a: &[f32]) -> f32 {
2019-04-12 23:44:09 +08:00
y0 + x.iter().zip(a.iter()).map(|(&i, &j)| i * j).sum::<f32>()
2019-03-30 02:33:32 +08:00
}
2019-03-28 02:48:22 +08:00
impl IIR {
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);
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) };
2019-03-28 23:10:04 +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")
2019-03-28 17:02:32 +08:00
}
2019-03-28 23:10:04 +08:00
(a1, b0, b1)
2019-03-30 02:33:32 +08:00
};
2019-03-28 17:02:32 +08:00
self.ba[0] = b0;
self.ba[1] = b1;
self.ba[2] = 0.;
self.ba[3] = a1;
self.ba[4] = 0.;
Ok(())
}
2019-03-31 19:33:18 +08:00
pub fn get_x_offset(&self) -> Result<f32, &str> {
let b: f32 = self.ba[..3].iter().sum();
if abs(b) < f32::EPSILON {
return Err("b is zero")
}
Ok(self.y_offset/b)
}
2019-03-30 02:33:32 +08:00
pub fn set_x_offset(&mut self, xo: f32) {
let b: f32 = self.ba[..3].iter().sum();
self.y_offset = xo*b;
}
2019-03-28 02:48:22 +08:00
pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
xy.rotate_right(1);
2019-03-30 02:33:32 +08:00
xy[0] = x0;
2019-03-28 17:02:32 +08:00
let y0 = macc(self.y_offset, xy, &self.ba)
2019-03-30 02:33:32 +08:00
.max(self.y_min).min(self.y_max);
2019-03-28 02:48:22 +08:00
xy[xy.len()/2] = y0;
y0
}
}