Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Allow reporting of airspeed consistency for a deselected sensor

This commit is contained in:
Paul Riseborough 2022-07-04 08:01:54 +10:00 committed by Peter Barker
parent eb0e9230a1
commit 399f30bfce
4 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -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;

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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;

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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 {

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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 {