nan-fill-in for pid parameters

quartiq · Mar 19, 2024 · 05bb6d0 · 05bb6d0
1 parent e6950e2
commit 05bb6d0
Showing 1 changed file with 32 additions and 14 deletions.
diff --git 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<f32>,
-    pub ld: Option<f32>,
+    pub li: f32,
+    pub ld: f32,
     pub setpoint: f32,
-    pub min: Option<f32>,
-    pub max: Option<f32>,
+    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<Pid> for iir::Biquad<f64> {
             .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)
     }
 }