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

AC_Avoid: Add feature to reject obstacles near home loc

This commit is contained in:
Rishabh 2020-08-20 10:47:05 +05:30 committed by Randy Mackay
parent 245e0230b5
commit 79a4b8fa32
2 changed files with 32 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AC_Avoidance/AP_OADatabase.cpp
View File

@ -31,6 +31,9 @@ extern const AP_HAL::HAL& hal;
#define AP_OADATABASE_QUEUE_SIZE_DEFAULT 80
#endif
#ifndef AP_OADATABASE_DISTANCE_FROM_HOME
#define AP_OADATABASE_DISTANCE_FROM_HOME 3
#endif
const AP_Param::GroupInfo AP_OADatabase::var_info[] = {
@ -91,6 +94,14 @@ const AP_Param::GroupInfo AP_OADatabase::var_info[] = {
// @User: Advanced
AP_GROUPINFO("DIST_MAX", 7, AP_OADatabase, _dist_max, 0.0f),
// @Param{Copter}: ALT_MIN
// @DisplayName: OADatabase minimum altitude above home before storing obstacles
// @Description: OADatabase will reject obstacle's if vehicle's altitude above home is below this parameter, in a 3 meter radius around home. Set 0 to disable this feature.
// @Units: m
// @Range: 0 4
// @User: Advanced
AP_GROUPINFO_FRAME("ALT_MIN", 8, AP_OADatabase, _min_alt, 2.0f, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
AP_GROUPEND
};
@ -137,6 +148,26 @@ void AP_OADatabase::queue_push(const Vector3f &pos, uint32_t timestamp_ms, float
return;
}
// check if this obstacle needs to be rejected from DB because of low altitude near home
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
if (!is_zero(_min_alt)) {
Vector2f current_pos;
if (!AP::ahrs().get_relative_position_NE_home(current_pos)) {
// we do not know where the vehicle is
return;
}
if (current_pos.length() < AP_OADATABASE_DISTANCE_FROM_HOME) {
// vehicle is within a small radius of home
float height_above_home;
AP::ahrs().get_relative_position_D_home(height_above_home);
if (-height_above_home < _min_alt) {
// vehicle is below the minimum alt
return;
}
}
}
#endif
// ignore objects that are far away
if ((_dist_max > 0.0f) && (distance > _dist_max)) {
return;

1
libraries/AC_Avoidance/AP_OADatabase.h
View File

@ -89,6 +89,7 @@ private:
AP_Float _beam_width; // beam width used when converting lidar readings to object radius
AP_Float _radius_min; // objects minimum radius (in meters)
AP_Float _dist_max; // objects maximum distance (in meters)
AP_Float _min_alt; // OADatabase minimum vehicle height check (in meters)
struct {
ObjectBuffer<OA_DbItem> *items; // thread safe incoming queue of points from proximity sensor to be put into database