Browse Source

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
zr-v5.1
Andrew Tridgell 4 years ago
parent
commit
79148498d3
  1. 6
      libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
  2. 10
      libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

6
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@ -219,7 +219,8 @@ void NavEKF2_core::ResetHeight(void) @@ -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() @@ -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

10
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp

@ -87,16 +87,6 @@ void NavEKF2_core::calcGpsGoodToAlign(void) @@ -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;
}

Loading…
Cancel
Save