AP_OpticalFlow: Use flow rates and add ground distance field in MAVLink driver

libraries/AP_AHRS/AP_AHRS.cpp

@@ -2455,13 +2455,13 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const
 }
 
 // write optical flow data to EKF
-void  AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
+void  AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
 {
 #if HAL_NAVEKF2_AVAILABLE
     EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
 #endif
 #if EK3_FEATURE_OPTFLOW_FUSION
-    EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
+    EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, rawGroundDistance, msecFlowMeas, posOffset, heightOverride);
 #endif
 }
 

libraries/AP_AHRS/AP_AHRS.h

@@ -308,7 +308,7 @@ class AP_AHRS {
     bool get_vert_pos_rate_D(float &velocity) const;
 
     // write optical flow measurements to EKF
-    void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride);
+    void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride);
 
     // retrieve latest corrected optical flow samples (used for calibration)
     bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;

libraries/AP_DAL/AP_DAL.cpp

@@ -341,14 +341,15 @@ bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
 }
 
 // log optical flow data
-void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
+void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
 {
     end_frame();
 
     const log_ROFH old = _ROFH;
     _ROFH.rawFlowQuality = rawFlowQuality;
     _ROFH.rawFlowRates = rawFlowRates;
     _ROFH.rawGyroRates = rawGyroRates;
+    _ROFH.rawGroundDistance = rawGroundDistance;
     _ROFH.msecFlowMeas = msecFlowMeas;
     _ROFH.posOffset = posOffset;
     _ROFH.heightOverride = heightOverride;
@@ -474,7 +475,7 @@ void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
     _ROFH = msg;
     ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
 #if EK3_FEATURE_OPTFLOW_FUSION
-    ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
+    ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.rawGroundDistance, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
 #endif
 }
 

libraries/AP_DAL/AP_DAL.h

@@ -220,7 +220,7 @@ class AP_DAL {
     }
 
     // log optical flow data
-    void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
+    void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
 
     // log external nav data
     void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);

libraries/AP_DAL/LogStructure.h

