AP_NavEKF2: use origin lat for earth rates

home may not yet be set when this code is run, so using home may be
invalid
This commit is contained in:
Andrew Tridgell 2019-07-27 15:54:02 +10:00
parent d43a386f83
commit 6634555e40
1 changed files with 2 additions and 2 deletions

View File

@ -431,7 +431,7 @@ bool NavEKF2_core::setOriginLLH(const Location &loc)
EKF_origin = loc;
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
calcEarthRateNED(earthRateNED, loc.lat);
validOrigin = true;
return true;
}
@ -447,7 +447,7 @@ void NavEKF2_core::setOrigin()
}
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
calcEarthRateNED(earthRateNED, EKF_origin.lat);
validOrigin = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u Origin set to GPS",(unsigned)imu_index);
}