diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 74cda3285c..f206625e52 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -443,6 +443,7 @@ private: const uint16_t hgtRetryTimeMode0_ms = 10000; // Height retry time with vertical velocity measurement (msec) const uint16_t hgtRetryTimeMode12_ms = 5000; // Height retry time without vertical velocity measurement (msec) const uint16_t tasRetryTime_ms = 5000; // True airspeed timeout and retry interval (msec) + const uint16_t dragFailTimeLimit_ms = 5000; // Drag timeout (msec) const uint32_t magFailTimeLimit_ms = 10000; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) const float magVarRateScale = 0.005f; // scale factor applied to magnetometer variance due to angular rate const float gyroBiasNoiseScaler = 2.0f; // scale factor applied to gyro bias state process noise when on ground diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 8f5f90a149..720d19027e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -270,6 +270,7 @@ void NavEKF3_core::SelectBetaDragFusion() if (!inhibitWindStates && storedDrag.recall(dragSampleDelayed,imuDataDelayed.time_ms)) { FuseDragForces(); } + dragTimeout = (imuSampleTime_ms - lastDragPassTime_ms) > frontend->dragFailTimeLimit_ms; #endif } @@ -751,6 +752,9 @@ void NavEKF3_core::FuseDragForces() } } } + + // record time of successful fusion + lastDragPassTime_ms = imuSampleTime_ms; } #endif // EK3_FEATURE_DRAG_FUSION diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 3ba49286cc..51ddab2a39 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -299,6 +299,9 @@ void NavEKF3_core::setAidingMode() // Check if airspeed data is being used bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms); + // check if drag data is being used + bool dragUsed = (imuSampleTime_ms - lastDragPassTime_ms <= minTestTime_ms); + // Check if range beacon data is being used bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms); @@ -307,10 +310,10 @@ void NavEKF3_core::setAidingMode() bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); // Check if attitude drift has been constrained by a measurement source - bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed; + bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed; // check if velocity drift has been constrained by a measurement source - bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed; + bool velAiding = gpsVelUsed || airSpdUsed || dragUsed || optFlowUsed || bodyOdmUsed; // check if position drift has been constrained by a measurement source bool posAiding = posUsed || rngBcnUsed; @@ -344,6 +347,7 @@ void NavEKF3_core::setAidingMode() posTimeout = true; velTimeout = true; tasTimeout = true; + dragTimeout = true; gpsIsInUse = false; } else if (posAidLossCritical) { // if the loss of position is critical, declare all sources of position aiding as being timed out @@ -683,10 +687,10 @@ void NavEKF3_core::updateFilterStatus(void) filterStatus.value = 0; bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000); bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid; - bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); + bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip()) || !dragTimeout; bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); bool someVertRefData = (!velTimeout && (useGpsVertVel || useExtNavVel)) || !hgtTimeout; - bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav || doingBodyVelNav; + bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout && dragTimeout) || doingFlowNav || doingBodyVelNav; bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE))); // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index b5e77945f5..889d4a8db0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -146,7 +146,8 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const velTimeout<<1 | hgtTimeout<<2 | magTimeout<<3 | - tasTimeout<<4; + tasTimeout<<4 | + dragTimeout<<5; nav_filter_status solutionStatus {}; getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index e80533cf50..ac86dde5b1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -226,6 +226,7 @@ void NavEKF3_core::InitialiseVariables() gpsNoiseScaler = 1.0f; hgtTimeout = true; tasTimeout = true; + dragTimeout = true; badIMUdata = false; badIMUdata_ms = 0; goodIMUdata_ms = 0; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 049c35164e..01f7e81b7a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -983,6 +983,7 @@ private: bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out 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 uint32_t badIMUdata_ms; // time stamp bad IMU data was last detected uint32_t goodIMUdata_ms; // time stamp good IMU data was last detected @@ -1319,6 +1320,7 @@ private: Vector2F innovDragVar; // multirotor drag measurement innovation variance ((m/sec**2)**2) Vector2F dragTestRatio; // drag innovation consistency check ratio #endif + uint32_t lastDragPassTime_ms; // system time that drag samples were last successfully fused bool dragFusionEnabled; // height source selection logic diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 702c066682..60340e8507 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -185,7 +185,7 @@ struct PACKED log_XKF3 { // @Field: OFN: Most recent position reset (North component) // @Field: OFE: Most recent position reset (East component) // @Field: FS: Filter fault status -// @Field: TS: Filter timeout status bitmask (0:position measurement, 1:velocity measurement, 2:height measurement, 3:magnetometer measurement, 4:airspeed measurement) +// @Field: TS: Filter timeout status bitmask (0:position measurement, 1:velocity measurement, 2:height measurement, 3:magnetometer measurement, 4:airspeed measurement, 5:drag measurement) // @Field: SS: Filter solution status // @Field: GPS: Filter GPS status // @Field: PI: Primary core index