AP_NavEKF3: add and use pr/EK3_FEATURE_OPTFLOW_FUSION

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1524,6 +1524,7 @@ bool NavEKF3::configuredToUseGPSForPosXY(void) const
 // 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
+#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)
 {
     AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
@@ -1544,6 +1545,7 @@ bool NavEKF3::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vecto
     }
     return false;
 }
+#endif
 
 // write yaw angle sensor measurements
 void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -267,7 +267,11 @@ void NavEKF3_core::setAidingMode()
             // GPS aiding is the preferred option unless excluded by the user
             if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
                 PV_AidingMode = AID_ABSOLUTE;
-            } else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
+            } else if (
+#if EK3_FEATURE_OPTFLOW_FUSION
+                readyToUseOptFlow() ||
+#endif
+                readyToUseBodyOdm()) {
                 PV_AidingMode = AID_RELATIVE;
             }
             break;
@@ -401,11 +405,14 @@ void NavEKF3_core::setAidingMode()
         case AID_RELATIVE:
             // We are doing relative position navigation where velocity errors are constrained, but position drift will occur
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
+#if EK3_FEATURE_OPTFLOW_FUSION
             if (readyToUseOptFlow()) {
                 // Reset time stamps
                 flowValidMeaTime_ms = imuSampleTime_ms;
                 prevFlowFuseTime_ms = imuSampleTime_ms;
-            } else if (readyToUseBodyOdm()) {
+            } else
+#endif
+                if (readyToUseBodyOdm()) {
                  // Reset time stamps
                 lastbodyVelPassTime_ms = imuSampleTime_ms;
                 prevBodyVelFuseTime_ms = imuSampleTime_ms;
@@ -499,6 +506,7 @@ bool NavEKF3_core::useRngFinder(void) const
     return true;
 }
 
+#if EK3_FEATURE_OPTFLOW_FUSION
 // return true if the filter is ready to start using optical flow measurements
 bool NavEKF3_core::readyToUseOptFlow(void) const
 {
@@ -514,6 +522,7 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
     // We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow
     return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
 }
+#endif  // EK3_FEATURE_OPTFLOW_FUSION
 
 // return true if the filter is ready to start using body frame odometry measurements
 bool NavEKF3_core::readyToUseBodyOdm(void) const

libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

@@ -195,7 +195,11 @@ void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
         offset : (int16_t)(100*terrainState),           // filter ground offset state error
         RI : (int16_t)(100*innovRng),                   // range finder innovations
         meaRng : (uint16_t)(100*rangeDataDelayed.rng),  // measured range
+#if EK3_FEATURE_OPTFLOW_FUSION
         errHAGL : (uint16_t)(100*sqrtF(Popt)),          // note Popt is constrained to be non-negative in EstimateTerrainOffset()
+#else
+        errHAGL : 0,          // note Popt is constrained to be non-negative in EstimateTerrainOffset()
+#endif
         angErr : (float)outputTrackError.x,             // output predictor angle error
         velErr : (float)outputTrackError.y,             // output predictor velocity error
         posErr : (float)outputTrackError.z              // output predictor position tracking error

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -165,6 +165,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
 }
 #endif // EK3_FEATURE_BODY_ODOM
 
+#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)
@@ -233,6 +234,7 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
         storedOF.push(ofDataNew);
     }
 }
+#endif  // EK3_FEATURE_OPTFLOW_FUSION
 
 
 /********************************************************

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@@ -1,7 +1,11 @@
 #include <AP_HAL/AP_HAL.h>
 
 #include "AP_NavEKF3.h"
+
 #include "AP_NavEKF3_core.h"
+
+#if EK3_FEATURE_OPTFLOW_FUSION
+
 #include <GCS_MAVLink/GCS.h>
 
 /********************************************************
@@ -779,3 +783,4 @@ bool NavEKF3_core::getOptFlowSample(uint32_t& timestamp_ms, Vector2f& flowRate,
 *                   MISC FUNCTIONS                      *
 ********************************************************/
 
+#endif  //  EK3_FEATURE_OPTFLOW_FUSION

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@@ -444,6 +444,7 @@ void NavEKF3_core::setTerrainHgtStable(bool val)
     terrainHgtStable = val;
 }
 
+#if EK3_FEATURE_OPTFLOW_FUSION
 // Detect takeoff for optical flow navigation
 void NavEKF3_core::detectOptFlowTakeoff(void)
 {
@@ -461,4 +462,4 @@ void NavEKF3_core::detectOptFlowTakeoff(void)
         takeOffDetected = false;
     }
 }
-
+#endif  // EK3_FEATURE_OPTFLOW_FUSION

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -247,7 +247,9 @@ void NavEKF3_core::InitialiseVariables()
     memset(&nextP[0][0], 0, sizeof(nextP));
     flowDataValid = false;
     rangeDataToFuse  = false;
+#if EK3_FEATURE_OPTFLOW_FUSION
     Popt = 0.0f;
+#endif
     terrainState = 0.0f;
     prevPosN = stateStruct.position.x;
     prevPosE = stateStruct.position.y;
