mirror of https://github.com/ArduPilot/ardupilot
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:
parent
a3411efdb5
commit
a01faa2f00
|
@ -424,7 +424,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;
|
||||
}
|
||||
|
@ -440,7 +440,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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue