EKF2: Enable simultaneous optical flow and GPS use

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -131,8 +131,8 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
 
     // @Param: GPS_TYPE
     // @DisplayName: GPS mode control
-    // @Description: This controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = use no GPS (optical flow will be used if available)
-    // @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS use optical flow
+    // @Description: This controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = Inhibit GPS use - this can be useful when flying with an optical flow sensor in an environment where GPS quality is poor and subject to large multipath errors.
+    // @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS
     // @User: Advanced
     AP_GROUPINFO("GPS_TYPE", 1, NavEKF2, _fusionModeGPS, 0),
 

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -170,15 +170,19 @@ void NavEKF2_core::setAidingMode()
         // 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()) {
+        } else if (optFlowDataPresent() && filterIsStable) {
             PV_AidingMode = AID_RELATIVE;
         }
     } else 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) {
+         // Enable switch to absolute position mode if GPS is available
+         // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
+         if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
+             PV_AidingMode = AID_ABSOLUTE;
+         } else if (flowSensorTimeout || flowFusionTimeout) {
              PV_AidingMode = AID_NONE;
          }
      } else if (PV_AidingMode == AID_ABSOLUTE) {
@@ -403,7 +407,7 @@ void  NavEKF2_core::updateFilterStatus(void)
     bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
     bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
     bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
-    bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3) && delAngBiasLearned;
+    bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
     bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
     bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
     // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -127,11 +127,10 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
         delAngBodyOF.zero();
         delTimeOF = 0.0f;
     }
-    // check for takeoff if relying on optical flow and zero measurements until takeoff detected
-    // if we haven't taken off - constrain position and velocity states
-    if (frontend->_fusionModeGPS == 3) {
-        detectOptFlowTakeoff();
-    }
+    // by definition if this function is called, then flow measurements have been provided so we
+    // need to run the optical flow takeoff detection
+    detectOptFlowTakeoff();
+
     // calculate rotation matrices at mid sample time for flow observations
     stateStruct.quat.rotation_matrix(Tbn_flow);
     // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -43,8 +43,10 @@ void NavEKF2_core::SelectFlowFusion()
     gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
     // Perform tilt check
     bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
-    // Constrain measurements to zero if we are on the ground
-    if (frontend->_fusionModeGPS == 3 && !takeOffDetected) {
+    // Constrain measurements to zero if takeoff is not detected and the height above ground
+    // is insuffient to achieve acceptable focus. This allows the vehicle to be picked up
+    // and carried to test optical flow operation
+    if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) {
         ofDataDelayed.flowRadXYcomp.zero();
         ofDataDelayed.flowRadXY.zero();
         flowDataValid = true;
@@ -59,8 +61,8 @@ void NavEKF2_core::SelectFlowFusion()
         EstimateTerrainOffset();
     }
 
-    // Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode
-    if (flowDataToFuse && tiltOK && PV_AidingMode == AID_RELATIVE)
+    // Fuse optical flow data into the main filter
+    if (flowDataToFuse && tiltOK)
     {
         // Set the flow noise used by the fusion processes
         R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -520,10 +520,11 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan)
     Vector2f offset;
     getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
 
-    // Only report range finder normalised innovation levels if the EKF needs the data.
-    // This prevents false alarms at the GCS if a range finder is fitted for other applications
+    // Only report range finder normalised innovation levels if the EKF needs the data for primary
+    // height estimation or optical flow operation. This prevents false alarms at the GCS if a
+    // range finder is fitted for other applications
     float temp;
-    if ((frontend->_useRngSwHgt > 0) || PV_AidingMode == AID_RELATIVE) {
+    if ((frontend->_useRngSwHgt > 0) || PV_AidingMode == AID_RELATIVE || flowDataValid) {
         temp = sqrtf(auxRngTestRatio);
     } else {
         temp = 0.0f;