Copter: fix unconstrained avoidance backup speed

Tools/autotest/arducopter.py

@@ -1604,6 +1604,58 @@ def MaxAltFence(self):
 
         self.zero_throttle()
 
+    # MaxAltFence - fly up and make sure fence action does not trigger
+    # Also check that the vehicle will not try and descend too fast when trying to backup from a max alt fence due to avoidance
+    def MaxAltFenceAvoid(self):
+        '''Test Max Alt Fence Avoidance'''
+        self.takeoff(10, mode="LOITER")
+        """Hold loiter position."""
+
+        # enable fence, only max altitude, defualt is 100m
+        # No action, rely on avoidance to prevent the breach
+        self.set_parameters({
+            "FENCE_ENABLE": 1,
+            "FENCE_TYPE": 1,
+            "FENCE_ACTION": 0,
+        })
+
+        # Try and fly past the fence
+        self.set_rc(3, 1920)
+
+        # Avoid should prevent the vehicle flying past the fence, so the altitude wait should timeouts
+        try:
+            self.wait_altitude(140, 150, timeout=90, relative=True)
+            raise NotAchievedException("Avoid should prevent reaching altitude")
+        except AutoTestTimeoutException:
+            pass
+        except Exception as e:
+            raise e
+
+        # Check descent is not too fast, allow 10% above the configured backup speed
+        max_descent_rate = -self.get_parameter("AVOID_BACKUP_SPD") * 1.1
+
+        def get_climb_rate(mav, m):
+            m_type = m.get_type()
+            if m_type != 'VFR_HUD':
+                return
+            if m.climb < max_descent_rate:
+                raise NotAchievedException("Decending too fast want %f got %f" % (max_descent_rate, m.climb))
+
+        self.context_push()
+        self.install_message_hook_context(get_climb_rate)
+
+        # Reduce fence alt, this will result in a fence breach, but there is no action.
+        # Avoid should then backup the vehicle to be under the new fence alt.
+        self.set_parameters({
+            "FENCE_ALT_MAX": 50,
+        })
+        self.wait_altitude(40, 50, timeout=90, relative=True)
+
+        self.context_pop()
+
+        self.set_rc(3, 1500)
+        self.do_RTL()
+
     # fly_alt_min_fence_test - fly down until you hit the fence floor
     def MinAltFence(self):
         '''Test Min Alt Fence'''
@@ -10188,6 +10240,7 @@ def tests1d(self):
              self.HorizontalFence,
              self.HorizontalAvoidFence,
              self.MaxAltFence,
+             self.MaxAltFenceAvoid,
              self.MinAltFence,
              self.FenceFloorEnabledLanding,
              self.AutoTuneSwitch,

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -80,12 +80,12 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
     AP_GROUPINFO_FRAME("BEHAVE", 5, AC_Avoid, _behavior, AP_AVOID_BEHAVE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_ROVER),
 
     // @Param: BACKUP_SPD
-    // @DisplayName: Avoidance maximum backup speed
-    // @Description: Maximum speed that will be used to back away from obstacles in GPS modes (m/s). Set zero to disable
+    // @DisplayName: Avoidance maximum horizontal backup speed
+    // @Description: Maximum speed that will be used to back away from obstacles horizontally in position control modes (m/s). Set zero to disable horizontal backup.
     // @Units: m/s
     // @Range: 0 2
     // @User: Standard
-    AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_max, 0.75f),
+    AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_xy_max, 0.75f),
 
     // @Param{Copter}: ALT_MIN
     // @DisplayName: Avoidance minimum altitude
@@ -111,6 +111,14 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("BACKUP_DZ", 9, AC_Avoid, _backup_deadzone, 0.10f),
 
+    // @Param: BACKZ_SPD
+    // @DisplayName: Avoidance maximum vertical backup speed
+    // @Description: Maximum speed that will be used to back away from obstacles vertically in height control modes (m/s). Set zero to disable vertical backup.
+    // @Units: m/s
+    // @Range: 0 2
+    // @User: Standard
+    AP_GROUPINFO("BACKZ_SPD", 10, AC_Avoid, _backup_speed_z_max, 0.75),
+
     AP_GROUPEND
 };
 
