Skip to content

Commit

Permalink
AP_AHRS: fixed relative home functions to calculate without origin
Browse files Browse the repository at this point in the history
this allows for FENCE_AUTOENABLE on planes with no compass
  • Loading branch information
tridge committed Jul 31, 2023
1 parent e9cb826 commit 049a238
Showing 1 changed file with 8 additions and 18 deletions.
26 changes: 8 additions & 18 deletions libraries/AP_AHRS/AP_AHRS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Location originLLH;
Vector3f originNED;
if (!get_relative_position_NED_origin(originNED) ||
!get_origin(originLLH)) {
Location loc;
if (!_home_is_set ||
!get_location(loc)) {
return false;
}

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;
vec = _home.get_distance_NED(loc);
return true;
}

Expand Down Expand Up @@ -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
{
Location originLLH;
Vector2f originNE;
if (!get_relative_position_NE_origin(originNE) ||
!get_origin(originLLH)) {
Location loc;
if (!_home_is_set ||
!get_location(loc)) {
return false;
}

const Vector2f offset = originLLH.get_distance_NE(_home);

posNE.x = originNE.x - offset.x;
posNE.y = originNE.y - offset.y;
posNE = _home.get_distance_NE(loc);
return true;
}

Expand Down

0 comments on commit 049a238

Please sign in to comment.