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

AP_AHRS: fixed relative home functions to calculate without origin

Browse Source 
this allows for FENCE_AUTOENABLE on planes with no compass
This commit is contained in:
Andrew Tridgell 2023-08-01 09:01:38 +10:00
parent b38fde2cf6
commit ce33149c9d
1 changed files with 8 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -1520,18 +1520,12 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
*/ */
bool AP_AHRS::get_relative_position_NED_home(Vector3f &vec) const bool AP_AHRS::get_relative_position_NED_home(Vector3f &vec) const
{ {
Location originLLH; Location loc;
Vector3f originNED; if (!_home_is_set ||
if (!get_relative_position_NED_origin(originNED) || !get_location(loc)) {
!get_origin(originLLH)) {
return false; return false;
} }
vec = _home.get_distance_NED(loc);
const Vector3f offset = originLLH.get_distance_NED(_home);
vec.x = originNED.x - offset.x;
vec.y = originNED.y - offset.y;
vec.z = originNED.z - offset.z;
return true; return true;
} }
@ -1577,17 +1571,13 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
*/ */
bool AP_AHRS::get_relative_position_NE_home(Vector2f &posNE) const bool AP_AHRS::get_relative_position_NE_home(Vector2f &posNE) const
{ {
Location originLLH; Location loc;
Vector2f originNE; if (!_home_is_set ||
if (!get_relative_position_NE_origin(originNE) || !get_location(loc)) {
!get_origin(originLLH)) {
return false; return false;
} }
const Vector2f offset = originLLH.get_distance_NE(_home); posNE = _home.get_distance_NE(loc);
posNE.x = originNE.x - offset.x;
posNE.y = originNE.y - offset.y;
return true; return true;
} }