diff --git a/src/output_channel.rs b/src/output_channel.rs index 3ac81c3..61e06be 100644 --- a/src/output_channel.rs +++ b/src/output_channel.rs @@ -9,26 +9,26 @@ use num_traits::Float; #[derive(Copy, Clone, Debug, Tree)] pub struct Pid { pub ki: f32, - pub kp: f32, + pub kp: f32, // sign reference for all gains and limits pub kd: f32, - pub li: Option, - pub ld: Option, + pub li: f32, + pub ld: f32, pub setpoint: f32, - pub min: Option, - pub max: Option, + pub min: f32, + pub max: f32, } impl Default for Pid { fn default() -> Self { Self { ki: 0., - kp: 0., // positive, sign reference for all gains and limits + kp: 0., // positive default kd: 0., - li: None, - ld: None, + li: f32::INFINITY, + ld: f32::INFINITY, setpoint: 25., - min: None, - max: None, + min: f32::NEG_INFINITY, + max: f32::INFINITY, } } } @@ -43,17 +43,35 @@ impl TryFrom for iir::Biquad { .gain(iir::Action::Kd, value.kd.copysign(value.kp) as _) .limit( iir::Action::Ki, - value.li.unwrap_or(f32::INFINITY).copysign(value.kp) as _, + if value.li.is_finite() { + value.li + } else { + f32::INFINITY + } + .copysign(value.kp) as _, ) .limit( iir::Action::Kd, - value.ld.unwrap_or(f32::INFINITY).copysign(value.kp) as _, + if value.ld.is_finite() { + value.ld + } else { + f32::INFINITY + } + .copysign(value.kp) as _, ) .build()? .into(); biquad.set_input_offset(-value.setpoint as _); - biquad.set_min(value.min.unwrap_or(f32::NEG_INFINITY) as _); - biquad.set_max(value.max.unwrap_or(f32::INFINITY) as _); + biquad.set_min(if value.min.is_finite() { + value.min + } else { + f32::NEG_INFINITY + } as _); + biquad.set_max(if value.max.is_finite() { + value.max + } else { + f32::INFINITY + } as _); Ok(biquad) } }