From 50e99b6e1a228f28d8776896073e708c18b51262 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 8 Apr 2019 15:16:20 +0200 Subject: [PATCH] AP_NavEKF2: use get_distance_NE instead of location_diff --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index c7c9387c49..ea52f3ef89 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -531,7 +531,7 @@ void NavEKF2_core::readGpsData() // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin if (validOrigin) { - gpsDataNew.pos = location_diff(EKF_origin, gpsloc); + gpsDataNew.pos = EKF_origin.get_distance_NE(gpsloc); gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt); storedGPS.push(gpsDataNew); // declare GPS available for use diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index b81deba87e..ed94474ae2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -252,7 +252,7 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const if ((AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { // If the origin has been set and we have GPS, then return the GPS position relative to the origin const struct Location &gpsloc = AP::gps().location(); - Vector2f tempPosNE = location_diff(EKF_origin, gpsloc); + const Vector2f tempPosNE = EKF_origin.get_distance_NE(gpsloc); posNE.x = tempPosNE.x; posNE.y = tempPosNE.y; return false;