|
|
|
@ -473,7 +473,7 @@ bool NavEKF3_core::setOriginLLH(const Location &loc)
@@ -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()
@@ -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); |
|
|
|
|
} |
|
|
|
|