@@ -465,6 +465,7 @@ struct log_RVOH {
 // @Field: FY: raw flow rate, Y-axis
 // @Field: GX: gyro rate, X-axis
 // @Field: GY: gyro rate, Y-axis
+// @Field: DG: altitude, Z-axis (mavlink only)
 // @Field: Tms: measurement timestamp
 // @Field: PX:gyro rate, X-axis
 // @Field: PY: body-frame offset, Y-axis
@@ -474,6 +475,7 @@ struct log_RVOH {
 struct log_ROFH {
     Vector2f rawFlowRates;
     Vector2f rawGyroRates;
+    float rawGroundDistance;
     uint32_t msecFlowMeas;
     Vector3f posOffset;
     float heightOverride;
@@ -636,7 +638,7 @@ struct log_RBOH {
     { LOG_RVOH_MSG, RLOG_SIZE(RVOH),                                   \
       "RVOH", "fffIBB", "OX,OY,OZ,Del,H,Ena", "------", "------" }, \
     { LOG_ROFH_MSG, RLOG_SIZE(ROFH),                                   \
-      "ROFH", "ffffIffffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,HgtOvr,Qual", "----------", "----------" }, \
+      "ROFH", "fffffIffffB", "FX,FY,GX,GY,DG,Tms,PX,PY,PZ,HgtOvr,Qual", "-----------", "-----------" }, \
     { LOG_REPH_MSG, RLOG_SIZE(REPH),                                   \
       "REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
     { LOG_RSLL_MSG, RLOG_SIZE(RSLL),                         \

libraries/AP_Logger/LogStructure.h

@@ -333,6 +333,7 @@ struct PACKED log_Optflow {
     float flow_y;
     float body_x;
     float body_y;
+    float distZ;
 };
 
 struct PACKED log_POWR {
@@ -825,6 +826,7 @@ struct PACKED log_VER {
 // @Field: flowY: Sensor flow rate,Y-axis
 // @Field: bodyX: derived rotational velocity, X-axis
 // @Field: bodyY: derived rotational velocity, Y-axis
+// @Field: distZ: altitude, Z-axis (mavlink only)
 
 // @LoggerMessage: PARM
 // @Description: parameter value
@@ -1234,7 +1236,7 @@ LOG_STRUCTURE_FROM_FENCE \
       "MAV", "QBHHHBHH",   "TimeUS,chan,txp,rxp,rxdp,flags,ss,tf", "s#----s-", "F-000-C-" },   \
 LOG_STRUCTURE_FROM_VISUALODOM \
     { LOG_OPTFLOW_MSG, sizeof(log_Optflow), \
-      "OF",   "QBffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEEE", "F-0000" , true }, \
+      "OF",   "QBfffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY,distZ", "s-EEEEE", "F-00000" , true }, \
     { LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \
       "WENC",  "Qfbfb", "TimeUS,Dist0,Qual0,Dist1,Qual1", "sm-m-", "F0-0-" , true }, \
     { LOG_ADSB_MSG, sizeof(log_ADSB), \

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -1216,7 +1216,7 @@ bool NavEKF2::use_compass(void) const
 // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
 void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
 {
-    AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
+    AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, 0.0f, msecFlowMeas, posOffset, heightOverride);
 
     if (core) {
         for (uint8_t i=0; i<num_cores; i++) {

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -120,6 +120,7 @@
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
 #define FLOW_USE_DEFAULT        1
+#define FLOW_HGT_SRC_DEFAULT    0
 #define WIND_P_NSE_DEFAULT      0.1
 
 #endif // APM_BUILD_DIRECTORY
@@ -744,6 +745,13 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
     // @User: Advanced
     AP_GROUPINFO("OPTIONS",  11, NavEKF3, _options, 0),
 
+    // @Param: FLOW_HGT_SRC
+    // @DisplayName: Optical flow height source
+    // @Description: Controls the source of height used by the optical flow sensor for fusion.
+    // @Values: 0:RangeFinder,1:OpticalFlow Mavlink
+    // @RebootRequired: True
+    AP_GROUPINFO("FLOW_HGT_SRC", 55, NavEKF3, _flowHgtSrc, FLOW_HGT_SRC_DEFAULT),
+
     AP_GROUPEND
 };
 
@@ -1578,13 +1586,13 @@ bool NavEKF3::configuredToUseGPSForPosXY(void) const
 // posOffset is the XYZ flow sensor position in the body frame in m
 // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
 #if EK3_FEATURE_OPTFLOW_FUSION
-void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
+void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
 {
-    dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
+    dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, rawGroundDistance, msecFlowMeas, posOffset, heightOverride);
 
     if (core) {
         for (uint8_t i=0; i<num_cores; i++) {
-            core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
+            core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, rawGroundDistance, msecFlowMeas, posOffset, heightOverride);
         }
     }
 }

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -195,7 +195,7 @@ class NavEKF3 {
     // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
     // posOffset is the XYZ flow sensor position in the body frame in m
     // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
-    void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
+    void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride);
 
     // retrieve latest corrected optical flow samples (used for calibration)
     bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
@@ -436,6 +436,7 @@ class NavEKF3 {
     AP_Float _visOdmVelErrMin;      // Observation 1-STD velocity error assumed for visual odometry sensor at highest reported quality (m/s)
     AP_Float _wencOdmVelErr;        // Observation 1-STD velocity error assumed for wheel odometry sensor (m/s)
     AP_Int8  _flowUse;              // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator.
+    AP_Int8  _flowHgtSrc;          // Controls the source of height used by the optical flow sensor
     AP_Float _hrt_filt_freq;        // frequency of output observer height rate complementary filter in Hz
     AP_Int16 _mag_ef_limit;         // limit on difference between WMM tables and learned earth field.
     AP_Int8 _gsfRunMask;            // mask controlling which EKF3 instances run a separate EKF-GSF yaw estimator
@@ -469,6 +470,10 @@ class NavEKF3 {
 #define FLOW_USE_NAV     1
 #define FLOW_USE_TERRAIN 2
 
+// Height source options for Optical Flow
+#define FLOW_HGT_SRC_DEFAULT 0
+#define FLOW_HGT_SRC_MAVLINK 1
+
     // Tuning parameters
     const float gpsNEVelVarAccScale = 0.05f;       // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
     const float gpsDVelVarAccScale = 0.07f;        // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -171,7 +171,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
 #if EK3_FEATURE_OPTFLOW_FUSION
 // write the raw optical flow measurements
 // this needs to be called externally.
-void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
+void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const float rawGroundDistance, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
 {
     // limit update rate to maximum allowed by sensor buffers
     if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) {
@@ -201,7 +201,6 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
     // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
     if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
         // correct flow sensor body rates for bias and write
-        of_elements ofDataNew {};
         ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
         ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
         // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
@@ -220,6 +219,8 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
         ofDataNew.flowRadXY = - rawFlowRates.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
         // write the flow sensor position in body frame
         ofDataNew.body_offset = posOffset.toftype();
+        // write the flow sensor's ground distance
+        ofDataNew.ground_distance = rawGroundDistance;
         // write the flow sensor height override
         ofDataNew.heightOverride = heightOverride;
         // write flow rate measurements corrected for body rates

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@@ -34,6 +34,16 @@ void NavEKF3_core::SelectFlowFusion()
 
     // Check for data at the fusion time horizon
     const bool flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
+    flowHgtToFuse = (frontend->_flowHgtSrc == FLOW_HGT_SRC_MAVLINK) && (ofDataDelayed.ground_distance > 0);
+
+    if (flowHgtToFuse) {
+        // If the optical flow sensor is providing ground distance, correct for the sensor's offset wrt tilt
+        Vector3F posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
+        if (!posOffsetBody.is_zero()) {
+            Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+            ofDataDelayed.ground_distance += posOffsetEarth.z / prevTnb.c.z;
+        }
+    }
 
     // Perform Data Checks
     // Check if the optical flow data is still valid
@@ -52,7 +62,7 @@ void NavEKF3_core::SelectFlowFusion()
     }
 
     // if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height
-    if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) {
+    if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse || flowHgtToFuse) && tiltOK) {
         // Estimate the terrain offset (runs a one state EKF)
         EstimateTerrainOffset(ofDataDelayed);
     }
@@ -86,7 +96,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
     || velHorizSq < 25.0f 
     || (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate));
 
-    if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
+    if ((!(rangeDataToFuse || flowHgtToFuse) && cantFuseFlowData) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
         // skip update
         inhibitGndState = true;
     } else {
@@ -105,6 +115,60 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
         Popt += Pincrement;
         timeAtLastAuxEKF_ms = imuSampleTime_ms;
 
+        // fuse ground distance data from optical flow
+        if (flowHgtToFuse) {
+            // reset terrain state if OF ground distance data not fused for 5 seconds
+            if (imuSampleTime_ms - gndHgtValidTime_ms > 5000) {
+                terrainState = MAX(ofDataDelayed.ground_distance * prevTnb.c.z, rngOnGnd) + stateStruct.position.z;
+            }
+
+            // predict range
+            ftype predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
+            // Copy required states to local variable names
+            ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
+            ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
+            ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
+            ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
+
+            // Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
+            // Use same noise variance for optical flow ground distance as for range finder
+            ftype R_RNG = frontend->_rngNoise.get();
+
+            // calculate Kalman gain
+            ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
+            ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
+
+            // Calculate the innovation variance for data logging
+            varInnovHgt = (R_RNG + Popt/sq(SK_RNG));
+
+            // constrain terrain height to be below the vehicle
+            terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
+
+            // Calculate the measurement innovation
+            innovHgt = predRngMeas - ofDataDelayed.ground_distance;
+
+            // calculate the innovation consistency test ratio
+            auxRngTestRatio = sq(innovHgt) / (sq(MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f)) * varInnovHgt);
+
+            // Check the innovation test ratio and don't fuse if too large
+            if (auxRngTestRatio < 1.0f) {
+                // correct the state
+                terrainState -= K_RNG * innovHgt;
+
+                // constrain the state
+                terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
+
+                // correct the covariance
+                Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
+
+                // prevent the state variance from becoming negative
+                Popt = MAX(Popt,0.0f);
+
+                // record the time we last updated the terrain offset state
+                gndHgtValidTime_ms = imuSampleTime_ms;
+            }
+        }
+
         // fuse range finder data
         if (rangeDataToFuse) {
             // reset terrain state if rangefinder data not fused for 5 seconds