Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Randy Mackay 2016-12-20 08:51:26 +09:00
parent 38478638de
commit b114d3928e
2 changed files with 10 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -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);

1
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -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