AP_NavEKF2: Fix bugs and consolidate aiding switch logic

Switching in and out of aiding modes was being performed in more than one place and was using two variables.
The reversion out of GPS mode due to prolonged loss of GPS was not working.
This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function.

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -157,33 +157,82 @@ void NavEKF2_core::setWindMagStateLearningMode()
 // Set inertial navigation aiding mode
 void NavEKF2_core::setAidingMode()
 {
-    // Determine when to commence aiding for inertial navigation
     // Save the previous status so we can detect when it has changed
-    prevIsAiding = isAiding;
-    // perform aiding checks if not aiding
-     if (!isAiding) {
+    PV_AidingModePrev = PV_AidingMode;
+
+    // Determine if we should start aiding
+     if (PV_AidingMode == AID_NONE) {
         // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
         // and IMU gyro bias estimates have stabilised
         bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
         // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
-        bool useFlowAiding = (frontend->_fusionModeGPS == 3) && optFlowDataPresent();
-        // Start aiding if we have a source of aiding data and the filter attitude algnment is complete
-        // Latch to on
-        isAiding = (readyToUseGPS() || useFlowAiding) && filterIsStable;
+        // GPS aiding is the perferred option unless excluded by the user
+        if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit) {
+            PV_AidingMode = AID_ABSOLUTE;
+        } else if ((frontend->_fusionModeGPS == 3) && optFlowDataPresent()) {
+            PV_AidingMode = AID_RELATIVE;
+        }
     }
 
+     // Determine if we should exit optical flow aiding
+     if (PV_AidingMode == AID_RELATIVE) {
+         // Check if the optical flow sensor has timed out
+         bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
+         // Check if the fusion has timed out (flow measurements have been rejected for too long)
+         bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
+         if (flowSensorTimeout || flowFusionTimeout) {
+             PV_AidingMode = AID_NONE;
+         }
+     }
+
+     // Determine if we should exit GPS aiding
+     if (PV_AidingMode == AID_ABSOLUTE) {
+         // check if we can use opticalflow as a backup
+         bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);
+
+         // Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
+         uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend->gpsRetryTimeUseTAS_ms : frontend->gpsRetryTimeNoTAS_ms;
+
+         // Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
+         uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend->gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
+
+         // If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
+         if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {
+
+             // Let other processes know that GPS is not available and that a timeout has occurred
+             posTimeout = true;
+             velTimeout = true;
+             gpsNotAvailable = true;
+
+             // If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
+             // If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
+             // If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
+             if (!useAirspeed() && !assume_zero_sideslip()) {
+                 if (optFlowBackupAvailable) {
+                     // attempt optical flow navigation
+                     PV_AidingMode = AID_RELATIVE;
+                 } else {
+                     // put the filter into constant position mode
+                     PV_AidingMode = AID_NONE;
+                 }
+             }
+         } else if (gpsInhibit) {
+             // put the filter into constant position mode in response to an exernal request
+             PV_AidingMode = AID_NONE;
+         }
+     }
+
     // check to see if we are starting or stopping aiding and set states and modes as required
