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

AP_NavEKF2: set rejecting_airspeed flag 

we report as rejecting airspeed when we have not fused airspeed for 3s
and want to use airspeed
This commit is contained in:
Andrew Tridgell 2022-01-26 17:35:14 +11:00
parent a6142fa3dd
commit 949975a856
3 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -113,6 +113,12 @@ void NavEKF2_core::FuseAirspeed()
bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms; tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms;
if (!tasHealth) {
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 // 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 (tasHealth || (tasTimeout && posTimeout)) {

4
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -519,6 +519,10 @@ void NavEKF2_core::updateFilterStatus(void)
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && !extNavUsedForPos; // GPS glitching is affecting navigation accuracy filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && !extNavUsedForPos; // GPS glitching is affecting navigation accuracy
filterStatus.flags.gps_quality_good = gpsGoodToAlign; filterStatus.flags.gps_quality_good = gpsGoodToAlign;
// for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data
filterStatus.flags.rejecting_airspeed = lastTasFailTime_ms != 0 &&
(imuSampleTime_ms - lastTasFailTime_ms) < 1000 &&
(imuSampleTime_ms - lastTasPassTime_ms) > 3000;
filterStatus.flags.initalized = filterStatus.flags.initalized || healthy(); filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
} }

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -833,6 +833,7 @@ private:
uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec) uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec) uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec) uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint32_t lastTasFailTime_ms; // time stamp when airspeed measurement last failed innovation consistency check (msec)
uint32_t lastTimeGpsReceived_ms;// last time we received GPS data uint32_t lastTimeGpsReceived_ms;// last time we received GPS data
uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements uint32_t timeAtLastAuxEKF_ms; // last time the auxiliary filter was run to fuse range or optical flow measurements
uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy