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

AP_NavEKF3: Fix default behaviour for height origin corrections 

Previous default was to apply in-flight height origin changes to local position instead of to reported origin height. This caused problems with copters that took off before getting GPS lock.
This commit is contained in:
priseborough 2017-07-16 10:37:46 +10:00 committed by Francisco Ferreira
parent 027552ef44
commit 0827b13feb
2 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -542,8 +542,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
// @Param: OGN_HGT_MASK
// @DisplayName: Bitmask control of EKF reference height correction
// @Description: When a height sensor other than GPS is used as the primary height source by the EKF, the position of the zero height datum is defined by that sensor and its frame of reference. If a GPS height measurement is also available, then the height of the WGS-84 height datum used by the EKF can be corrected so that the height returned by the getLLH() function is compensated for primary height sensor drift and change in datum over time. The first two bit positions control when the height datum will be corrected. Correction is performed using a Bayes filter and only operates when GPS quality permits. The third bit position controls where the corrections to the GPS reference datum are applied. Corrections can be applied to the local vertical position (default) or to the reported EKF origin height.
// @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to origin height
// @Description: When a height sensor other than GPS is used as the primary height source by the EKF, the position of the zero height datum is defined by that sensor and its frame of reference. If a GPS height measurement is also available, then the height of the WGS-84 height datum used by the EKF can be corrected so that the height returned by the getLLH() function is compensated for primary height sensor drift and change in datum over time. The first two bit positions control when the height datum will be corrected. Correction is performed using a Bayes filter and only operates when GPS quality permits. The third bit position controls where the corrections to the GPS reference datum are applied. Corrections can be applied to the local vertical position or to the reported EKF origin height (default).
// @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to local position
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("OGN_HGT_MASK", 50, NavEKF3, _originHgtMode, 0),

6
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -283,8 +283,8 @@ bool NavEKF3_core::getPosD(float &posD) const
{
// The EKF always has a height estimate regardless of mode of operation
// Correct for the IMU offset (EKF calculations are at the IMU)
// Correct for
if (frontend->_originHgtMode & (1<<2)) {
// Also correct for changes to the origin height
if ((frontend->_originHgtMode & (1<<2)) == 0) {
// Any sensor height drift corrections relative to the WGS-84 reference are applied to the origin.
posD = outputDataNew.position.z + posOffsetNED.z;
} else {
@ -381,7 +381,7 @@ bool NavEKF3_core::getOriginLLH(struct Location &loc) const
if (validOrigin) {
loc = EKF_origin;
// report internally corrected reference height if enabled
if (frontend->_originHgtMode & (1<<2)) {
if ((frontend->_originHgtMode & (1<<2)) == 0) {
loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt);
}
}