-    if (isAiding != prevIsAiding) {
-        // We have transitioned either into or out of aiding
-        // zero stored velocities used to do dead-reckoning
-        heldVelNE.zero();
+    if (PV_AidingMode != PV_AidingModePrev) {
         // set various  usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
-        if (!isAiding) {
+        if (PV_AidingMode == AID_NONE) {
             // We have ceased aiding
             // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
-            PV_AidingMode = AID_NONE;
             posTimeout = true;
-            velTimeout = true;
+            velTimeout = true;            
+            // Reset the normalised innovation to avoid false failing bad fusion tests
+            velTestRatio = 0.0f;
+            posTestRatio = 0.0f;
              // store the current position to be used to keep reporting the last known position
             lastKnownPositionNE.x = stateStruct.position.x;
             lastKnownPositionNE.y = stateStruct.position.y;
@@ -192,20 +241,18 @@ void NavEKF2_core::setAidingMode()
             meaHgtAtTakeOff = baroDataDelayed.hgt;
             // reset the vertical position state to faster recover from baro errors experienced during touchdown
             stateStruct.position.z = -meaHgtAtTakeOff;
-        } else if (frontend->_fusionModeGPS == 3) {
+        } else if (PV_AidingMode == AID_RELATIVE) {
             // We have commenced aiding, but GPS usage has been prohibited so use optical flow only
             hal.console->printf("EKF2 IMU%u is using optical flow\n",(unsigned)imu_index);
-            PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
             posTimeout = true;
             velTimeout = true;
             // Reset the last valid flow measurement time
             flowValidMeaTime_ms = imuSampleTime_ms;
             // Reset the last valid flow fusion time
             prevFlowFuseTime_ms = imuSampleTime_ms;
-        } else {
+        } else if (PV_AidingMode == AID_ABSOLUTE) {
             // We have commenced aiding and GPS usage is allowed
             hal.console->printf("EKF2 IMU%u is using GPS\n",(unsigned)imu_index);
-            PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
             posTimeout = false;
             velTimeout = false;
             // we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
@@ -215,18 +262,13 @@ void NavEKF2_core::setAidingMode()
             // reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
             lastPosPassTime_ms = imuSampleTime_ms;
         }
-        // Reset the position and velocity
+
+        // Always reset the position and velocity when changing mode
         ResetVelocity();
         ResetPosition();
 
     }
 
-    // Always turn aiding off when the vehicle is disarmed
-    if (!isAiding) {
-        PV_AidingMode = AID_NONE;
-        posTimeout = true;
-        velTimeout = true;
-    }
 }
 
 // Check the tilt and yaw alignmnent status
@@ -299,7 +341,7 @@ bool NavEKF2_core::assume_zero_sideslip(void) const
 // set the LLH location of the filters NED origin
 bool NavEKF2_core::setOriginLLH(struct Location &loc)
 {
-    if (isAiding) {
+    if (PV_AidingMode == AID_ABSOLUTE) {
         return false;
     }
     EKF_origin = loc;
@@ -346,16 +388,13 @@ bool NavEKF2_core::checkGyroCalStatus(void)
 // Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
 uint8_t NavEKF2_core::setInhibitGPS(void)
 {
-    if(!isAiding) {
+    if(PV_AidingMode == AID_ABSOLUTE && motorsArmed) {
         return 0;
-    }
-    if (optFlowDataPresent()) {
-        frontend->_fusionModeGPS = 3;
-//#error writing to a tuning parameter
-        return 2;
     } else {
+        gpsInhibit = true;
         return 1;
     }
+    // option 2 is not yet implemented as it requires a deeper integration of optical flow and GPS operation
 }
 
 // Update the filter status

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -454,54 +454,6 @@ void NavEKF2_core::readGpsData()
             gpsCheckStatus.bad_fix = true;
         }
     }
-
-    // We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon
-    // If that happens we need to put the filter into a constant position mode, reset the velocity states to zero
-    // and use the last estimated position as a synthetic GPS position
-
-    // check if we can use opticalflow as a backup
-    bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);
-
-    // Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
-    uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend->gpsRetryTimeUseTAS_ms : frontend->gpsRetryTimeNoTAS_ms;
-
-    // Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
-    uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend->gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
-
-    // If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
-    if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {
-
-        // Let other processes know that GPS is not available and that a timeout has occurred
-        posTimeout = true;
-        velTimeout = true;
-        gpsNotAvailable = true;
-
-        // If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
-        // If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
-        // If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
-        if (PV_AidingMode == AID_ABSOLUTE && !useAirspeed() && !assume_zero_sideslip()) {
-            if (optFlowBackupAvailable) {
-                // we can do optical flow only nav
-                frontend->_fusionModeGPS = 3;
-                PV_AidingMode = AID_RELATIVE;
-            } else {
-                // store the current position
-                lastKnownPositionNE.x = stateStruct.position.x;
-                lastKnownPositionNE.y = stateStruct.position.y;
-
-                // put the filter into constant position mode
-                PV_AidingMode = AID_NONE;
-
-                // Reset the velocity and position states
-                ResetVelocity();
-                ResetPosition();
-
-                // Reset the normalised innovation to avoid false failing bad fusion tests
-                velTestRatio = 0.0f;
-                posTestRatio = 0.0f;
-            }
-        }
-    }
 }
 
 // read the delta angle and corresponding time interval from the IMU
@@ -534,7 +486,7 @@ void NavEKF2_core::readBaroData()
 
         // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
         // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
