diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 5f7290de4c..38508e44a9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -68,7 +68,7 @@ void NavEKF3_core::FuseAirspeed() } SK_TAS[1] = SH_TAS[1]; - if (tasDataDelayed.healthy && !airDataFusionWindOnly) { + if (tasDataDelayed.allowFusion && !airDataFusionWindOnly) { Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]); Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]); Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]); @@ -84,7 +84,7 @@ void NavEKF3_core::FuseAirspeed() zero_range(&Kfusion[0], 0, 9); } - if (tasDataDelayed.healthy && !inhibitDelAngBiasStates && !airDataFusionWindOnly) { + if (tasDataDelayed.allowFusion && !inhibitDelAngBiasStates && !airDataFusionWindOnly) { Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]); Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]); Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]); @@ -93,7 +93,7 @@ void NavEKF3_core::FuseAirspeed() zero_range(&Kfusion[0], 10, 12); } - if (tasDataDelayed.healthy && !inhibitDelVelBiasStates && !airDataFusionWindOnly) { + if (tasDataDelayed.allowFusion && !inhibitDelVelBiasStates && !airDataFusionWindOnly) { for (uint8_t index = 0; index < 3; index++) { const uint8_t stateIndex = index + 13; if (!dvelBiasAxisInhibit[index]) { @@ -108,7 +108,7 @@ void NavEKF3_core::FuseAirspeed() } // zero Kalman gains to inhibit magnetic field state estimation - if (tasDataDelayed.healthy && !inhibitMagStates && !airDataFusionWindOnly) { + if (tasDataDelayed.allowFusion && !inhibitMagStates && !airDataFusionWindOnly) { Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]); Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]); Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]); @@ -120,7 +120,7 @@ void NavEKF3_core::FuseAirspeed() zero_range(&Kfusion[0], 16, 21); } - if (tasDataDelayed.healthy && !inhibitWindStates) { + if (tasDataDelayed.allowFusion && !inhibitWindStates) { 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 { @@ -135,16 +135,16 @@ void NavEKF3_core::FuseAirspeed() tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); // fail if the ratio is > 1, but don't fail if bad IMU data - const bool tasHealth = tasDataDelayed.healthy && ((tasTestRatio < 1.0f) || badIMUdata); + const bool isConsistent = (tasTestRatio < 1.0f) || badIMUdata; tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms; - if (!tasHealth) { + if (!isConsistent) { lastTasFailTime_ms = imuSampleTime_ms; } else { lastTasFailTime_ms = 0; } // test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth - if (tasHealth || (tasTimeout && posTimeout)) { + if (tasDataDelayed.allowFusion && (isConsistent || (tasTimeout && posTimeout))) { // restart the counter lastTasPassTime_ms = imuSampleTime_ms; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 0eee482cde..07bd834c02 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -77,7 +77,7 @@ void NavEKF3_core::setWindMagStateLearningMode() Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); ftype trueAirspeedVariance; - const bool haveAirspeedMeasurement = usingDefaultAirspeed || (tasDataDelayed.healthy && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed()); + const bool haveAirspeedMeasurement = usingDefaultAirspeed || (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; @@ -738,7 +738,7 @@ void NavEKF3_core::runYawEstimatorPrediction() } ftype trueAirspeed; - if (tasDataDelayed.healthy && assume_zero_sideslip()) { + if (tasDataDelayed.allowFusion && assume_zero_sideslip()) { trueAirspeed = MAX(tasDataDelayed.tas, 0.0f); } else { trueAirspeed = 0.0f; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index c0afcf6ae6..5398f7acac 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -831,13 +831,12 @@ void NavEKF3_core::readAirSpdData() const auto *airspeed = dal.airspeed(); if (airspeed && - airspeed->use(selected_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.healthy = airspeed->healthy(selected_airspeed); + 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; @@ -856,7 +855,7 @@ void NavEKF3_core::readAirSpdData() is_positive(defaultAirSpeed)) { tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar)); - tasDataDelayed.healthy = true; + tasDataDelayed.allowFusion = true; tasDataDelayed.time_ms = 0; usingDefaultAirspeed = true; } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 0eef8fd4f7..13250a71ec 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -559,7 +559,7 @@ private: struct tas_elements : EKF_obs_element_t { ftype tas; // true airspeed measurement (m/sec) ftype tasVariance; // variance of true airspeed measurement (m/sec)^2 - bool healthy; // true if measurement can be allowed to modify EKF states. + bool allowFusion; // true if measurement can be allowed to modify EKF states. }; struct of_elements : EKF_obs_element_t {