Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Initialise EKF origin height to field elevation

Browse Source 
Enables copters to set bit position 2 in the EK3_OGN_HGT_MASK parameter without having a large jump in reported local position height.
This commit is contained in:
priseborough 2017-07-17 09:07:40 +10:00 committed by Francisco Ferreira
parent ac568bae53
commit 58d58b4eac
1 changed files with 4 additions and 0 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

@ -477,6 +477,10 @@ void NavEKF3_core::setOrigin()
{ {
// assume origin at current GPS location (no averaging) // assume origin at current GPS location (no averaging)
EKF_origin = _ahrs->get_gps().location(); EKF_origin = _ahrs->get_gps().location();
// if flying, correct for height change from takeoff so that the origin is at field elevation
if (inFlight) {
EKF_origin.alt += (int32_t)(100.0f * stateStruct.position.z);
}
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt; ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
// define Earth rotation vector in the NED navigation frame at the origin // define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);