Browse Source

AP_NavEKF3: 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
603e0c090d
  1. 6
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
  2. 10
      libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@ -245,7 +245,8 @@ void NavEKF3_core::ResetHeight(void) @@ -245,7 +245,8 @@ void NavEKF3_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->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) {
if (inFlight && !gpsNotAvailable && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) &&
!frontend->inhibitGpsVertVelUse && dal.gps().have_vertical_velocity(selected_gps)) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
stateStruct.velocity.z = extNavVelDelayed.vel.z;
@ -720,7 +721,8 @@ void NavEKF3_core::FuseVelPosNED() @@ -720,7 +721,8 @@ void NavEKF3_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 ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) {
if ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE ||
frontend->inhibitGpsVertVelUse || !dal.gps().have_vertical_velocity(selected_gps)) && !useExtNavVel) {
imax = 1;
}
float innovVelSumSq = 0; // sum of squares of velocity innovations

10
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@ -83,16 +83,6 @@ void NavEKF3_core::calcGpsGoodToAlign(void) @@ -83,16 +83,6 @@ void NavEKF3_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->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !gps.have_vertical_velocity(preferred_gps)) {
// 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
// EK3_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity
if (gps.status(preferred_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {
frontend->sources.setVelZSource(AP_NavEKF_Source::SourceZ::NONE);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EK3: Changed EK3_GPS_TYPE to 1");
}
} else {
gpsVertVelFail = false;
}

Loading…
Cancel
Save