AC_PID: apply PD limit in such that limit flag is set and reported P …

…and D terms are correct
lthall · Sep 12, 2023 · e7364a0 · e7364a0
1 parent 8161905
commit e7364a0
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 8 deletions.
diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp
@@ -172,15 +172,23 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
     P_out *= boost;
     D_out *= boost;
 
+    // Apply PD sum limit if enabled
+    if (is_positive(_kpdmax)) {
+        const float PD_sum_abs = fabsf(P_out + D_out);
+        if (PD_sum_abs > _kpdmax) {
+            const float PD_scale = _kpdmax / PD_sum_abs;
+            P_out *= PD_scale;
+            D_out *= PD_scale;
+            _pid_info.PD_limit = true;
+        }
+    }
+
     _pid_info.target = _target;
     _pid_info.actual = measurement;
     _pid_info.error = _error;
     _pid_info.P = P_out;
     _pid_info.D = D_out;
 
-    if (is_positive(_kpdmax)) {
-        return constrain_float(P_out + D_out, -_kpdmax, _kpdmax) + _integrator;
-    }
     return P_out + D_out + _integrator;
 }
 
@@ -227,16 +235,24 @@ float AC_PID::update_error(float error, float dt, bool limit)
 
     P_out *= _pid_info.Dmod;
     D_out *= _pid_info.Dmod;
-
+
+    // Apply PD sum limit if enabled
+    if (is_positive(_kpdmax)) {
+        const float PD_sum_abs = fabsf(P_out + D_out);
+        if (PD_sum_abs > _kpdmax) {
+            const float PD_scale = _kpdmax / PD_sum_abs;
+            P_out *= PD_scale;
+            D_out *= PD_scale;
+            _pid_info.PD_limit = true;
+        }
+    }
+
     _pid_info.target = 0.0f;
     _pid_info.actual = 0.0f;
     _pid_info.error = _error;
     _pid_info.P = P_out;
     _pid_info.D = D_out;
 
-    if (is_positive(_kpdmax)) {
-        return constrain_float(P_out + D_out, -_kpdmax, _kpdmax) + _integrator;
-    }
     return P_out + D_out + _integrator;
 }
 

diff --git a/libraries/AC_PID/AP_PIDInfo.h b/libraries/AC_PID/AP_PIDInfo.h
@@ -16,5 +16,6 @@ struct AP_PIDInfo {
     float FF;
     float Dmod;
     float slew_rate;
-    bool  limit;
+    bool limit;
+    bool PD_limit;
 };