@ -4513,14 +4513,20 @@ return filter function status as a bitmasked integer
void NavEKF : : getFilterStatus ( uint8_t & status ) const
void NavEKF : : getFilterStatus ( uint8_t & status ) const
{
{
// add code to set bits using private filter data here
// add code to set bits using private filter data here
status = ( ! state . quat . is_nan ( ) < < 0 | // we can implement a better way to detect invalid attitude, but it is tricky to do reliably
bool doingFlowNav = ( PV_AidingMode = = RELATIVE ) & & flowDataValid ;
( ! ( velTimeout & & posTimeout & & tasTimeout ) & & ! constVelMode & & ! constPosMode ) < < 1 |
bool doingWindRelNav = ! tasTimeout & & assume_zero_sideslip ( ) ;
! ( ( velTimeout & & hgtTimeout ) | | ( hgtTimeout & & _fusionModeGPS > 0 ) ) < < 2 |
bool doingNormalGpsNav = ! posTimeout & & ( PV_AidingMode = = ABSOLUTE ) ;
( ( PV_AidingMode = = RELATIVE & & flowDataValid ) | | ( ! tasTimeout & & assume_zero_sideslip ( ) ) | | ( ! posTimeout & & PV_AidingMode = = ABSOLUTE ) & & ! constVelMode & & ! constPosMode ) < < 3 |
bool notDeadReckoning = ! constVelMode & & ! constPosMode ;
( ( ! posTimeout & & PV_AidingMode = = ABSOLUTE ) & & ! constVelMode & & ! constPosMode ) < < 4 |
bool someVertRefData = ( ! velTimeout & & ( _fusionModeGPS = = 0 ) ) | | ! hgtTimeout ;
! hgtTimeout < < 5 |
bool someHorizRefData = ! ( velTimeout & & posTimeout & & tasTimeout ) ;
! inhibitGndState < < 6 |
status = ( ! state . quat . is_nan ( ) < < 0 | // attitude valid (we need a better check)
constPosMode < < 7 ) ;
( someHorizRefData & & notDeadReckoning ) < < 1 | // horizontal velocity estimate valid
someVertRefData < < 2 | // vertical velocity estimate valid
( ( doingFlowNav | | doingWindRelNav | | doingNormalGpsNav ) & & notDeadReckoning ) < < 3 | // relative horizontal position estimate valid
( doingNormalGpsNav & & notDeadReckoning ) < < 4 | // absolute horizontal position estimate valid
! hgtTimeout < < 5 | // vertical position estimate valid
! inhibitGndState < < 6 | // terrain height estimate valid
constPosMode < < 7 ) ; // constant position mode
}
}
// Check arm status and perform required checks and mode changes
// Check arm status and perform required checks and mode changes