pounder_test/stabilizer/src/iir.rs

use core::ops::{Add, Mul};
use serde::{Deserialize, Serialize};

use core::f32;

pub type IIRState = [f32; 5];

#[derive(Copy, Clone, Deserialize, Serialize)]
pub struct IIR {
    pub ba: IIRState,
    pub y_offset: f32,
    pub y_min: f32,
    pub y_max: f32,
}

fn abs(x: f32) -> f32 {
    if x >= 0. {
        x
    } else {
        -x
    }
}

fn copysign(x: f32, y: f32) -> f32 {
    if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) {
        x
    } else {
        -x
    }
}

fn max(x: f32, y: f32) -> f32 {
    if x > y {
        x
    } else {
        y
    }
}

fn min(x: f32, y: f32) -> f32 {
    if x < y {
        x
    } else {
        y
    }
}

fn macc<T>(y0: T, x: &[T], a: &[T]) -> T
where
    T: Add<Output = T> + Mul<Output = T> + Copy,
{
    x.iter()
        .zip(a.iter())
        .map(|(&i, &j)| i * j)
        .fold(y0, |y, xa| y + xa)
}

impl IIR {
    pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &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;
            if abs(b0 + b1) < f32::EPSILON {
                return Err("low integrator gain and/or gain limit");
            }
            (a1, b0, b1)
        };
        self.ba[0] = b0;
        self.ba[1] = b1;
        self.ba[2] = 0.;
        self.ba[3] = a1;
        self.ba[4] = 0.;
        Ok(())
    }

    pub fn get_x_offset(&self) -> Result<f32, &str> {
        let b: f32 = self.ba[..3].iter().sum();
        if abs(b) < f32::EPSILON {
            Err("b is zero")
        } else {
            Ok(self.y_offset / b)
        }
    }

    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;
        let y0 = macc(self.y_offset, xy, &self.ba);
        let y0 = max(self.y_min, min(self.y_max, y0));
        xy[xy.len() / 2] = y0;
        y0
    }
}