From 79148498d3793487732a89768e09b637bfddc798 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 15 Nov 2020 20:38:05 +1100 Subject: [PATCH] AP_NavEKF2: don't reset EKx_GPS_TYPE when GPS has no vertical velocity setting the parameter to 1 causes the following issues: - the GPS may not have vertical velocity at the time the parameter set happens, but may get it later when the GPS is fully configured - we may switch between GPS modules which do/don't have vertical velocity - the user may download parameters after the set(1), and end up with incorrect parameters they may later load onto the vehicle, permanently disabling use of vertical velocity --- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 6 ++++-- libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 10 ---------- 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 93e6059b30..78068d603b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -219,7 +219,8 @@ void NavEKF2_core::ResetHeight(void) // Reset the vertical velocity state using GPS vertical velocity if we are airborne // Check that GPS vertical velocity data is available and can be used - if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) { + if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 && + dal.gps().have_vertical_velocity() && !frontend->inhibitGpsVertVelUse) { stateStruct.velocity.z = gpsDataNew.vel.z; } else if (inFlight && useExtNavVel) { stateStruct.velocity.z = extNavVelNew.vel.z; @@ -705,7 +706,8 @@ void NavEKF2_core::FuseVelPosNED() // test velocity measurements uint8_t imax = 2; // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data - if (!useExtNavVel && (frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse)) { + if (!useExtNavVel && (frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE || + frontend->inhibitGpsVertVelUse || !dal.gps().have_vertical_velocity())) { imax = 1; } float innovVelSumSq = 0; // sum of squares of velocity innovations diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index d403bce0f3..9f49eb15ea 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -87,16 +87,6 @@ void NavEKF2_core::calcGpsGoodToAlign(void) gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); - } else if ((frontend->_fusionModeGPS == 0) && !gps.have_vertical_velocity()) { - // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail - gpsVertVelFail = true; - // if we have a 3D fix with no vertical velocity and - // EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not - // capable of giving a vertical velocity - if (gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D) { - frontend->_fusionModeGPS.set(1); - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1"); - } } else { gpsVertVelFail = false; }