From 6ba8cd05ff2fa139331697f97610c29e58edeee8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Jun 2015 16:16:34 +1000 Subject: [PATCH] AP_NavEKF: Fix bug preventing GPS vertical velocity being used This fixes a bug that prevents GPS velocity being used permanently if there is a temporary unavailability --- libraries/AP_NavEKF/AP_NavEKF.cpp | 17 ++++++++--------- libraries/AP_NavEKF/AP_NavEKF.h | 1 + 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index f6f703c785..117ee192db 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -874,7 +874,7 @@ void NavEKF::SelectVelPosFusion() // If we haven't received height data for a while, then declare the height data as being timed out // set timeout period based on whether we have vertical GPS velocity available to constrain drift - hgtRetryTime = (_fusionModeGPS == 0 && !velTimeout) ? hgtRetryTimeMode0 : hgtRetryTimeMode12; + hgtRetryTime = (useGpsVertVel && !velTimeout) ? hgtRetryTimeMode0 : hgtRetryTimeMode12; if (imuSampleTime_ms - lastHgtMeasTime > hgtRetryTime) { hgtTimeout = true; } @@ -1974,7 +1974,7 @@ void NavEKF::FuseVelPosNED() // if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting // the accelerometers and we should disable the GPS and barometer innovation consistency checks. - if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtMeasTime) < (2 * msecHgtAvg)) { + if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtMeasTime) < (2 * msecHgtAvg)) { // calculate innovations for height and vertical GPS vel measurements float hgtErr = statesAtHgtTime.position.z - observation[5]; float velDErr = statesAtVelTime.velocity.z - observation[2]; @@ -2124,7 +2124,7 @@ void NavEKF::FuseVelPosNED() } // set range for sequential fusion of velocity and position measurements depending on which data is available and its health - if (fuseVelData && _fusionModeGPS == 0 && velHealth && !constPosMode && PV_AidingMode == AID_ABSOLUTE) { + if (fuseVelData && useGpsVertVel && velHealth && !constPosMode && PV_AidingMode == AID_ABSOLUTE) { fuseData[0] = true; fuseData[1] = true; fuseData[2] = true; @@ -4140,11 +4140,10 @@ void NavEKF::readGpsData() } // Check if GPS can output vertical velocity and set GPS fusion mode accordingly - if (!_ahrs->get_gps().have_vertical_velocity()) { - // vertical velocity should not be fused - if (_fusionModeGPS == 0) { - _fusionModeGPS = 1; - } + if (_ahrs->get_gps().have_vertical_velocity() && _fusionModeGPS == 0) { + useGpsVertVel = true; + } else { + useGpsVertVel = false; } // Monitor quality of the GPS velocity data for alignment @@ -4778,7 +4777,7 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); bool notDeadReckoning = !constVelMode && !constPosMode; - bool someVertRefData = (!velTimeout && (_fusionModeGPS == 0)) || !hgtTimeout; + bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool optFlowNavPossible = flowDataValid && (_fusionModeGPS == 3); bool gpsNavPossible = !gpsNotAvailable && (_fusionModeGPS <= 2); diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 80cdaca1be..ebb2fb1ba5 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -667,6 +667,7 @@ private: float posDownAtArming; // flight vehicle vertical position at arming used as a reference point bool highYawRate; // true when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference float yawRateFilt; // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference + bool useGpsVertVel; // true if GPS vertical velocity should be used // Used by smoothing of state corrections Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement