diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 5630adb270..99d52e118d 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -80,6 +80,14 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @User: Standard AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_max, 0.5f), + // @Param: ALT_MIN + // @DisplayName: Avoidance minimum altitude + // @Description: Minimum altitude above which proximity based avoidance will start working. This requires a valid downward facing rangefinder reading to work. Set zero to disable + // @Units: m + // @Range: 0 6 + // @User: Standard + AP_GROUPINFO("ALT_MIN", 7, AC_Avoid, _alt_min, 2.0f), + AP_GROUPEND }; @@ -167,7 +175,7 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa float back_vel_down = 0.0f; // Avoidance in response to proximity sensor - if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled) { + if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled && _proximity_alt_enabled) { // Store velocity needed to back away from physical obstacles Vector3f backup_vel_proximity; adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel_cms, backup_vel_proximity, kP_z,accel_cmss_z, dt); diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index b61521b2b4..9cac722761 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -82,6 +82,7 @@ public: // enable/disable proximity based avoidance void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; } bool proximity_avoidance_enabled() const { return _proximity_enabled; } + void proximity_alt_avoidance_enable(bool on_off) { _proximity_alt_enabled = on_off; } // helper functions @@ -103,6 +104,8 @@ public: // return margin (in meters) that the vehicle should stay from objects float get_margin() const { return _margin; } + // return minimum alt (in meters) above which avoidance will be active + float get_min_alt() const { return _alt_min; } // return true if limiting is active bool limits_active() const {return (AP_HAL::millis() - _last_limit_time) < AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS;}; @@ -200,8 +203,10 @@ private: AP_Float _margin; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes AP_Int8 _behavior; // avoidance behaviour (slide or stop) AP_Float _backup_speed_max; // Maximum speed that will be used to back away (in m/s) + AP_Float _alt_min; // alt below which Proximity based avoidance is turned off bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) + bool _proximity_alt_enabled = true; // true if proximity sensor based avoidance is enabled based on altitude uint32_t _last_limit_time; // the last time a limit was active uint32_t _last_log_ms; // the last time simple avoidance was logged