@@ -310,7 +312,9 @@ void NavEKF3_core::InitialiseVariables()
     ZERO_FARRAY(statesArray);
     memset(&vertCompFiltState, 0, sizeof(vertCompFiltState));
     posVelFusionDelayed = false;
+#if EK3_FEATURE_OPTFLOW_FUSION
     optFlowFusionDelayed = false;
+#endif
     flowFusionActive = false;
     airSpdFusionDelayed = false;
     sideSlipFusionDelayed = false;
@@ -634,8 +638,10 @@ void NavEKF3_core::CovarianceInit()
     P[23][23]  = P[22][22];
 
 
+#if EK3_FEATURE_OPTFLOW_FUSION
     // optical flow ground height covariance
     Popt = 0.25f;
+#endif
 
 }
 
@@ -695,8 +701,10 @@ void NavEKF3_core::UpdateFilter(bool predict)
         SelectRngBcnFusion();
 #endif
 
+#if EK3_FEATURE_OPTFLOW_FUSION
         // Update states using optical flow data
         SelectFlowFusion();
+#endif
 
 #if EK3_FEATURE_BODY_ODOM
         // Update states using body frame odometry data

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -844,8 +844,10 @@ class NavEKF3_core : public NavEKF_core_common
     // Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
     void calcIMU_Weighting(ftype K1, ftype K2);
 
+#if EK3_FEATURE_OPTFLOW_FUSION
     // return true if the filter is ready to start using optical flow measurements for position and velocity estimation
     bool readyToUseOptFlow(void) const;
+#endif
 
     // return true if the filter is ready to start using body frame odometry measurements
     bool readyToUseBodyOdm(void) const;
@@ -856,18 +858,22 @@ class NavEKF3_core : public NavEKF_core_common
     // return true if we should use the range finder sensor
     bool useRngFinder(void) const;
 
+#if EK3_FEATURE_OPTFLOW_FUSION
     // determine when to perform fusion of optical flow measurements
     void SelectFlowFusion();
+#endif
 
     // determine when to perform fusion of body frame odometry measurements
     void SelectBodyOdomFusion();
 
     // Estimate terrain offset using a single state EKF
     void EstimateTerrainOffset(const of_elements &ofDataDelayed);
 
+#if EK3_FEATURE_OPTFLOW_FUSION
     // fuse optical flow measurements into the main filter
     // really_fuse should be true to actually fuse into the main filter, false to only calculate variances
     void FuseOptFlow(const of_elements &ofDataDelayed, bool really_fuse);
+#endif
 
     // Control filter mode changes
     void controlFilterModes();
@@ -907,8 +913,10 @@ class NavEKF3_core : public NavEKF_core_common
     // Apply a median filter to range finder data
     void readRangeFinder();
 
+#if EK3_FEATURE_OPTFLOW_FUSION
     // check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
     void detectOptFlowTakeoff(void);
+#endif
 
     // align the NE earth magnetic field states with the published declination
     void alignMagStateDeclination();
@@ -1147,7 +1155,9 @@ class NavEKF3_core : public NavEKF_core_common
     bool motorsArmed;               // true when the motors have been armed
     bool prevMotorsArmed;           // value of motorsArmed from previous frame
     bool posVelFusionDelayed;       // true when the position and velocity fusion has been delayed
+#if EK3_FEATURE_OPTFLOW_FUSION
     bool optFlowFusionDelayed;      // true when the optical flow fusion has been delayed
+#endif
     bool airSpdFusionDelayed;       // true when the air speed fusion has been delayed
     bool sideSlipFusionDelayed;     // true when the sideslip fusion has been delayed
     bool airDataFusionWindOnly;     // true when  sideslip and airspeed fusion is only allowed to modify the wind states
@@ -1228,7 +1238,9 @@ class NavEKF3_core : public NavEKF_core_common
     Vector2 flowVarInnov;           // optical flow innovations variances (rad/sec)^2
     Vector2 flowInnov;              // optical flow LOS innovations (rad/sec)
     uint32_t flowInnovTime_ms;      // system time that optical flow innovations and variances were recorded (to detect timeouts)
+#if EK3_FEATURE_OPTFLOW_FUSION
     ftype Popt;                     // Optical flow terrain height state covariance (m^2)
+#endif
     ftype terrainState;             // terrain position state (m)
     ftype prevPosN;                 // north position at last measurement
     ftype prevPosE;                 // east position at last measurement

libraries/AP_NavEKF3/AP_NavEKF3_feature.h

@@ -7,6 +7,7 @@
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <AP_Beacon/AP_Beacon_config.h>
+#include <AP_OpticalFlow/AP_OpticalFlow.h>
 
 // define for when to include all features
 #define EK3_FEATURE_ALL APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) || APM_BUILD_TYPE(APM_BUILD_Replay)
@@ -30,3 +31,8 @@
 #ifndef EK3_FEATURE_BEACON_FUSION
 #define EK3_FEATURE_BEACON_FUSION AP_BEACON_ENABLED
 #endif
+
+// Flow Fusion if Flow data available
+#ifndef EK3_FEATURE_OPTFLOW_FUSION
+#define EK3_FEATURE_OPTFLOW_FUSION AP_OPTICALFLOW_ENABLED
+#endif