Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2019-07-27 15:54:02 +10:00
parent 6634555e40
commit 4d57ea27dc
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -476,7 +476,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;
}
@ -492,7 +492,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);
}