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

AP_AHRS: Internalize EKF getLLH altitude management

Browse Source
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 // dead-reckoning support
bool AP_AHRS_NavEKF::get_position(struct Location &loc) const bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
{ {
Vector3f ned_pos;
Location origin;
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKF_TYPE_NONE: case EKF_TYPE_NONE:
return AP_AHRS_DCM::get_position(loc); return AP_AHRS_DCM::get_position(loc);
case EKF_TYPE2: case EKF_TYPE2:
if (EKF2.getLLH(loc) && EKF2.getPosD(-1,ned_pos.z) && EKF2.getOriginLLH(-1,origin)) { if (EKF2.getLLH(loc)) {
// fixup altitude using relative position from EKF origin
loc.alt = origin.alt - ned_pos.z*100;
return true; return true;
} }
break; break;
case EKF_TYPE3: case EKF_TYPE3:
if (EKF3.getLLH(loc) && EKF3.getPosD(-1,ned_pos.z) && EKF3.getOriginLLH(-1,origin)) { if (EKF3.getLLH(loc)) {
// fixup altitude using relative position from EKF origin
loc.alt = origin.alt - ned_pos.z*100;
return true; return true;
} }
break; break;