From 35a965ebc5d0d4700ce52347be28c2440756ce38 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 12 Apr 2017 21:09:52 +0900 Subject: [PATCH] AC_Avoidance: add configurable margin vehicle will attempt to stop MARGIN meters from objects in GPS modes --- libraries/AC_Avoidance/AC_Avoid.cpp | 27 +++++++++++++++++++-------- libraries/AC_Avoidance/AC_Avoid.h | 11 +++-------- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 7215f976cf..f1e19f9f39 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -25,6 +25,14 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @User: Standard AP_GROUPINFO("DIST_MAX", 3, AC_Avoid, _dist_max, AC_AVOID_NONGPS_DIST_MAX_DEFAULT), + // @Param: MARGIN + // @DisplayName: Avoidance distance margin in GPS modes + // @Description: Vehicle will attempt to stay at least this distance (in meters) from objects while in GPS modes + // @Units: meters + // @Range: 1 10 + // @User: Standard + AP_GROUPINFO("MARGIN", 4, AC_Avoid, _margin, 2.0f), + AP_GROUPEND }; @@ -107,7 +115,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // get distance from proximity sensor (in meters, convert to cm) float proximity_alt_diff_m; if (_proximity.get_upward_distance(proximity_alt_diff_m)) { - float proximity_alt_diff_cm = (proximity_alt_diff_m - AC_AVOID_UPWARD_MARGIN_M) * 100.0f; + float proximity_alt_diff_cm = (proximity_alt_diff_m - _margin) * 100.0f; if (!limit_alt || proximity_alt_diff_cm < alt_diff_cm) { alt_diff_cm = proximity_alt_diff_cm; } @@ -198,17 +206,17 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f // 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(); + const float margin_cm = _fence.get_margin() * 100.0f; 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) { + if (stopping_point_length > fence_radius - margin_cm) { // 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 = stopping_point * ((fence_radius - margin_cm) / 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); @@ -243,7 +251,7 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2 Vector2f* boundary = _fence.get_polygon_points(num_points); // adjust velocity using polygon - adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, true); + adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, true, _fence.get_margin()); } /* @@ -264,13 +272,13 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &d // get boundary from proximity sensor uint16_t num_points; const Vector2f *boundary = _proximity.get_boundary_points(num_points); - adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, false); + adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, false, _margin); } /* * Adjusts the desired velocity for the polygon fence. */ -void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points, bool earth_frame) +void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin) { // exit if there are no points if (boundary == nullptr || num_points == 0) { @@ -298,6 +306,9 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des safe_vel.y = desired_vel.y * _ahrs.cos_yaw() - desired_vel.x * _ahrs.sin_yaw(); // forward } + // calc margin in cm + float margin_cm = MAX(margin * 100.0f, 0); + uint16_t i, j; for (i = 1, j = num_points-1; i < num_points; j = i++) { // end points of current edge @@ -311,7 +322,7 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des // We are strictly inside the given edge. // Adjust velocity to not violate this edge. limit_direction /= limit_distance; - limit_velocity(kP, accel_cmss, safe_vel, limit_direction, MAX(limit_distance - get_margin(),0.0f)); + limit_velocity(kP, accel_cmss, safe_vel, limit_direction, MAX(limit_distance - margin_cm,0.0f)); } else { // We are exactly on the edge - treat this as a fence breach. // i.e. do not adjust velocity. diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index aef6d4f7b8..80d1fdb0c2 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -21,8 +21,6 @@ #define AC_AVOID_NONGPS_DIST_MAX_DEFAULT 10.0f // objects over 10m away are ignored (default value for DIST_MAX parameter) #define AC_AVOID_ANGLE_MAX_PERCENT 0.75f // object avoidance max lean angle as a percentage (expressed in 0 ~ 1 range) of total vehicle max lean angle -#define AC_AVOID_UPWARD_MARGIN_M 2.0f // stop 2m before objects above the vehicle - /* * This class prevents the vehicle from leaving a polygon fence in * 2 dimensions by limiting velocity (adjust_velocity). @@ -75,8 +73,9 @@ private: /* * Adjusts the desired velocity given an array of boundary points * earth_frame should be true if boundary is in earth-frame, false for body-frame + * margin is the distance (in meters) that the vehicle should stop short of the polygon */ - void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points, bool earth_frame); + void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin); /* * Limits the component of desired_vel in the direction of the unit vector @@ -104,11 +103,6 @@ private: */ float get_stopping_distance(float kP, float accel_cmss, float speed) const; - /* - * Gets the fence margin in cm - */ - float get_margin() const { return _fence.get_margin() * 100.0f; } - /* * methods for avoidance in non-GPS flight modes */ @@ -129,6 +123,7 @@ private: 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 bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) };