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 Peter Barker
parent 0561637350
commit 64dc3bbe41
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; 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; ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin // define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, EKF_origin.lat); 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 // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if (gpsGoodToAlign && !validOrigin) { 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 // set an error as an attempt was made to set the origin more than once
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return; return;