AC_Avoidance: add library to stop at circular fence

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -0,0 +1,117 @@
+#include "AC_Avoid.h"
+
+const AP_Param::GroupInfo AC_Avoid::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Avoidance control enable/disable
+    // @Description: Enabled/disable stopping at fence
+    // @Values: 0:None,1:StopAtFence
+    // @User: Standard
+    AP_GROUPINFO("ENABLE", 1,  AC_Avoid, _enabled, AC_AVOID_STOP_AT_FENCE),
+
+    AP_GROUPEND
+};
+
+/// Constructor
+AC_Avoid::AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence)
+    : _ahrs(ahrs),
+      _inav(inav),
+      _fence(fence)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+void AC_Avoid::adjust_velocity(const float kP, const float accel_cmss, Vector2f &desired_vel)
+{
+    // exit immediately if disabled
+    if (_enabled == AC_AVOID_DISABLED) {
+        return;
+    }
+
+    // limit acceleration
+    float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
+
+    if (_enabled == AC_AVOID_STOP_AT_FENCE) {
+        adjust_velocity_circle(kP, accel_cmss_limited, desired_vel);
+    }
+}
+
+/*
+ * Adjusts the desired velocity for the circular fence.
+ */
+void AC_Avoid::adjust_velocity_circle(const float kP, const float accel_cmss, Vector2f &desired_vel)
+{
+    // exit if circular fence is not enabled
+    if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) == 0) {
+        return;
+    }
+
+    // get position as a 2D offset in cm from ahrs home
+    const Vector2f position_xy = get_position();
+
+    float speed = desired_vel.length();
+    // get the fence radius in cm
+    const float fence_radius = _fence.get_radius() * 100.0f;
+    // get the margin to the fence in cm
+    const float margin = get_margin();
+
+    if (!is_zero(speed) && position_xy.length() <= fence_radius) {
+        // Currently inside circular fence
+        Vector2f stopping_point = position_xy + desired_vel*(get_stopping_distance(kP, accel_cmss, speed)/speed);
+        float stopping_point_length = stopping_point.length();
+        if (stopping_point_length > fence_radius - margin) {
+            // Unsafe desired velocity - will not be able to stop before fence breach
+            // Project stopping point radially onto fence boundary
+            // Adjusted velocity will point towards this projected point at a safe speed
+            Vector2f target = stopping_point * ((fence_radius - margin) / stopping_point_length);
+            Vector2f target_direction = target - position_xy;
+            float distance_to_target = target_direction.length();
+            float max_speed = get_max_speed(kP, accel_cmss, distance_to_target);
+            desired_vel = target_direction * (MIN(speed,max_speed) / distance_to_target);
+        }
+    }
+}
+
+/*
+ * Gets the current xy-position, relative to home (not relative to EKF origin)
+ */
+Vector2f AC_Avoid::get_position()
+{
+    const Vector3f position_xyz = _inav.get_position();
+    const Vector2f position_xy(position_xyz.x,position_xyz.y);
+    const Vector2f diff = location_diff(_inav.get_origin(),_ahrs.get_home()) * 100.0f;
+    return position_xy - diff;
+}
+
+/*
+ * Computes the speed such that the stopping distance
+ * of the vehicle will be exactly the input distance.
+ */
+float AC_Avoid::get_max_speed(const float kP, const float accel_cmss, const float distance)
+{
+    return AC_AttitudeControl::sqrt_controller(distance, kP, accel_cmss);
+}
+
+/*
+ * Computes distance required to stop, given current speed.
+ *
+ * Implementation copied from AC_PosControl.
+ */
+float AC_Avoid::get_stopping_distance(const float kP, const float accel_cmss, const float speed)
+{
+    // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
+    if (kP <= 0.0f || accel_cmss <= 0.0f || is_zero(speed)) {
+        return 0.0f;
+    }
+
+    // calculate point at which velocity switches from linear to sqrt
+    float linear_speed = accel_cmss/kP;
+
+    // calculate distance within which we can stop
+    if (speed < linear_speed) {
+        return speed/kP;
+    } else {
+        float linear_distance = accel_cmss/(2.0f*kP*kP);
+        return linear_distance + (speed*speed)/(2.0f*accel_cmss);
+    }
+}

libraries/AC_Avoidance/AC_Avoid.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_AHRS/AP_AHRS.h>     // AHRS library
+#include <AP_InertialNav/AP_InertialNav.h>     // Inertial Navigation library
+#include <AC_AttitudeControl/AC_AttitudeControl.h> // Attitude controller library for sqrt controller
+#include <AC_Fence/AC_Fence.h>         // Failsafe fence library
+
+#define AC_AVOID_ACCEL_CMSS_MAX         250.0f  // maximum acceleration/deceleration in cm/s/s used to avoid hitting fence
+
+// bit masks for enabled fence types.
+#define AC_AVOID_DISABLED               0       // avoidance disabled
+#define AC_AVOID_STOP_AT_FENCE          1       // stop at fence
+
+/*
+ * This class prevents the vehicle from leaving a polygon fence in
+ * 2 dimensions by limiting velocity (adjust_velocity).
+ */
+class AC_Avoid {
+public:
+
+    /// Constructor
+    AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence);
+
+    /*
+     * Adjusts the desired velocity so that the vehicle can stop
+     * before the fence/object.
+     */
+    void adjust_velocity(const float kP, const float accel_cmss, Vector2f &desired_vel);
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    /*
+     * Adjusts the desired velocity for the circular fence.
+     */
+    void adjust_velocity_circle(const float kP, const float accel_cmss, Vector2f &desired_vel);
+
+    /*
+     * Gets the current position, relative to home (not relative to EKF origin)
+     */
+    Vector2f get_position();
+
+    /*
+     * Computes the speed such that the stopping distance
+     * of the vehicle will be exactly the input distance.
+     */
+    float get_max_speed(const float kP, const float accel_cmss, const float distance);
+
+    /*
+     * Computes distance required to stop, given current speed.
+     */
+    float get_stopping_distance(const float kP, const float accel_cmss, const float speed);
+
+    /*
+     * Gets the fence margin in cm
+     */
+    float get_margin() { return _fence.get_margin() * 100.0f; }
+
+    // external references
+    const AP_AHRS& _ahrs;
+    const AP_InertialNav& _inav;
+    const AC_Fence& _fence;
+
+    // parameters
+    AP_Int8 _enabled;
+};