From c81748bbe6f70492298c40c7e590ccf60c5cec94 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 3 Oct 2016 09:48:10 +1100 Subject: [PATCH 1/7] AP_NavEKF2: Enable simultaneous optical flow and GPS use Enables simultaneous use of GPS and optical flow data with automatic fallback to relative position mode if GPS is lost and automatic switch-up to absolute position status if GPS gained/re-gained. --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 6 +++++- libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 4 ++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index df39da9edd894..5c7e318302f32 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -178,7 +178,11 @@ void NavEKF2_core::setAidingMode() 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) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 8a2795a4fe8de..2126faa0ba099 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -59,8 +59,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)); From 596db2d6c91995aa09df972977b42328eefd6f60 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 5 Oct 2016 16:34:01 +1100 Subject: [PATCH 2/7] AP_NavEKF: Update GPS type parameter description --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index f498c50a28955..e7d5a76b00e66 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/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), From 775a12ab100b1c6a29507458430d8c03102a5b4f Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 5 Oct 2016 18:22:20 +1100 Subject: [PATCH 3/7] AP_NavEKF2: Enable entry into relative position mode on start-up --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 5c7e318302f32..cca8365be0929 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -170,7 +170,7 @@ 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) { From 93898ed65d2611a214cc33c4b4ac5150536792c8 Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 7 Oct 2016 07:51:08 +1100 Subject: [PATCH 4/7] AP_NavEKF2: fix reporting of optical flow use status --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index cca8365be0929..d3b154ff2096e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -407,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 From 9e679896fe8bd7f2c548343987e03d8af13f3775 Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 7 Oct 2016 07:58:20 +1100 Subject: [PATCH 5/7] AP_NavEKF2: Fix reporting of terrain estimator innovations Terrain height is relevant whenever optical flow data is present --- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index bc7de76d2060c..f20a00b7ea051 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/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; From 153fa434cc61940c4750c71c506ba8e8fa3d56e4 Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 7 Oct 2016 08:00:39 +1100 Subject: [PATCH 6/7] AP_NavEKF2: Update protection for out of focus flow data --- libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 2126faa0ba099..a46a930bc15f4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/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; From 86d43fb3ac5510c19befde71b0715d9866c7729e Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 7 Oct 2016 20:35:55 +1100 Subject: [PATCH 7/7] AP_NavEKF2: Always perform optical flow takeoff check when receiving data A specialised takeoff check is now always performed when we receive new flow data as the default behaviour is to try and use flow data whenever it is received, rather than limit its use to a use to a flow-only mode of operation that had to be selected via user parameter. --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index fbebe931b89c0..0b319761b32e5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/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)