Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: fixed EKF3 origin alt inconsistency

Browse Source 
always use common origin, and adjust output posD for difference
between public and local lane origin heights
This commit is contained in:
Andrew Tridgell 2022-09-29 11:33:21 +10:00 committed by Randy Mackay
parent 36c88661c8
commit 03c27c6626
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1366,6 +1366,10 @@ bool NavEKF3::getOriginLLH(struct Location &loc) const
if (!core) {
return false;
}
if (common_origin_valid) {
loc = common_EKF_origin;
return true;
}
return core[primary].getOriginLLH(loc);
}

6
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -255,6 +255,12 @@ bool NavEKF3_core::getPosD(float &posD) const
posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
}
// adjust posD for difference between our origin and the public_origin
Location local_origin;
if (getOriginLLH(local_origin)) {
posD += (public_origin.alt - local_origin.alt) * 0.01;
}
// Return the current height solution status
return filterStatus.flags.vert_pos;