From 1b582b2009962a5c4583dba5e25fbca66faa0845 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 17 Dec 2016 12:42:40 +0900 Subject: [PATCH] AC_Avoid: avoidance for non-GPS flight modes --- libraries/AC_Avoidance/AC_Avoid.cpp | 124 +++++++++++++++++++++++++++- libraries/AC_Avoidance/AC_Avoid.h | 27 ++++++ 2 files changed, 150 insertions(+), 1 deletion(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 0a1373ef4b..f4f0d7e078 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -10,6 +10,21 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @User: Standard AP_GROUPINFO("ENABLE", 1, AC_Avoid, _enabled, AC_AVOID_ALL), + // @Param: ANGLE MAX + // @DisplayName: Avoidance max lean angle in non-GPS flight modes + // @Description: Max lean angle used to avoid obstacles while in non-GPS modes + // @Range: 0 4500 + // @User: Standard + AP_GROUPINFO("ANGLE_MAX", 2, AC_Avoid, _angle_max, 1000), + + // @Param: DIST_MAX + // @DisplayName: Avoidance distance maximum in non-GPS flight modes + // @Description: Distance from object at which obstacle avoidance will begin in non-GPS modes + // @Units: meters + // @Range: 3 30 + // @User: Standard + AP_GROUPINFO("DIST_MAX", 3, AC_Avoid, _dist_max, AC_AVOID_NONGPS_DIST_MAX_DEFAULT), + AP_GROUPEND }; @@ -38,7 +53,7 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel adjust_velocity_polygon_fence(kP, accel_cmss_limited, desired_vel); } - if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0) { + if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled) { adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel); } } @@ -52,6 +67,54 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel desired_vel.y = des_vel_xy.y; } +// 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) +{ + // exit immediately if proximity based avoidance is disabled + if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) == 0 || !_proximity_enabled) { + return; + } + + // exit immediately if angle max is zero + if (_angle_max <= 0.0f || angle_max <= 0.0f) { + return; + } + + float roll_positive = 0.0f; // maximum positive roll value + float roll_negative = 0.0f; // minimum negative roll value + float pitch_positive = 0.0f; // maximum position pitch value + float pitch_negative = 0.0f; // minimum negative pitch value + + // get maximum positive and negative roll and pitch percentages from proximity sensor + get_proximity_roll_pitch_pct(roll_positive, roll_negative, pitch_positive, pitch_negative); + + // add maximum positive and negative percentages together for roll and pitch, convert to centi-degrees + Vector2f rp_out((roll_positive + roll_negative) * 4500.0f, (pitch_positive + pitch_negative) * 4500.0f); + + // 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); + float vec_len = rp_out.length(); + if (vec_len > angle_limit) { + rp_out *= (angle_limit / vec_len); + } + + // add passed in roll, pitch angles + rp_out.x += roll; + rp_out.y += pitch; + + // apply total angular limits + vec_len = rp_out.length(); + if (vec_len > angle_max) { + rp_out *= (angle_max / vec_len); + } + + // return adjusted roll, pitch + roll = rp_out.x; + pitch = rp_out.y; +} + /* * Adjusts the desired velocity for the circular fence. */ @@ -264,3 +327,62 @@ float AC_Avoid::get_stopping_distance(float kP, float accel_cmss, float speed) c return accel_cmss/(2.0f*kP*kP) + (speed*speed)/(2.0f*accel_cmss); } } + +// convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes +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) { + 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); +} + +// returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor +void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative) +{ + // exit immediately if proximity sensor is not present + if (_proximity.get_status() != AP_Proximity::Proximity_Good) { + return; + } + + uint8_t obj_count = _proximity.get_object_count(); + + // if no objects return + if (obj_count == 0) { + return; + } + + // calculate maximum roll, pitch values from objects + for (uint8_t i=0; i 0.0f) { + roll_positive = MAX(roll_positive, roll_pct); + } + if (roll_pct < 0.0f) { + roll_negative = MIN(roll_negative, roll_pct); + } + if (pitch_pct > 0.0f) { + pitch_positive = MAX(pitch_positive, pitch_pct); + } + if (pitch_pct < 0.0f) { + pitch_negative = MIN(pitch_negative, pitch_pct); + } + } + } + } +} diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index b17867da0f..92c97c6a15 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -17,6 +17,11 @@ #define AC_AVOID_USE_PROXIMITY_SENSOR 2 // stop based on proximity sensor output #define AC_AVOID_ALL 3 // use fence and promiximity sensor +// 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 * 2 dimensions by limiting velocity (adjust_velocity). @@ -35,6 +40,14 @@ public: void adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel); void 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 + // angle_max is the user defined maximum lean angle for the vehicle in centi-degrees + void adjust_roll_pitch(float &roll, float &pitch, float angle_max); + + // enable/disable proximity based avoidance + void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; } + static const struct AP_Param::GroupInfo var_info[]; private: @@ -90,6 +103,16 @@ private: */ float get_margin() const { return _fence.get_margin() * 100.0f; } + /* + * methods for avoidance in non-GPS flight modes + */ + + // convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes + float distance_to_lean_pct(float dist_m); + + // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor + void get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative); + // external references const AP_AHRS& _ahrs; const AP_InertialNav& _inav; @@ -98,4 +121,8 @@ private: // parameters 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 + + bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) };