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

AC_Avoidance: get Vector3f when checking all components of relpos

This commit is contained in:
Peter Barker 2022-01-30 00:10:27 +11:00 committed by Peter Barker
parent af1a454820
commit 8572c2bef3
1 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AP_OADatabase.cpp
View File

@ -151,16 +151,14 @@ void AP_OADatabase::queue_push(const Vector3f &pos, uint32_t timestamp_ms, float
// check if this obstacle needs to be rejected from DB because of low altitude near home
#if APM_BUILD_COPTER_OR_HELI
if (!is_zero(_min_alt)) {
Vector2f current_pos;
if (!AP::ahrs().get_relative_position_NE_home(current_pos)) {
Vector3f current_pos;
if (!AP::ahrs().get_relative_position_NED_home(current_pos)) {
// we do not know where the vehicle is
return;
}
if (current_pos.length() < AP_OADATABASE_DISTANCE_FROM_HOME) {
if (current_pos.xy().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) {
if (-current_pos.z < _min_alt) {
// vehicle is below the minimum alt
return;
}