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

AP_AHRS: Internalize EKF getLLH altitude management

This commit is contained in:
Michael du Breuil 2018-09-13 13:39:52 -07:00 committed by Francisco Ferreira
parent 1991f223ac
commit c02863e001
1 changed files with 2 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -403,24 +403,18 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con
// dead-reckoning support
bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
{
Vector3f ned_pos;
Location origin;
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return AP_AHRS_DCM::get_position(loc);
case EKF_TYPE2:
if (EKF2.getLLH(loc) && EKF2.getPosD(-1,ned_pos.z) && EKF2.getOriginLLH(-1,origin)) {
// fixup altitude using relative position from EKF origin
loc.alt = origin.alt - ned_pos.z*100;
if (EKF2.getLLH(loc)) {
return true;
}
break;
case EKF_TYPE3:
if (EKF3.getLLH(loc) && EKF3.getPosD(-1,ned_pos.z) && EKF3.getOriginLLH(-1,origin)) {
// fixup altitude using relative position from EKF origin
loc.alt = origin.alt - ned_pos.z*100;
if (EKF3.getLLH(loc)) {
return true;
}
break;