AP_NavEKF: Fix bug in reporting of solution status

Because synthetic  position measurements at 0,0 are fused during position hold mode, a position timeout cannot be used as the only means of detecting position solution status.
This commit is contained in:
priseborough 2014-12-21 21:30:52 +11:00 committed by Randy Mackay
parent 92c3026072
commit 0a5de21dc7

View File

@ -4551,7 +4551,7 @@ void NavEKF::getFilterStatus(uint8_t &status) const
!(velTimeout && tasTimeout)<<1 |
!((velTimeout && hgtTimeout) || (hgtTimeout && _fusionModeGPS > 0))<<2 |
((gpsInhibitMode == 2 && flowDataValid) || (!tasTimeout && assume_zero_sideslip()))<<3 |
!posTimeout<<4 |
!(posTimeout || gpsInhibitMode == 1)<<4 |
!hgtTimeout<<5 |
!inhibitGndState<<6 |
posHoldMode<<7);