From c1e9a30c1d25b96fce34895d0e85eae675aa351e Mon Sep 17 00:00:00 2001 From: chobits Date: Thu, 17 Aug 2017 10:59:06 +0800 Subject: [PATCH] AP_AHRS: fix get_relative_position_NE_home calcalation --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 47771803d2..3ce8e7c70d 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -790,8 +790,8 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_home(Vector3f &vec) const Vector3f offset = location_3d_diff_NED(originLLH, _home); - vec.x = originNED.x + offset.x; - vec.y = originNED.y + offset.y; + vec.x = originNED.x - offset.x; + vec.y = originNED.y - offset.y; vec.z = originNED.z - offset.z; return true; } @@ -839,8 +839,8 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_home(Vector2f &posNE) const Vector2f offset = location_diff(originLLH, _home); - posNE.x = originNE.x + offset.x; - posNE.y = originNE.y + offset.y; + posNE.x = originNE.x - offset.x; + posNE.y = originNE.y - offset.y; return true; }