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

AP_NavEKF2: Fix #17789 core's EKF origin altitude different if flying

This commit is contained in:
Josh Henderson 2021-06-17 23:08:02 -04:00 committed by Peter Barker
parent 0ae3730f11
commit 0561637350
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_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -444,10 +444,6 @@ bool NavEKF2_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_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -598,8 +598,13 @@ void NavEKF2_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;