aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Fix parenthesis error in EKF status reporting and clean up logic

Browse Source 
This fixes a bug which could have caused the realative position status to be incorrectly reported under some conditions and also caused a compiler warning message. the logic used to report the filter solution status has been broken down into smaller, easier to understand statements.
This commit is contained in:
priseborough 2015-01-01 14:48:52 +11:00
parent c06f6e105e
commit 3e3dd9d695
1 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4513,14 +4513,20 @@ return filter function status as a bitmasked integer
void NavEKF::getFilterStatus(uint8_t &status) const
{
// 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
(!(velTimeout && posTimeout && tasTimeout) && !constVelMode && !constPosMode)<<1 |
!((velTimeout && hgtTimeout) || (hgtTimeout && _fusionModeGPS > 0))<<2 |
((PV_AidingMode == RELATIVE && flowDataValid) || (!tasTimeout && assume_zero_sideslip()) || (!posTimeout && PV_AidingMode == ABSOLUTE) && !constVelMode && !constPosMode)<<3 |
((!posTimeout && PV_AidingMode == ABSOLUTE) && !constVelMode && !constPosMode)<<4 |
!hgtTimeout<<5 |
!inhibitGndState<<6 |
constPosMode<<7);
bool doingFlowNav = (PV_AidingMode == RELATIVE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == ABSOLUTE);
bool notDeadReckoning = !constVelMode && !constPosMode;
bool someVertRefData = (!velTimeout && (_fusionModeGPS == 0)) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout);
status = (!state.quat.is_nan()<<0 | // attitude valid (we need a better check)
(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