mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 22:18:29 -04:00
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:
parent
92c3026072
commit
0a5de21dc7
@ -4551,7 +4551,7 @@ void NavEKF::getFilterStatus(uint8_t &status) const
|
|||||||
!(velTimeout && tasTimeout)<<1 |
|
!(velTimeout && tasTimeout)<<1 |
|
||||||
!((velTimeout && hgtTimeout) || (hgtTimeout && _fusionModeGPS > 0))<<2 |
|
!((velTimeout && hgtTimeout) || (hgtTimeout && _fusionModeGPS > 0))<<2 |
|
||||||
((gpsInhibitMode == 2 && flowDataValid) || (!tasTimeout && assume_zero_sideslip()))<<3 |
|
((gpsInhibitMode == 2 && flowDataValid) || (!tasTimeout && assume_zero_sideslip()))<<3 |
|
||||||
!posTimeout<<4 |
|
!(posTimeout || gpsInhibitMode == 1)<<4 |
|
||||||
!hgtTimeout<<5 |
|
!hgtTimeout<<5 |
|
||||||
!inhibitGndState<<6 |
|
!inhibitGndState<<6 |
|
||||||
posHoldMode<<7);
|
posHoldMode<<7);
|
||||||
|
Loading…
Reference in New Issue
Block a user