From 0fcb2037e131f7b400c910f4af7cbad477121201 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 27 Jul 2019 15:54:02 +1000 Subject: [PATCH] AP_NavEKF3: use origin lat for earth rates home may not yet be set when this code is run, so using home may be invalid --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index ca3ae345a9..b3ed8464eb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -473,7 +473,7 @@ bool NavEKF3_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; } @@ -489,7 +489,7 @@ void NavEKF3_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, "EKF3 IMU%u Origin set to GPS",(unsigned)imu_index); }