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_NavEKF2: use get_distance_NE instead of location_diff

Browse Source
This commit is contained in:
Pierre Kancir 2019-04-08 15:16:20 +02:00 committed by Tom Pittenger
parent 058cade92c
commit 50e99b6e1a
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -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 // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) { 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); gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
storedGPS.push(gpsDataNew); storedGPS.push(gpsDataNew);
// declare GPS available for use // declare GPS available for use

2
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -252,7 +252,7 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const
if ((AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D)) { 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 // 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(); 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.x = tempPosNE.x;
posNE.y = tempPosNE.y; posNE.y = tempPosNE.y;
return false; return false;