Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix #17789 core's ekf origin altitude different if flying

This commit is contained in:
Josh Henderson 2021-06-17 23:08:25 -04:00 committed by Andrew Tridgell
parent 678fe0480a
commit c881fac1de
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -607,10 +607,6 @@ bool NavEKF3_core::setOrigin(const Location &loc)
}
EKF_origin = loc;
// if flying, correct for height change from takeoff so that the origin is at field elevation
if (inFlight) {
EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z);
}
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, EKF_origin.lat);

7
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -659,8 +659,13 @@ void NavEKF3_core::readGpsData()
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if (gpsGoodToAlign && !validOrigin) {
Location gpsloc_fieldelevation = gpsloc;
// if flying, correct for height change from takeoff so that the origin is at field elevation
if (inFlight) {
gpsloc_fieldelevation.alt += (int32_t)(100.0f * stateStruct.position.z);
}
if (!setOrigin(gpsloc)) {
if (!setOrigin(gpsloc_fieldelevation)) {
// set an error as an attempt was made to set the origin more than once
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return;