AC_Avoidance: constrain Z backup to set back up speed and pos control…

… limits
ArduPilot · Apr 23, 2024 · 156e21c · 156e21c
1 parent b78f966
commit 156e21c
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 1 deletion.
diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp
@@ -25,6 +25,7 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <stdio.h>
+#include <AC_AttitudeControl/AC_PosControl.h>
 
 #if !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
 
@@ -422,7 +423,18 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
         if (alt_diff <= 0.0f) {
             climb_rate_cms = MIN(climb_rate_cms, 0.0f);
             // also calculate backup speed that will get us back to safe altitude
-            backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -alt_diff*100.0f, dt));
+            const float max_back_spd_cms = _backup_speed_max * 100.0;
+            if (is_positive(max_back_spd_cms)) {
+                backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -alt_diff*100.0f, dt));
+
+                // Constrain to max backup speed
+                backup_speed = MAX(backup_speed, -max_back_spd_cms);
+                // Ensure still within pos control limits
+                if (_pos_control != nullptr) {
+                    climb_rate_cms = constrain_float(climb_rate_cms, _pos_control->get_max_speed_down_cms(), _pos_control->get_max_speed_up_cms());
+                }
+                return;
+            }
             return;
         }
 

diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h
@@ -113,6 +113,9 @@ class AC_Avoid {
     // return true if limiting is active
     bool limits_active() const {return (AP_HAL::millis() - _last_limit_time) < AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS;};
 
+    // Set the pos control pointer which is used to get velocity limits
+    void set_pos_control(AC_PosControl *pos_control) { _pos_control = pos_control; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -226,6 +229,9 @@ class AC_Avoid {
     uint32_t _last_log_ms;          // the last time simple avoidance was logged
     Vector3f _prev_avoid_vel;       // copy of avoidance adjusted velocity
 
+    // Pointer to pos control to allow getting of velocity limits
+    AC_PosControl *_pos_control;
+
     static AC_Avoid *_singleton;
 };