From 02c6784c11bb9d9292f794f260e0bfb9b670bbb5 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 0a6dcb01f1..97eb479a73 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1551,6 +1551,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); @@ -1571,6 +1572,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 0ba289cd9b..6125d26d93 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -289,7 +289,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; @@ -432,11 +436,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; @@ -530,6 +537,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 { @@ -545,6 +553,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 8a351d75b1..03e23b7ab8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -202,7 +202,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 f635c185af..070e1f6460 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -167,6 +167,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) @@ -235,6 +236,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 84dd164bac..c634ce1207 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 bb43be9883..3e5651b43b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -460,6 +460,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) { @@ -477,4 +478,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 e4934a3d96..ae850c23b1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -249,7 +249,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; @@ -318,7 +320,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; @@ -609,8 +613,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 } @@ -667,8 +673,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 29b7310119..a6ee193702 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -868,8 +868,10 @@ private: // 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; @@ -880,8 +882,10 @@ private: // 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(); @@ -889,9 +893,11 @@ private: // 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(); @@ -934,8 +940,10 @@ private: // 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(); @@ -1182,7 +1190,9 @@ private: 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 @@ -1262,7 +1272,9 @@ private: 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 60500497d7..18a3c4ae61 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h @@ -8,6 +8,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) @@ -40,3 +41,8 @@ #ifndef EK3_FEATURE_RANGEFINDER_MEASUREMENTS #define EK3_FEATURE_RANGEFINDER_MEASUREMENTS AP_RANGEFINDER_ENABLED #endif + +// Flow Fusion if Flow data available +#ifndef EK3_FEATURE_OPTFLOW_FUSION +#define EK3_FEATURE_OPTFLOW_FUSION HAL_NAVEKF3_AVAILABLE && AP_OPTICALFLOW_ENABLED +#endif