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

AP_NavEKF3: fixed EKF3 origin alt inconsistency 

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
parent 0210d7c60c
commit 17d9018a91
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

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

6
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -268,6 +268,12 @@ bool NavEKF3_core::getPosD(float &posD) const
posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt; 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 the current height solution status
return filterStatus.flags.vert_pos; return filterStatus.flags.vert_pos;