diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 38508e44a9..79e18027ac 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -256,10 +256,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; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 0489aee688..c76d650aae 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; + windStateLastObsIsValid = 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; @@ -342,6 +343,15 @@ void NavEKF3_core::setAidingMode() // a sensor that only observes attitude velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed; + // Store the last wind states whose errors were constrained by measurements + // This will be used to synthesise an airspeed measurement to enable dead reckoning + bool newWindStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed); + if (!inhibitWindStates && !newWindStateIsObservable && windStateIsObservable) { + windStateLastObs = stateStruct.wind_vel; + windStateLastObsIsValid = true; + } + windStateIsObservable = newWindStateIsObservable; + // check if position drift has been constrained by a measurement source bool posAiding = posUsed || rngBcnUsed; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 1b25e84e65..8533490443 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -861,18 +861,29 @@ void NavEKF3_core::readAirSpdData() // 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; - } else { - usingDefaultAirspeed = false; + // Allow use of a default value or a value synthesised from a stored wind velocity vector + if (!useAirspeed()) { + if (is_positive(defaultAirSpeed)) { + if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) { + tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; + tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f))); + tasDataToFuse = true; + tasDataDelayed.allowFusion = true; + } else { + tasDataToFuse = false; + } + } else if (windStateLastObsIsValid && !windStateIsObservable) { + if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) { + // use stored wind state to synthesise an airspeed measurement + const Vector3F windRelVel = stateStruct.velocity - Vector3F(windStateLastObs.x, windStateLastObs.y, 0.0F); + tasDataDelayed.tas = windRelVel.length(); + tasDataDelayed.tasVariance = sq(MAX(frontend->_easNoise, 0.5f)); + tasDataToFuse = true; + tasDataDelayed.allowFusion = true; + } else { + tasDataToFuse = false; + } + } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index d1b4ba86c8..f608f6d4d2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -268,6 +268,9 @@ void NavEKF3_core::InitialiseVariables() prevInFlight = false; manoeuvring = false; inhibitWindStates = true; + windStateLastObs.zero(); + windStateIsObservable = false; + windStateLastObsIsValid = false; windStatesAligned = false; inhibitDelVelBiasStates = true; inhibitDelAngBiasStates = true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 8f3fd2d057..69cbfb5acf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1110,6 +1110,9 @@ private: 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 + Vector2F windStateLastObs; // wind states from the last time wind states were constrained using observations (m/s) + bool windStateLastObsIsValid; + bool windStateIsObservable; // true when wind states are observable from measurements. bool windStatesAligned; // true when wind states have been aligned bool inhibitMagStates; // true when magnetic field states are inactive bool lastInhibitMagStates; // previous inhibitMagStates @@ -1145,7 +1148,6 @@ private: 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