Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: Add param to switch off proximity avoidance based on alt

Browse Source
This commit is contained in:
Rishabh 2021-02-09 23:27:23 +05:30 committed by Randy Mackay
parent fdd39ca3a8
commit 865f3cda79
2 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -80,6 +80,14 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_max, 0.5f), 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 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; float back_vel_down = 0.0f;
// Avoidance in response to proximity sensor // 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 // Store velocity needed to back away from physical obstacles
Vector3f backup_vel_proximity; Vector3f backup_vel_proximity;
adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel_cms, backup_vel_proximity, kP_z,accel_cmss_z, dt); adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel_cms, backup_vel_proximity, kP_z,accel_cmss_z, dt);

5
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -82,6 +82,7 @@ public:
// enable/disable proximity based avoidance // enable/disable proximity based avoidance
void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; } void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; }
bool proximity_avoidance_enabled() const { return _proximity_enabled; } bool proximity_avoidance_enabled() const { return _proximity_enabled; }
void proximity_alt_avoidance_enable(bool on_off) { _proximity_alt_enabled = on_off; }
// helper functions // helper functions
@ -103,6 +104,8 @@ public:
// return margin (in meters) that the vehicle should stay from objects // return margin (in meters) that the vehicle should stay from objects
float get_margin() const { return _margin; } 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 // return true if limiting is active
bool limits_active() const {return (AP_HAL::millis() - _last_limit_time) < AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS;}; 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_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_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 _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_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_limit_time; // the last time a limit was active
uint32_t _last_log_ms; // the last time simple avoidance was logged uint32_t _last_log_ms; // the last time simple avoidance was logged