kai/pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

cleanup iir

Browse Source
This commit is contained in:
Robert Jördens 2019-03-28 15:10:04 +00:00
parent dc2e0f8b07
commit 74f10a7ac4
2 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/iir.rs
View File

@ -10,34 +10,37 @@ pub struct IIR {
pub scale: f32, pub scale: f32,
} }
fn abs(x: f32) -> f32 {
if x >= 0. { x } else { -x }
}
fn copysign(x: f32, y: f32) -> f32 {
match () {
_ if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) => y,
_ => -y
}
}
impl IIR { impl IIR {
pub fn pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> { pub fn pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
if ki < 0. { let ki = copysign(kp, ki);
return Err("ki < 0"); let g = copysign(kp, g);
} let (a1, b0, b1) = match () {
let (a1, b1, b0) = match () { _ if abs(ki) < f32::EPSILON => (0., kp, 0.),
_ if ki < 0. => return Err("ki < 0"),
_ if ki < f32::EPSILON => (0., 0., kp),
_ => { _ => {
let (c, a1) = match () { let c = match () {
_ if g < 0. => return Err("g < 0"), _ if abs(g) < f32::EPSILON => 1.,
_ if g < f32::EPSILON => (1., 1.), _ => 1./(1. + ki/g)
_ => {
let c = 1./(1. + ki/g);
(c, 2.*c - 1.)
}
}; };
let b0 = kp + ki*c; let a1 = 2.*c - 1.;
let b1 = b0 - 2.*kp*c; let b0 = ki*c + kp;
if b0 + b1 > -f32::EPSILON && b0 + b1 < f32::EPSILON { let b1 = ki*c - a1*kp;
return Err("low integrator gain and/or gain limit"); if abs(b0 + b1) < f32::EPSILON {
return Err("low integrator gain and/or gain limit")
} }
(a1, b1, b0) (a1, b0, b1)
} }
}; };
if b0 > 1. || b0 < -1. || b1 > 1. || b1 < -1. {
return Err("high gains");
}
self.ba[0] = b0; self.ba[0] = b0;
self.ba[1] = b1; self.ba[1] = b1;
self.ba[2] = 0.; self.ba[2] = 0.;

4
src/main.rs
View File

@ -542,8 +542,8 @@ fn main() -> ! {
rcc.apb1lenr.modify(|_, w| w.tim2en().set_bit()); rcc.apb1lenr.modify(|_, w| w.tim2en().set_bit());
tim2_setup(&dp.TIM2); tim2_setup(&dp.TIM2);
unsafe { IIR_CH[0].pi(-0.1, 10.*2e-6/2., 0.).expect("bad coefficients"); } unsafe { IIR_CH[0].pi(0.1, 10.*2e-6/2., 0.).expect("bad coefficients"); }
unsafe { IIR_CH[1].pi(0.1, 10.*2e-6/2., 0.).expect("bad coefficients"); } unsafe { IIR_CH[1].pi(-0.1, 10.*2e-6/2., 0.).expect("bad coefficients"); }
cortex_m::interrupt::free(|cs| { cortex_m::interrupt::free(|cs| {
cp.NVIC.enable(stm32::Interrupt::SPI1); cp.NVIC.enable(stm32::Interrupt::SPI1);