From 79a4b8fa3288d84308fbab6da5fe5756c8bfe10f Mon Sep 17 00:00:00 2001 From: Rishabh Date: Thu, 20 Aug 2020 10:47:05 +0530 Subject: [PATCH] AC_Avoid: Add feature to reject obstacles near home loc --- libraries/AC_Avoidance/AP_OADatabase.cpp | 31 ++++++++++++++++++++++++ libraries/AC_Avoidance/AP_OADatabase.h | 1 + 2 files changed, 32 insertions(+) diff --git a/libraries/AC_Avoidance/AP_OADatabase.cpp b/libraries/AC_Avoidance/AP_OADatabase.cpp index eedd131787..1c5205c8d6 100644 --- a/libraries/AC_Avoidance/AP_OADatabase.cpp +++ b/libraries/AC_Avoidance/AP_OADatabase.cpp @@ -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; diff --git a/libraries/AC_Avoidance/AP_OADatabase.h b/libraries/AC_Avoidance/AP_OADatabase.h index 86c1b062d3..edf9df7b32 100644 --- a/libraries/AC_Avoidance/AP_OADatabase.h +++ b/libraries/AC_Avoidance/AP_OADatabase.h @@ -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 *items; // thread safe incoming queue of points from proximity sensor to be put into database