AC_Avoid: Limit acceleration while simple avoidance is active

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -88,6 +88,14 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ALT_MIN", 7, AC_Avoid, _alt_min, 2.0f),
 
+    // @Param: ACCEL_MAX
+    // @DisplayName: Avoidance maximum acceleration
+    // @Description: Maximum acceleration with which obstacles will be avoided with. Set zero to disable acceleration limits
+    // @Units: m/s/s
+    // @Range: 0 9
+    // @User: Standard
+    AP_GROUPINFO("ACCEL_MAX", 8, AC_Avoid, _accel_max, 3.0f),
+
     AP_GROUPEND
 };
 
@@ -166,6 +174,9 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa
         return;
     }
 
+    // make a copy of input velocity, because desired_vel_cms might be changed
+    const Vector3f desired_vel_cms_original = desired_vel_cms;
+
     // limit acceleration
     const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
 
@@ -224,6 +235,26 @@ 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);
+}
+
+// Limit acceleration so that change of velocity output by avoidance library is controlled
+void AC_Avoid::limit_accel(const Vector3f &original_vel, Vector3f &modified_vel, float dt)
+{
+    if (original_vel == modified_vel || is_zero(_accel_max) || !is_positive(dt)) {
+        // we can't limit accel if any of these conditions are true
+        return;
+    }
+    // max accel in cm
+    const float max_accel_cm = _accel_max * 100.0f;
+    // accel desired by avoidance library
+    const Vector3f accel = (modified_vel - original_vel)/dt;
+    if (accel.length() > max_accel_cm) {
+        // pull back on the acceleration
+        modified_vel = (accel.normalized() * max_accel_cm) * dt + original_vel;
+    }
+    return;
 }
 
 // This method is used in most Rover modes and not in Copter

libraries/AC_Avoidance/AC_Avoid.h

@@ -106,6 +106,7 @@ public:
 
     // return minimum alt (in meters) above which avoidance will be active
     float get_min_alt() const { return _alt_min; }
+
     // return true if limiting is active
     bool limits_active() const {return (AP_HAL::millis() - _last_limit_time) < AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS;};
 
@@ -118,6 +119,11 @@ private:
         BEHAVIOR_STOP = 1
     };
 
+    /*
+     * Limit acceleration so that change of velocity output by avoidance library is controlled
+     */
+    void limit_accel(const Vector3f &original_vel, Vector3f &modified_vel, float dt);
+
     /*
      * Adjusts the desired velocity for the circular fence.
      */
@@ -204,6 +210,7 @@ private:
     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 accelration while simple avoidance is active
 
     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