Browse Source

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
master
Andrew Tridgell 6 years ago
parent
commit
0fcb2037e1
  1. 4
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -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);
}

Loading…
Cancel
Save