Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 01:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: consolidate extNav height source timeouts

Browse Source
This commit is contained in:
Randy Mackay 2020-10-20 09:40:53 +09:00
parent 6989a28bfc
commit e4ccd21dd7
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -992,6 +992,7 @@ void NavEKF3_core::selectHeightForFusion()
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms); baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500); bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500);
const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500);
// select height source // select height source
if (extNavUsedForPos && (frontend->_altSource == 4)) { if (extNavUsedForPos && (frontend->_altSource == 4)) {
// always use external navigation as the height source if using for position. // always use external navigation as the height source if using for position.
@ -1043,14 +1044,14 @@ void NavEKF3_core::selectHeightForFusion()
activeHgtSource = HGT_SOURCE_GPS; activeHgtSource = HGT_SOURCE_GPS;
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) { } else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = HGT_SOURCE_BCN; activeHgtSource = HGT_SOURCE_BCN;
} else if ((frontend->_altSource == 4) && ((imuSampleTime_ms - extNavMeasTime_ms) < 500)) { } else if ((frontend->_altSource == 4) && extNavDataIsFresh) {
activeHgtSource = HGT_SOURCE_EXTNAV; activeHgtSource = HGT_SOURCE_EXTNAV;
} }
// Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon // Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && !rangeFinderDataIsFresh); bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && !rangeFinderDataIsFresh);
bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000)); bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && !extNavDataIsFresh);
bool lostRngBcnHgt = ((activeHgtSource == HGT_SOURCE_BCN) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000)); bool lostRngBcnHgt = ((activeHgtSource == HGT_SOURCE_BCN) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
if (lostRngHgt || lostGpsHgt || lostExtNavHgt || lostRngBcnHgt) { if (lostRngHgt || lostGpsHgt || lostExtNavHgt || lostRngBcnHgt) {
activeHgtSource = HGT_SOURCE_BARO; activeHgtSource = HGT_SOURCE_BARO;