Browse Source

AP_NavEKF3: Add missing fallback to baro for range beacon use case

c415-sdk
Paul Riseborough 5 years ago committed by Randy Mackay
parent
commit
344d621a08
  1. 3
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

3
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@ -1034,7 +1034,8 @@ void NavEKF3_core::selectHeightForFusion() @@ -1034,7 +1034,8 @@ void NavEKF3_core::selectHeightForFusion()
bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500));
bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000));
if (lostRngHgt || lostGpsHgt || lostExtNavHgt) {
bool lostRngBcnHgt = ((activeHgtSource == HGT_SOURCE_BCN) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
if (lostRngHgt || lostGpsHgt || lostExtNavHgt || lostRngBcnHgt) {
activeHgtSource = HGT_SOURCE_BARO;
}

Loading…
Cancel
Save