aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Initialise EKF origin height to field elevation

Browse Source 
Enables copters to set bit position 2 in the EK2_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:24 +10:00 committed by Francisco Ferreira
parent 0827b13feb
commit ac568bae53
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_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -421,6 +421,10 @@ void NavEKF2_core::setOrigin()
{
// assume origin at current GPS location (no averaging)
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;
// define Earth rotation vector in the NED navigation frame at the origin
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);