add get_x_offset

This commit is contained in:
Robert Jördens 2019-03-31 11:33:18 +00:00
parent d7e47b7ebf
commit 5369b928c1
2 changed files with 10 additions and 1 deletions

View File

@ -27,7 +27,7 @@ fn macc(y0: f32, x: &[f32], a: &[f32]) -> f32 {
} }
impl IIR { impl IIR {
pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &'static str> { pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> {
let ki = copysign(ki, kp); let ki = copysign(ki, kp);
let g = copysign(g, kp); let g = copysign(g, kp);
let (a1, b0, b1) = let (a1, b0, b1) =
@ -52,6 +52,14 @@ impl IIR {
Ok(()) Ok(())
} }
pub fn get_x_offset(&self) -> Result<f32, &str> {
let b: f32 = self.ba[..3].iter().sum();
if abs(b) < f32::EPSILON {
return Err("b is zero")
}
Ok(self.y_offset/b)
}
pub fn set_x_offset(&mut self, xo: f32) { pub fn set_x_offset(&mut self, xo: f32) {
let b: f32 = self.ba[..3].iter().sum(); let b: f32 = self.ba[..3].iter().sum();
self.y_offset = xo*b; self.y_offset = xo*b;

View File

@ -563,6 +563,7 @@ fn main() -> ! {
IIR_CH[1].set_pi(-0.1, -10.*t, 0.).expect("bad coefficients"); IIR_CH[1].set_pi(-0.1, -10.*t, 0.).expect("bad coefficients");
IIR_CH[1].set_x_offset(0.1*SCALE); IIR_CH[1].set_x_offset(0.1*SCALE);
IIR_CH[1].get_x_offset().expect("bad coefficients");
} }
cortex_m::interrupt::free(|cs| { cortex_m::interrupt::free(|cs| {