@@ -224,14 +232,12 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa
     const float desired_backup_vel_z = back_vel_down + back_vel_up;
     Vector3f desired_backup_vel{desired_backup_vel_xy.x, desired_backup_vel_xy.y, desired_backup_vel_z};
 
-    const float max_back_spd_cms = _backup_speed_max * 100.0f;
-    if (!desired_backup_vel.is_zero() && is_positive(max_back_spd_cms)) {
+    const float max_back_spd_xy_cms = _backup_speed_xy_max * 100.0;
+    if (!desired_backup_vel.xy().is_zero() && is_positive(max_back_spd_xy_cms)) {
         backing_up = true;
-        // Constrain backing away speed
-        if (desired_backup_vel.length() > max_back_spd_cms) {
-            desired_backup_vel = desired_backup_vel.normalized() * max_back_spd_cms;
-        }
-
+        // Constrain horizontal backing away speed
+        desired_backup_vel.xy().limit_length(max_back_spd_xy_cms);
+
         // let user take control if they are backing away at a greater speed than what we have calculated
         // this has to be done for x,y,z separately. For eg, user is doing fine in "x" direction but might need backing up in "y".
         if (!is_zero(desired_backup_vel.x)) {
@@ -248,6 +254,15 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa
                 desired_vel_cms.y = MIN(desired_vel_cms.y, desired_backup_vel.y);
             }
         }
+    }
+
+    const float max_back_spd_z_cms = _backup_speed_z_max * 100.0;
+    if (!is_zero(desired_backup_vel.z) && is_positive(max_back_spd_z_cms)) {
+        backing_up = true;
+
+        // Constrain vertical backing away speed
+        desired_backup_vel.z = constrain_float(desired_backup_vel.z, -max_back_spd_z_cms, max_back_spd_z_cms);
+
         if (!is_zero(desired_backup_vel.z)) {
             if (is_positive(desired_backup_vel.z)) {
                 desired_vel_cms.z = MAX(desired_vel_cms.z, desired_backup_vel.z);
@@ -256,6 +271,7 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa
             }
         }
     }
+
     // limit acceleration
     limit_accel(desired_vel_cms_original, desired_vel_cms, dt);
 
@@ -422,7 +438,13 @@ 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_z_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);
+            }
             return;
         }
 

libraries/AC_Avoidance/AC_Avoid.h

@@ -211,14 +211,15 @@ class AC_Avoid {
 
     // parameters
     AP_Int8 _enabled;
-    AP_Int16 _angle_max;        // maximum lean angle to avoid obstacles (only used in non-GPS flight modes)
-    AP_Float _dist_max;         // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes
-    AP_Float _margin;           // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes
-    AP_Int8 _behavior;          // avoidance behaviour (slide or stop)
-    AP_Float _backup_speed_max; // Maximum speed that will be used to back away (in m/s)
-    AP_Float _alt_min;          // alt below which Proximity based avoidance is turned off
-    AP_Float _accel_max;        // maximum acceleration while simple avoidance is active
-    AP_Float _backup_deadzone;  // distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles
+    AP_Int16 _angle_max;           // maximum lean angle to avoid obstacles (only used in non-GPS flight modes)
+    AP_Float _dist_max;            // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes
+    AP_Float _margin;              // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes
+    AP_Int8 _behavior;             // avoidance behaviour (slide or stop)
+    AP_Float _backup_speed_xy_max; // Maximum speed that will be used to back away horizontally (in m/s)
+    AP_Float _backup_speed_z_max;  // Maximum speed that will be used to back away verticality (in m/s)
+    AP_Float _alt_min;             // alt below which Proximity based avoidance is turned off
+    AP_Float _accel_max;           // maximum acceleration while simple avoidance is active
+    AP_Float _backup_deadzone;     // distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles
 
     bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable)
     bool _proximity_alt_enabled = true; // true if proximity sensor based avoidance is enabled based on altitude