Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: set rejecting_airspeed flag

Browse Source 
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 949975a856
commit 9dcff1a23f
3 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -137,6 +137,11 @@ void NavEKF3_core::FuseAirspeed()
// fail if the ratio is > 1, but don't fail if bad IMU data
bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
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
if (tasHealth || (tasTimeout && posTimeout)) {

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -709,6 +709,10 @@ void NavEKF3_core::updateFilterStatus(void)
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy
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();
}

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1039,6 +1039,7 @@ private:
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 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 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