pounder_test/dsp/src/iir.rs

109 lines
2.3 KiB
Rust
Raw Normal View History

2019-04-16 19:27:11 +08:00
use core::ops::{Add, Mul};
2019-11-24 22:09:52 +08:00
use serde::{Deserialize, Serialize};
2019-04-16 19:27:11 +08:00
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-11-24 22:09:52 +08:00
#[derive(Copy, Clone, Deserialize, Serialize)]
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 {
2019-11-24 22:09:52 +08:00
if x >= 0. {
x
} else {
-x
}
2019-03-28 23:10:04 +08:00
}
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-04-16 22:55:26 +08:00
fn max(x: f32, y: f32) -> f32 {
2019-11-24 22:09:52 +08:00
if x > y {
x
} else {
y
}
2019-04-16 22:55:26 +08:00
}
fn min(x: f32, y: f32) -> f32 {
2019-11-24 22:09:52 +08:00
if x < y {
x
} else {
y
}
2019-04-16 22:55:26 +08:00
}
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()
.zip(a.iter())
.map(|(&i, &j)| i * j)
.fold(y0, |y, xa| y + xa)
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);
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)
};
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 {
2019-04-16 19:27:11 +08:00
Err("b is zero")
} else {
2019-11-24 22:09:52 +08:00
Ok(self.y_offset / b)
2019-03-31 19:33:18 +08:00
}
}
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();
2019-11-24 22:09:52 +08:00
self.y_offset = xo * b;
2019-03-30 02:33:32 +08:00
}
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-04-16 22:55:26 +08:00
let y0 = macc(self.y_offset, xy, &self.ba);
let y0 = max(self.y_min, min(self.y_max, y0));
2019-11-24 22:09:52 +08:00
xy[xy.len() / 2] = y0;
2019-03-28 02:48:22 +08:00
y0
}
}