From 3e3dd9d695a4108cba2a93b4f2f4026ae759019b Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 1 Jan 2015 14:48:52 +1100 Subject: [PATCH] AP_NavEKF: Fix parenthesis error in EKF status reporting and clean up logic 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. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 3a21d211b7..83a3ffe28b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -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