mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: Fix #17789 core's ekf origin altitude different if flying
This commit is contained in:
parent
678fe0480a
commit
c881fac1de
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue