mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: use get_distance_NE instead of location_diff
This commit is contained in:
parent
50e99b6e1a
commit
b3a1c9c90c
|
@ -576,7 +576,7 @@ void NavEKF3_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
|
||||
|
|
|
@ -250,7 +250,7 @@ bool NavEKF3_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;
|
||||
|
|
|
@ -49,7 +49,7 @@ bool NavEKF3_core::calcGpsGoodToAlign(void)
|
|||
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
|
||||
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
|
||||
// Sum distance moved
|
||||
gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length();
|
||||
gpsDriftNE += gpsloc_prev.get_distance(gpsloc);
|
||||
gpsloc_prev = gpsloc;
|
||||
// Decay distance moved exponentially to zero
|
||||
gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
|
||||
|
|
Loading…
Reference in New Issue