From 74f10a7ac4bc5b17fc987a35fe05631bd8f0e13a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Thu, 28 Mar 2019 15:10:04 +0000
Subject: [PATCH] cleanup iir

---
 src/iir.rs  | 45 ++++++++++++++++++++++++---------------------
 src/main.rs |  4 ++--
 2 files changed, 26 insertions(+), 23 deletions(-)

diff --git a/src/iir.rs b/src/iir.rs
index 8acb288..76c7739 100644
--- a/src/iir.rs
+++ b/src/iir.rs
@@ -10,34 +10,37 @@ pub struct IIR {
     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 {
     pub fn pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
-        if ki < 0. {
-            return Err("ki < 0");
-        }
-        let (a1, b1, b0) = match () {
-            _ if ki < 0. => return Err("ki < 0"),
-            _ if ki < f32::EPSILON => (0., 0., kp),
+        let ki = copysign(kp, ki);
+        let g = copysign(kp, g);
+        let (a1, b0, b1) = match () {
+            _ if abs(ki) < f32::EPSILON => (0., kp, 0.),
             _ => {
-                let (c, a1) = match () {
-                    _ if g < 0. => return Err("g < 0"),
-                    _ if g < f32::EPSILON => (1., 1.),
-                    _ => {
-                        let c = 1./(1. + ki/g);
-                        (c, 2.*c - 1.)
-                    }
+                let c = match () {
+                    _ if abs(g) < f32::EPSILON => 1.,
+                    _ => 1./(1. + ki/g)
                 };
-                let b0 = kp + ki*c;
-                let b1 = b0 - 2.*kp*c;
-                if b0 + b1 > -f32::EPSILON && b0 + b1 < f32::EPSILON {
-                    return Err("low integrator gain and/or gain limit");
+                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, b1, b0)
+                (a1, b0, b1)
             }
         };
-        if b0 > 1. || b0 < -1. || b1 > 1. || b1 < -1. {
-            return Err("high gains");
-        }
         self.ba[0] = b0;
         self.ba[1] = b1;
         self.ba[2] = 0.;
diff --git a/src/main.rs b/src/main.rs
index b1342ca..27a2d56 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -542,8 +542,8 @@ fn main() -> ! {
     rcc.apb1lenr.modify(|_, w| w.tim2en().set_bit());
     tim2_setup(&dp.TIM2);
 
-    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[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"); }
 
     cortex_m::interrupt::free(|cs| {
         cp.NVIC.enable(stm32::Interrupt::SPI1);