From 7ad8b909ab70e30ef3af3e6c108be3c58e94ec01 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 10 Feb 2023 18:46:40 +1100 Subject: [PATCH] AP_NavEKF3: add and use pr/EK3_FEATURE_OPTFLOW_FUSION --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 2 ++ libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 13 +++++++++++-- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 4 ++++ libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 ++ libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 5 +++++ libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 8 ++++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 12 ++++++++++++ libraries/AP_NavEKF3/AP_NavEKF3_feature.h | 6 ++++++ 9 files changed, 52 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index c198b20bc24ff9..f4941340b1fe4a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1526,6 +1526,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); @@ -1546,6 +1547,7 @@ bool NavEKF3::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vecto } return false; } +#endif // EK3_FEATURE_OPTFLOW_FUSION // write yaw angle sensor measurements void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index c78bc763cbfe69..31ce6f0ae20b0a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -268,7 +268,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; @@ -402,11 +406,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; @@ -500,6 +507,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 { @@ -515,6 +523,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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 100c47a253c87d..8b432283a6bf45 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index e409061698bf2c..ef83b7ec6203ac 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/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 /******************************************************** diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index e457bf7a03e524..efa3d7dbccc9b3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -1,7 +1,11 @@ #include #include "AP_NavEKF3.h" + #include "AP_NavEKF3_core.h" + +#if EK3_FEATURE_OPTFLOW_FUSION + #include /******************************************************** @@ -779,3 +783,4 @@ bool NavEKF3_core::getOptFlowSample(uint32_t& timestamp_ms, Vector2f& flowRate, * MISC FUNCTIONS * ********************************************************/ +#endif // EK3_FEATURE_OPTFLOW_FUSION diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 47e42773613fff..2190c006a4ad30 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 0fc7d0f7c21fbe..fbd7a651551b8c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/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; @@ -311,7 +313,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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 0bb9739b23dbeb..76649ca042e3ca 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -853,8 +853,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; @@ -865,8 +867,10 @@ 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(); @@ -874,9 +878,11 @@ class NavEKF3_core : public NavEKF_core_common // 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(); @@ -916,8 +922,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(); @@ -1157,7 +1165,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 @@ -1238,7 +1248,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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h index 620540b994e6ca..e9c34d2ad1d5f8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h @@ -7,6 +7,7 @@ #include #include #include +#include // 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