-        if (isAiding && getTakeoffExpected()) {
+        if (getTakeoffExpected()) {
             baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
         }
 

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -39,33 +39,17 @@ void NavEKF2_core::SelectFlowFusion()
     // Perform Data Checks
     // Check if the optical flow data is still valid
     flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
-    // Check if the optical flow sensor has timed out
-    bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
-    // Check if the fusion has timed out (flow measurements have been rejected for too long)
-    bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
     // check is the terrain offset estimate is still valid
     gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
     // Perform tilt check
     bool tiltOK = (Tnb_flow.c.z > frontend->DCM33FlowMin);
-    // Constrain measurements to zero if we are using optical flow and are on the ground
-    if (frontend->_fusionModeGPS == 3 && !takeOffDetected && isAiding) {
+    // Constrain measurements to zero if we are on the ground
+    if (frontend->_fusionModeGPS == 3 && !takeOffDetected) {
         ofDataDelayed.flowRadXYcomp.zero();
         ofDataDelayed.flowRadXY.zero();
         flowDataValid = true;
     }
 
-    // If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
-    if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
-        PV_AidingMode = AID_NONE;
-        // reset the velocity
-        ResetVelocity();
-        // store the current position to be used to as a sythetic position measurement
-        lastKnownPositionNE.x = stateStruct.position.x;
-        lastKnownPositionNE.y = stateStruct.position.y;
-        // reset the position
-        ResetPosition();
-    }
-
     // if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
     // we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
     if ((flowDataToFuse || rangeDataToFuse) && tiltOK) {

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -27,7 +27,7 @@ void NavEKF2_core::ResetVelocity(void)
 
     if (PV_AidingMode != AID_ABSOLUTE) {
         stateStruct.velocity.zero();
-    } else if (!gpsNotAvailable) {
+    } else {
         // reset horizontal velocity states to the GPS velocity
         stateStruct.velocity.x  = gpsDataNew.vel.x; // north velocity from blended accel data
         stateStruct.velocity.y  = gpsDataNew.vel.y; // east velocity from blended accel data
@@ -68,7 +68,7 @@ void NavEKF2_core::ResetPosition(void)
         // reset all position state history to the last known position
         stateStruct.position.x = lastKnownPositionNE.x;
         stateStruct.position.y = lastKnownPositionNE.y;
-    } else if (!gpsNotAvailable) {
+    } else  {
         // write to state vector and compensate for offset  between last GPs measurement and the EKF time horizon
         stateStruct.position.x = gpsDataNew.pos.x  + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
         stateStruct.position.y = gpsDataNew.pos.y  + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -176,10 +176,9 @@ void NavEKF2_core::InitialiseVariables()
     flowGyroBias.y = 0;
     heldVelNE.zero();
     PV_AidingMode = AID_NONE;
+    PV_AidingModePrev = AID_NONE;
     posTimeout = true;
     velTimeout = true;
-    isAiding = false;
-    prevIsAiding = false;
     memset(&faultStatus, 0, sizeof(faultStatus));
     hgtRate = 0.0f;
     mag_state.q0 = 1;
@@ -264,6 +263,7 @@ void NavEKF2_core::InitialiseVariables()
     magFieldLearned = false;
     delAngBiasLearned = false;
     memset(&filterStatus, 0, sizeof(filterStatus));
+    gpsInhibit = false;
 
     // zero data buffers
     storedIMU.reset();

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -733,8 +733,6 @@ private:
     bool inhibitWindStates;         // true when wind states and covariances are to remain constant
     bool inhibitMagStates;          // true when magnetic field states and covariances are to remain constant
     bool gpsNotAvailable;           // bool true when valid GPS data is not available
-    bool isAiding;                  // true when the filter is fusing position, velocity or flow measurements
-    bool prevIsAiding;              // isAiding from previous frame
     struct Location EKF_origin;     // LLH origin of the NED axis system - do not change unless filter is reset
     bool validOrigin;               // true when the EKF origin is valid
     float gpsSpdAccuracy;           // estimated speed accuracy in m/s returned by the GPS receiver
@@ -883,7 +881,9 @@ private:
                      AID_NONE=1,       // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
                      AID_RELATIVE=2    // only optical flow aiding is being used so position estimates will be relative
                     };
-    AidingMode PV_AidingMode;           // Defines the preferred mode for aiding of velocity and position estimates from the INS
+    AidingMode PV_AidingMode;       // Defines the preferred mode for aiding of velocity and position estimates from the INS
+    AidingMode PV_AidingModePrev;   // Value of PV_AidingMode from the previous frame - used to detect transitions
+    bool gpsInhibit;                // externally set flag informing the EKF not to use the GPS
     bool gndOffsetValid;            // true when the ground offset state can still be considered valid
     Vector3f delAngBodyOF;          // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
     float delTimeOF;                // time that delAngBodyOF is summed across