From b114d3928e3fab23c9fd5f50e88923197563ab5c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 20 Dec 2016 08:51:26 +0900 Subject: [PATCH] AC_Avoid: fix distance to lean angle logic Fixes from PR review including (a) make some local variables const (b) rename angle_max to veh_angle_max (c) fix distance_to_lean_pct logic so distance of zero returns maximum lean angle --- libraries/AC_Avoidance/AC_Avoid.cpp | 24 ++++++++++-------------- libraries/AC_Avoidance/AC_Avoid.h | 1 - 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index f4f0d7e078..7bd9b8ab51 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -69,7 +69,7 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel // adjust roll-pitch to push vehicle away from objects // roll and pitch value are in centi-degrees -void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float angle_max) +void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float veh_angle_max) { // exit immediately if proximity based avoidance is disabled if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) == 0 || !_proximity_enabled) { @@ -77,7 +77,7 @@ void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float angle_max) } // exit immediately if angle max is zero - if (_angle_max <= 0.0f || angle_max <= 0.0f) { + if (_angle_max <= 0.0f || veh_angle_max <= 0.0f) { return; } @@ -94,7 +94,7 @@ void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float angle_max) // apply avoidance angular limits // the object avoidance lean angle is never more than 75% of the total angle-limit to allow the pilot to override - float angle_limit = constrain_float(_angle_max, 0.0f, angle_max * AC_AVOID_ANGLE_MAX_PERCENT); + const float angle_limit = constrain_float(_angle_max, 0.0f, veh_angle_max * AC_AVOID_ANGLE_MAX_PERCENT); float vec_len = rp_out.length(); if (vec_len > angle_limit) { rp_out *= (angle_limit / vec_len); @@ -106,8 +106,8 @@ void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float angle_max) // apply total angular limits vec_len = rp_out.length(); - if (vec_len > angle_max) { - rp_out *= (angle_max / vec_len); + if (vec_len > veh_angle_max) { + rp_out *= (veh_angle_max / vec_len); } // return adjusted roll, pitch @@ -332,13 +332,9 @@ float AC_Avoid::get_stopping_distance(float kP, float accel_cmss, float speed) c float AC_Avoid::distance_to_lean_pct(float dist_m) { // ignore objects beyond DIST_MAX - if (dist_m <= 0.0f || dist_m >= _dist_max || _dist_max <= 0.0f) { + if (dist_m < 0.0f || dist_m >= _dist_max || _dist_max <= 0.0f) { return 0.0f; } - // objects at 0m cause maximum lean - if (dist_m <= 0.0f) { - return 1.0f; - } // inverted but linear response return 1.0f - (dist_m / _dist_max); } @@ -364,11 +360,11 @@ void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_ne if (_proximity.get_object_angle_and_distance(i, ang_deg, dist_m)) { if (dist_m < _dist_max) { // convert distance to lean angle (in 0 to 1 range) - float lean_pct = distance_to_lean_pct(dist_m); + const float lean_pct = distance_to_lean_pct(dist_m); // convert angle to roll and pitch lean percentages - float angle_rad = radians(ang_deg); - float roll_pct = -sinf(angle_rad) * lean_pct; - float pitch_pct = cosf(angle_rad) * lean_pct; + const float angle_rad = radians(ang_deg); + const float roll_pct = -sinf(angle_rad) * lean_pct; + const float pitch_pct = cosf(angle_rad) * lean_pct; // update roll, pitch maximums if (roll_pct > 0.0f) { roll_positive = MAX(roll_positive, roll_pct); diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 92c97c6a15..67297b52c0 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -20,7 +20,6 @@ // definitions for non-GPS avoidance #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_NONGPS_P 1.0f // p gain used in conversion from distance to lean angle /* * This class prevents the vehicle from leaving a polygon fence in