diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 6df913906b75a..b646f909a7fb6 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1922,6 +1922,22 @@ def GPSGlitchAuto(self, timeout=180): self.show_gps_and_sim_positions(False) raise e + # stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode + self.change_mode("LOITER") + self.set_parameters({ + "SIM_GPS_DISABLE": 1, + }) + self.delay_sim_time(2) + self.set_parameters({ + "SIM_GPS_DISABLE": 0, + }) + # regaining GPS should not result in it falling back to a non-navigation mode + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1) + # It should still be navigating after enougnh time has passed for any pending timeouts to activate. + self.delay_sim_time(10) + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1) + self.change_mode("AUTO") + # record time and position tstart = self.get_sim_time() diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ba7a2eed20db4..3f714260b008f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2136,6 +2136,9 @@ def AIRSPEED_AUTOCAL(self): self.fly_home_land_and_disarm() def deadreckoning_main(self, disable_airspeed_sensor=False): + self.set_parameter("EK3_OPTIONS", 1) + self.set_parameter("AHRS_OPTIONS", 3) + self.set_parameter("LOG_REPLAY", 1) self.reboot_sitl() self.wait_ready_to_arm() self.gpi = None @@ -2153,7 +2156,10 @@ def validate_global_position_int_against_simstate(mav, m): if self.simstate is None: return divergence = self.get_distance_int(self.gpi, self.simstate) - max_allowed_divergence = 200 + if disable_airspeed_sensor: + max_allowed_divergence = 300 + else: + max_allowed_divergence = 150 if (time.time() - self.last_print > 1 or divergence > self.max_divergence): self.progress("position-estimate-divergence=%fm" % (divergence,)) @@ -2188,15 +2194,42 @@ def validate_global_position_int_against_simstate(mav, m): frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, ) self.wait_location(loc, accuracy=100) - self.progress("Stewing") - self.delay_sim_time(20) + self.progress("Orbit with GPS and learn wind") + # allow longer to learn wind if there is no airspeed sensor + if disable_airspeed_sensor: + self.delay_sim_time(60) + else: + self.delay_sim_time(20) self.set_parameter("SIM_GPS_DISABLE", 1) - self.progress("Roasting") + self.progress("Continue orbit without GPS") self.delay_sim_time(20) self.change_mode("RTL") self.wait_distance_to_home(100, 200, timeout=200) + # go into LOITER to create additonal time for a GPS re-enable test + self.change_mode("LOITER") self.set_parameter("SIM_GPS_DISABLE", 0) + t_enabled = self.get_sim_time() + # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning + # to prevent bad GPS being used when coming back after loss of lock due to interence. + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15) + if self.get_sim_time() < (t_enabled+9): + raise NotAchievedException("GPS use re-started too quickly") + # wait for EKF and vehicle position to stabilise, then test response to jamming + self.delay_sim_time(20) + + self.set_parameter("AHRS_OPTIONS", 1) + self.set_parameter("SIM_GPS_JAM", 1) self.delay_sim_time(10) + self.set_parameter("SIM_GPS_JAM", 0) + t_enabled = self.get_sim_time() + # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning + # to prevent bad GPS being used when coming back after loss of lock due to interence. + # The EKF_STATUS_REPORT does not tell us when the good to align check passes, so the minimum time + # value of 3.0 seconds is an arbitrary value set on inspection of dataflash logs from this test + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15) + time_since_jamming_stopped = self.get_sim_time() - t_enabled + if time_since_jamming_stopped < 3: + raise NotAchievedException("GPS use re-started %f sec after jamming stopped" % time_since_jamming_stopped) self.set_rc(3, 1000) self.fly_home_land_and_disarm() self.progress("max-divergence: %fm" % (self.max_divergence,)) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 6b26791589446..2021b69ba2195 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -734,6 +734,13 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { // @Units: m AP_GROUPINFO("GPS_VACC_MAX", 10, NavEKF3, _gpsVAccThreshold, 0.0f), + // @Param: OPTIONS + // @DisplayName: Optional EKF behaviour + // @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF nehaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad + // @Bitmask: 0:JammingExpected + // @User: Advanced + AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 0), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 9d3303b40df2a..f47f96e8ac157 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -448,6 +448,12 @@ class NavEKF3 { AP_Int8 _primary_core; // initial core number AP_Enum _log_level; // log verbosity level AP_Float _gpsVAccThreshold; // vertical accuracy threshold to use GPS as an altitude source + AP_Int32 _options; // bit mask of processing options + + // enum for processing options + enum class Options { + JammingExpected = (1<<0), + }; // Possible values for _flowUse #define FLOW_USE_NONE 0 @@ -487,6 +493,7 @@ class NavEKF3 { const uint8_t extNavIntervalMin_ms = 20; // The minimum allowed time between measurements from external navigation sensors (msec) const float maxYawEstVelInnov = 2.0f; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s) const uint16_t deadReckonDeclare_ms = 1000; // Time without equivalent position or velocity observation to constrain drift before dead reckoning is declared (msec) + const uint16_t gpsNoFixTimeout_ms = 2000; // Time without a fix required to reset GPS alignment checks when EK3_OPTIONS bit 0 is set (msec) // time at start of current filter update uint64_t imuSampleTime_us; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 38508e44a9a15..a6b71e4a03407 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -120,7 +120,7 @@ void NavEKF3_core::FuseAirspeed() zero_range(&Kfusion[0], 16, 21); } - if (tasDataDelayed.allowFusion && !inhibitWindStates) { + if (tasDataDelayed.allowFusion && !inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); } else { @@ -212,8 +212,10 @@ void NavEKF3_core::SelectTasFusion() readAirSpdData(); // if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion + if (tasDataToFuse && statesInitialised && !inhibitWindStates) { FuseAirspeed(); + tasDataToFuse = false; prevTasStep_ms = imuSampleTime_ms; } } @@ -256,10 +258,6 @@ void NavEKF3_core::SelectBetaDragFusion() // we are required to correct only wind states airDataFusionWindOnly = true; } - // Fuse estimated airspeed to aid wind estimation - if (usingDefaultAirspeed) { - FuseAirspeed(); - } FuseSideslip(); prevBetaDragStep_ms = imuSampleTime_ms; } @@ -424,7 +422,7 @@ void NavEKF3_core::FuseSideslip() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index c886f48ab6328..2f858548721ca 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -64,6 +64,7 @@ void NavEKF3_core::setWindMagStateLearningMode() PV_AidingMode != AID_NONE; if (!inhibitWindStates && !canEstimateWind) { inhibitWindStates = true; + lastAspdEstIsValid = false; updateStateIndexLim(); } else if (inhibitWindStates && canEstimateWind && (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) { @@ -77,7 +78,7 @@ void NavEKF3_core::setWindMagStateLearningMode() Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); ftype trueAirspeedVariance; - const bool haveAirspeedMeasurement = usingDefaultAirspeed || (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed()); + const bool haveAirspeedMeasurement = (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed()); if (haveAirspeedMeasurement) { trueAirspeedVariance = constrain_ftype(tasDataDelayed.tasVariance, WIND_VEL_VARIANCE_MIN, WIND_VEL_VARIANCE_MAX); const ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; @@ -337,8 +338,17 @@ void NavEKF3_core::setAidingMode() // Check if attitude drift has been constrained by a measurement source bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed; - // check if velocity drift has been constrained by a measurement source - bool velAiding = gpsVelUsed || airSpdUsed || dragUsed || optFlowUsed || bodyOdmUsed; + // Check if velocity drift has been constrained by a measurement source + // Currently these are all the same source as will stabilise attitude because we do not currently have + // a sensor that only observes attitude + velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed; + + // Store the last valid airspeed estimate + windStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed); + if (windStateIsObservable) { + lastAirspeedEstimate = (stateStruct.velocity - Vector3F(stateStruct.wind_vel.x, stateStruct.wind_vel.y, 0.0F)).length(); + lastAspdEstIsValid = true; + } // check if position drift has been constrained by a measurement source bool posAiding = posUsed || rngBcnUsed; @@ -684,9 +694,11 @@ bool NavEKF3_core::setOrigin(const Location &loc) return true; } -// record a yaw reset event -void NavEKF3_core::recordYawReset() +// record all requested yaw resets completed +void NavEKF3_core::recordYawResetsCompleted() { + gpsYawResetRequest = false; + magYawResetRequest = false; yawAlignComplete = true; if (inFlight) { finalInflightYawInit = true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 93e3db98d5b90..726b091b3e71b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -100,7 +100,9 @@ void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const baro_index : selected_baro, gps_index : selected_gps, airspeed_index : getActiveAirspeed(), - source_set : frontend->sources.getPosVelYawSourceSet() + source_set : frontend->sources.getPosVelYawSourceSet(), + gps_good_to_align : gpsGoodToAlign, + wait_for_gps_checks : waitingForGpsChecks }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index e8b635e560fed..402cfd97867bc 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -154,6 +154,15 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete; + // get yaw variance from GPS speed uncertainty + const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise)); + const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F))); + if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) { + yawAlignGpsValidCount++; + } else { + yawAlignGpsValidCount = 0; + } + // correct yaw angle using GPS ground course if compass yaw bad if (badMagYaw) { // attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches @@ -163,16 +172,6 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) return; } - // get yaw variance from GPS speed uncertainty - const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise)); - const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F))); - - if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) { - yawAlignGpsValidCount++; - } else { - yawAlignGpsValidCount = 0; - } - if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) { yawAlignGpsValidCount = 0; // keep roll and pitch and reset yaw @@ -194,6 +193,10 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) allMagSensorsFailed = false; } } + } else if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) { + // There is no need to do a yaw reset + yawAlignGpsValidCount = 0; + recordYawResetsCompleted(); } } else { yawAlignGpsValidCount = 0; @@ -642,7 +645,7 @@ void NavEKF3_core::FuseMagnetometer() } // zero Kalman gains to inhibit wind state estimation - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); } else { @@ -725,7 +728,7 @@ void NavEKF3_core::FuseMagnetometer() } // zero Kalman gains to inhibit wind state estimation - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); } else { @@ -809,7 +812,7 @@ void NavEKF3_core::FuseMagnetometer() } // zero Kalman gains to inhibit wind state estimation - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); } else { @@ -1345,7 +1348,7 @@ void NavEKF3_core::FuseDeclination(ftype declErr) zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); } else { @@ -1579,7 +1582,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset) } // record the yaw reset event - recordYawReset(); + recordYawResetsCompleted(); // reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly ResetVelocity(resetDataSource::DEFAULT); @@ -1655,10 +1658,5 @@ void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationO lastYawReset_ms = imuSampleTime_ms; // record the yaw reset event - recordYawReset(); - - // clear all pending yaw reset requests - gpsYawResetRequest = false; - magYawResetRequest = false; - + recordYawResetsCompleted(); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 1440a18a49828..b5a933645c790 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -633,6 +633,27 @@ void NavEKF3_core::readGpsData() useGpsVertVel = false; } + if ((frontend->_options & (int32_t)NavEKF3::Options::JammingExpected) && + (lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) { + const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000); + const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);; + const bool canDoWindRelNav = assume_zero_sideslip(); + const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || canDoWindRelNav || doingBodyVelNav); + if (canDeadReckon) { + // If we can do dead reckoning with a data source other than GPS there is time to wait + // for GPS alignment checks to pass before using GPS inside the EKF. + // this gets set back to false in calcGpsGoodToAlign() when GPS checks pass + waitingForGpsChecks = true; + // force GPS checks to restart + lastPreAlignGpsCheckTime_ms = 0; + lastGpsVelFail_ms = imuSampleTime_ms; + lastGpsVelPass_ms = 0; + gpsGoodToAlign = false; + } else { + waitingForGpsChecks = false; + } + } + // Monitor quality of the GPS velocity data before and after alignment calcGpsGoodToAlign(); @@ -681,7 +702,8 @@ void NavEKF3_core::readGpsData() } // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin - if (validOrigin) { + // and are not waiting for GPs checks to pass + if (validOrigin && !waitingForGpsChecks) { gpsDataNew.lat = gpsloc.lat; gpsDataNew.lng = gpsloc.lng; if ((frontend->_originHgtMode & (1<<2)) == 0) { @@ -828,37 +850,52 @@ void NavEKF3_core::readAirSpdData() // we take a new reading, convert from EAS to TAS and set the flag letting other functions // know a new measurement is available - const auto *airspeed = dal.airspeed(); - if (airspeed && - (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) { - tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS; - timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed); - tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; - tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f)); - tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed); - - // Correct for the average intersampling delay due to the filter update rate - tasDataNew.time_ms -= localFilterTimeStep_ms/2; - - // Save data into the buffer to be fused when the fusion time horizon catches up with it - storedTAS.push(tasDataNew); - } - - // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused - tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms); - - float easErrVar = sq(MAX(frontend->_easNoise, 0.5f)); - // Allow use of a default value if enabled - if (!useAirspeed() && - imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200 && - is_positive(defaultAirSpeed)) { - tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; - tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar)); - tasDataDelayed.allowFusion = true; - tasDataDelayed.time_ms = 0; - usingDefaultAirspeed = true; + if (useAirspeed()) { + const auto *airspeed = dal.airspeed(); + if (airspeed && + (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) { + tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed); + if (tasDataNew.allowFusion) { + tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS; + timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed); + tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; + tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f)); + // Correct for the average intersampling delay due to the filter update rate + tasDataNew.time_ms -= localFilterTimeStep_ms/2; + // Save data into the buffer to be fused when the fusion time horizon catches up with it + storedTAS.push(tasDataNew); + } + } + // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused + tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms); } else { - usingDefaultAirspeed = false; + if (is_positive(defaultAirSpeed)) { + // this is the preferred method with the autopilot providing a model based airspeed estimate + if (imuDataDelayed.time_ms - prevTasStep_ms > 200 ) { + tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; + tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f))); + tasDataToFuse = true; + tasDataDelayed.allowFusion = true; + tasDataDelayed.time_ms = imuDataDelayed.time_ms; + } else { + tasDataToFuse = false; + tasDataDelayed.allowFusion = false; + } + } else if (lastAspdEstIsValid && !windStateIsObservable) { + // this uses the last airspeed estimated before dead reckoning started and + // wind states became unobservable + if (lastAspdEstIsValid && imuDataDelayed.time_ms - prevTasStep_ms > 200) { + tasDataDelayed.tas = lastAirspeedEstimate; + // this airspeed estimate has a lot of uncertainty + tasDataDelayed.tasVariance = sq(MAX(MAX(frontend->_easNoise, 0.5f), 0.5f * lastAirspeedEstimate)); + tasDataToFuse = true; + tasDataDelayed.allowFusion = true; + tasDataDelayed.time_ms = imuDataDelayed.time_ms; + } else { + tasDataToFuse = false; + tasDataDelayed.allowFusion = false; + } + } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index e457bf7a03e52..84dd164bac161 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -491,7 +491,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); } else { @@ -668,7 +668,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index d8a28e15ad4f8..e995a9e30c76b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -494,7 +494,8 @@ void NavEKF3_core::SelectVelPosFusion() readGpsYawData(); // get data that has now fallen behind the fusion time horizon - gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms); + gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms) && !waitingForGpsChecks; + if (gpsDataToFuse) { CorrectGPSForAntennaOffset(gpsDataDelayed); // calculate innovations and variances for reporting purposes only @@ -794,7 +795,7 @@ void NavEKF3_core::FuseVelPosNED() // if timed out or outside the specified uncertainty radius, reset to the external sensor // if velocity drift is being constrained, dont reset until gps passes quality checks const bool posVarianceIsTooLarge = (frontend->_gpsGlitchRadiusMax > 0) && (P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax)); - if (posTimeout || posVarianceIsTooLarge) { + if ((posTimeout || posVarianceIsTooLarge) && (!velAiding || gpsGoodToAlign)) { // reset the position to the current external sensor position ResetPosition(resetDataSource::DEFAULT); @@ -1045,7 +1046,7 @@ void NavEKF3_core::FuseVelPosNED() } // inhibit wind state estimation by setting Kalman gains to zero - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = P[22][stateIndex]*SK; Kfusion[23] = P[23][stateIndex]*SK; } else { @@ -1562,7 +1563,7 @@ void NavEKF3_core::FuseBodyVel() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24); Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24); } else { @@ -1739,7 +1740,7 @@ void NavEKF3_core::FuseBodyVel() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25); Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25); } else { @@ -1917,7 +1918,7 @@ void NavEKF3_core::FuseBodyVel() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24); Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 92a2ba4eeca5f..f3b332dbf1e8a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -234,7 +234,7 @@ void NavEKF3_core::FuseRngBcn() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9); Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 47e42773613ff..e6876b07a4484 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -12,7 +12,7 @@ */ void NavEKF3_core::calcGpsGoodToAlign(void) { - if (inFlight && assume_zero_sideslip() && !use_compass()) { + if (inFlight && !finalInflightYawInit && assume_zero_sideslip() && !use_compass()) { // this is a special case where a plane has launched without magnetometer // is now in the air and needs to align yaw to the GPS and start navigating as soon as possible gpsGoodToAlign = true; @@ -230,6 +230,10 @@ void NavEKF3_core::calcGpsGoodToAlign(void) } else if (gpsGoodToAlign && imuSampleTime_ms - lastGpsVelPass_ms > 5000) { gpsGoodToAlign = false; } + + if (gpsGoodToAlign && waitingForGpsChecks) { + waitingForGpsChecks = false; + } } // update inflight calculaton that determines if GPS data is good enough for reliable navigation diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 6c750ff63a891..008c9e2c01c7e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -258,6 +258,8 @@ void NavEKF3_core::InitialiseVariables() PV_AidingModePrev = AID_NONE; posTimeout = true; velTimeout = true; + velAiding = false; + waitingForGpsChecks = false; memset(&faultStatus, 0, sizeof(faultStatus)); hgtRate = 0.0f; onGround = true; @@ -266,6 +268,9 @@ void NavEKF3_core::InitialiseVariables() prevInFlight = false; manoeuvring = false; inhibitWindStates = true; + windStateIsObservable = false; + treatWindStatesAsTruth = false; + lastAspdEstIsValid = false; windStatesAligned = false; inhibitDelVelBiasStates = true; inhibitDelAngBiasStates = true; @@ -1080,10 +1085,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) if (!inhibitWindStates) { const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout; - if (isDragFusionDeadReckoning) { - // when dead reckoning using drag fusion stop learning wind states to provide a more stable velocity estimate + const bool newTreatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable; + if (newTreatWindStatesAsTruth) { + treatWindStatesAsTruth = true; P[23][23] = P[22][22] = 0.0f; } else { + if (treatWindStatesAsTruth) { + treatWindStatesAsTruth = false; + if (windStateIsObservable) { + // allow EKF to relearn wind states rapidly + P[23][23] = P[22][22] = sq(WIND_VEL_VARIANCE_MAX); + } + } ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate))); if (!tasDataDelayed.allowFusion) { // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor @@ -1941,7 +1954,11 @@ void NavEKF3_core::ConstrainVariances() } if (!inhibitWindStates) { - for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX); + if (treatWindStatesAsTruth) { + P[23][23] = P[22][22] = 0.0f; + } else { + for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX); + } } else { zeroCols(P,22,23); zeroRows(P,22,23); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index eef68881dd4a6..2049e7278fad6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -957,8 +957,8 @@ class NavEKF3_core : public NavEKF_core_common // zero attitude state covariances, but preserve variances void zeroAttCovOnly(); - // record a yaw reset event - void recordYawReset(); + // record all requested yaw resets completed + void recordYawResetsCompleted(); // record a magnetic field state reset event void recordMagReset(); @@ -1037,6 +1037,8 @@ class NavEKF3_core : public NavEKF_core_common bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out bool dragTimeout; // boolean true if drag measurements have failed for too long and have timed out bool badIMUdata; // boolean true if the bad IMU data is detected + bool velAiding; // boolean true if the velocity drift is constrained by observations + bool waitingForGpsChecks; // boolean true if the EKF should write GPS data to the buffer until quality checks have passed uint32_t badIMUdata_ms; // time stamp bad IMU data was last detected uint32_t goodIMUdata_ms; // time stamp good IMU data was last detected uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit @@ -1107,7 +1109,11 @@ class NavEKF3_core : public NavEKF_core_common ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold - bool inhibitWindStates; // true when wind states and covariances are to remain constant + bool inhibitWindStates; // true when wind states and covariances should not be used + ftype lastAirspeedEstimate; // last true airspeed estimate (m/s) + bool lastAspdEstIsValid; // true when the last true airspeed estimate is valid (m/s) + bool windStateIsObservable; // true when wind states are observable from measurements. + bool treatWindStatesAsTruth; // true when wind states should be used as a truth reference bool windStatesAligned; // true when wind states have been aligned bool inhibitMagStates; // true when magnetic field states are inactive bool lastInhibitMagStates; // previous inhibitMagStates @@ -1143,7 +1149,6 @@ class NavEKF3_core : public NavEKF_core_common range_elements rangeDataDelayed;// Range finder data at the fusion time horizon tas_elements tasDataNew; // TAS data at the current time horizon tas_elements tasDataDelayed; // TAS data at the fusion time horizon - bool usingDefaultAirspeed; // true when a default airspeed is being used instead of a measured value mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon gps_elements gpsDataNew; // GPS data at the current time horizon gps_elements gpsDataDelayed; // GPS data at the fusion time horizon diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 8e124462fcf0d..59ea98f9e42b5 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -344,6 +344,8 @@ struct PACKED log_XKQ { // @Field: GI: GPS selection index // @Field: AI: airspeed selection index // @Field: SS: Source Set (primary=0/secondary=1/tertiary=2) +// @Field: GPS_GTA: GPS good to align +// @Field: GPS_CHK_WAIT: Waiting for GPS checks to pass struct PACKED log_XKFS { LOG_PACKET_HEADER; uint64_t time_us; @@ -353,6 +355,8 @@ struct PACKED log_XKFS { uint8_t gps_index; uint8_t airspeed_index; uint8_t source_set; + uint8_t gps_good_to_align; + uint8_t wait_for_gps_checks; }; // @LoggerMessage: XKTV @@ -439,8 +443,8 @@ struct PACKED log_XKV { { LOG_XKFM_MSG, sizeof(log_XKFM), \ "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \ { LOG_XKFS_MSG, sizeof(log_XKFS), \ - "XKFS","QBBBBBB","TimeUS,C,MI,BI,GI,AI,SS", "s#-----", "F------" , true }, \ - { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#----", "F-0000" , true }, \ + "XKFS","QBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT", "s#-------", "F--------" , true }, \ + { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \ { LOG_XKT_MSG, sizeof(log_XKT), \ "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \ { LOG_XKTV_MSG, sizeof(log_XKTV), \