iir: tweak, add offset

openocd.gdb

@@ -19,10 +19,11 @@ load
 # monitor reset halt
 
 # cycle counter delta tool, place two bkpts around the section
+set var $cc=0xe0001004
 define qq
-print *0xe0001004-$t0
-set var $t0=*0xe0001004
+print *$cc-$t0
+set var $t0=*$cc
 continue
 end
-set var $t0=0xe0001004
+set var $t0=*$cc
 continue

src/iir.rs

@@ -2,12 +2,12 @@ use core::f32;
 
 pub type IIRState = [f32; 5];
 
-#[derive(Default,Copy,Clone,Debug)]
+#[derive(Copy,Clone)]
 pub struct IIR {
-    pub x_offset: f32,
-    pub y_offset: f32,
     pub ba: IIRState,
-    pub scale: f32,
+    pub y_offset: f32,
+    pub y_min: f32,
+    pub y_max: f32,
 }
 
 fn abs(x: f32) -> f32 {
@@ -15,23 +15,27 @@ fn abs(x: f32) -> f32 {
 }
 
 fn copysign(x: f32, y: f32) -> f32 {
-    match () {
-        _ if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) => y,
-        _ => -y
+    if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) {
+        x
+    } else {
+        -x
     }
 }
 
+fn macc(y0: f32, x: &[f32], a: &[f32]) -> f32 {
+    y0 + x.iter().zip(a.iter()) .map(|(&i, &j)| i * j).sum::<f32>()
+}
+
 impl IIR {
-    pub fn pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
-        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 = match () {
-                    _ if abs(g) < f32::EPSILON => 1.,
-                    _ => 1./(1. + ki/g)
-                };
+    pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &'static 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;
@@ -39,8 +43,7 @@ impl IIR {
                     return Err("low integrator gain and/or gain limit")
                 }
                 (a1, b0, b1)
-            }
-        };
+            };
         self.ba[0] = b0;
         self.ba[1] = b1;
         self.ba[2] = 0.;
@@ -49,17 +52,17 @@ impl IIR {
         Ok(())
     }
 
+    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 + self.x_offset;
+        xy[0] = x0;
         let y0 = macc(self.y_offset, xy, &self.ba)
-            .min(self.scale).max(-self.scale);
+            .max(self.y_min).min(self.y_max);
         xy[xy.len()/2] = y0;
         y0
     }
 }
-
-fn macc(y0: f32, x: &[f32], a: &[f32]) -> f32 {
-    y0 + x.iter().zip(a.iter())
-        .map(|(&i, &j)| i * j).sum::<f32>()
-}

src/main.rs

@@ -556,8 +556,14 @@ 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 {
+        let t = 2e-6*2.;
+        IIR_CH[0].set_pi(1., 0., 0.).expect("bad coefficients");
+        IIR_CH[0].set_x_offset(0.*SCALE);
+
+        IIR_CH[1].set_pi(-0.1, -10.*t, 0.).expect("bad coefficients");
+        IIR_CH[1].set_x_offset(0.1*SCALE);
+    }
 
     cortex_m::interrupt::free(|cs| {
         cp.NVIC.enable(stm32::Interrupt::SPI1);
@@ -584,8 +590,8 @@ fn TIM2() {  // FIXME
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 static mut IIR_STATE: [IIRState; 2] = [[0.; 5]; 2];
 static mut IIR_CH: [IIR; 2] = [
-    IIR{ y_offset: 0., x_offset: 0., ba: [0., 0., 0., 0., 0.], scale: SCALE },
-    IIR{ y_offset: 0., x_offset: 0., ba: [0., 0., 0., 0., 0.], scale: SCALE }];
+    IIR{ ba: [0., 0., 0., 0., 0.], y_offset: 0.,
+         y_min: -SCALE, y_max: SCALE }; 2];
 
 #[interrupt]
 fn SPI1() {
@@ -594,8 +600,8 @@ fn SPI1() {
     cortex_m::interrupt::free(|cs| {
         let spip = SPIP.borrow(cs).borrow();
         let (spi1, spi2, spi4, spi5) = spip.as_ref().unwrap();
-        let sr = spi1.sr.read();
 
+        let sr = spi1.sr.read();
         if sr.eot().bit_is_set() {
            spi1.ifcr.write(|w| w.eotc().set_bit());
         }