diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 4aea462831..9bd1a2e48b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -459,6 +459,7 @@ bool NavEKF3_core::readyToUseOptFlow(void) const // return true if the filter is ready to start using body frame odometry measurements bool NavEKF3_core::readyToUseBodyOdm(void) const { +#if EK3_FEATURE_BODY_ODOM if (!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV) && !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) { // exit immediately if sources not configured to fuse external nav or wheel encoders @@ -476,6 +477,9 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const return (visoDataGood || wencDataGood) && tiltAlignComplete && delAngBiasLearned; +#else + return false; +#endif // EK3_FEATURE_BODY_ODOM } // return true if the filter to be ready to use gps diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 9c0cddd35d..8e63dff10b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -379,8 +379,10 @@ void NavEKF3_core::Log_Write(uint64_t time_us) // write range beacon fusion debug packet if the range value is non-zero Log_Write_Beacon(time_us); +#if EK3_FEATURE_BODY_ODOM // write debug data for body frame odometry fusion Log_Write_BodyOdom(time_us); +#endif // log state variances every 0.49s Log_Write_State_Variances(time_us); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index d259b31ab3..834e5a2d1e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -113,6 +113,7 @@ void NavEKF3_core::readRangeFinder(void) void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset) { +#if EK3_FEATURE_BODY_ODOM // protect against NaN if (isnan(quality) || delPos.is_nan() || delAng.is_nan() || isnan(delTime) || posOffset.is_nan()) { return; @@ -138,9 +139,10 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con bodyOdmDataNew.velErr = frontend->_visOdmVelErrMin + (frontend->_visOdmVelErrMax - frontend->_visOdmVelErrMin) * (1.0f - 0.01f * quality); storedBodyOdm.push(bodyOdmDataNew); - +#endif // EK3_FEATURE_BODY_ODOM } +#if EK3_FEATURE_BODY_ODOM void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius) { // This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units @@ -166,6 +168,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam storedWheelOdm.push(wheelOdmDataNew); } +#endif // EK3_FEATURE_BODY_ODOM // write the raw optical flow measurements // this needs to be called externally. diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index d06d5693e9..900351d2e2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -59,6 +59,7 @@ float NavEKF3_core::errorScore() const return score; } +#if EK3_FEATURE_BODY_ODOM // return data for debugging body frame odometry fusion uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar) { @@ -70,6 +71,7 @@ uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velIn velInnovVar.z = varInnovBodyVel[2]; return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms); } +#endif // EK3_FEATURE_BODY_ODOM // provides the height limit to be observed by the control loops // returns false if no height limiting is required diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index f94529a9ef..78d7154389 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1146,6 +1146,7 @@ void NavEKF3_core::selectHeightForFusion() } } +#if EK3_FEATURE_BODY_ODOM /* * Fuse body frame velocity measurements using explicit algebraic equations generated with Matlab symbolic toolbox. * The script file used to generate these and other equations in this filter can be found here: @@ -1781,7 +1782,9 @@ void NavEKF3_core::FuseBodyVel() } } } +#endif // EK3_FEATURE_BODY_ODOM +#if EK3_FEATURE_BODY_ODOM // select fusion of body odometry measurements void NavEKF3_core::SelectBodyOdomFusion() { @@ -1834,4 +1837,4 @@ void NavEKF3_core::SelectBodyOdomFusion() } } } - +#endif // EK3_FEATURE_BODY_ODOM diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index ae26463410..993fa80f39 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -116,6 +116,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) if(dal.opticalflow_enabled() && !storedOF.init(flow_buffer_length)) { return false; } +#if EK3_FEATURE_BODY_ODOM if(frontend->sources.ext_nav_enabled() && !storedBodyOdm.init(obs_buffer_length)) { return false; } @@ -123,6 +124,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) // initialise to same length of IMU to allow for multiple wheel sensors return false; } +#endif // EK3_FEATURE_BODY_ODOM if(frontend->sources.gps_yaw_enabled() && !storedYawAng.init(obs_buffer_length)) { return false; } @@ -372,9 +374,11 @@ void NavEKF3_core::InitialiseVariables() bcnPosOffsetNED.zero(); bcnOriginEstInit = false; +#if EK3_FEATURE_BODY_ODOM // body frame displacement fusion memset((void *)&bodyOdmDataNew, 0, sizeof(bodyOdmDataNew)); memset((void *)&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed)); +#endif lastbodyVelPassTime_ms = 0; memset(&bodyVelTestRatio, 0, sizeof(bodyVelTestRatio)); memset(&varInnovBodyVel, 0, sizeof(varInnovBodyVel)); @@ -408,8 +412,10 @@ void NavEKF3_core::InitialiseVariables() storedRange.reset(); storedOutput.reset(); storedRangeBeacon.reset(); +#if EK3_FEATURE_BODY_ODOM storedBodyOdm.reset(); storedWheelOdm.reset(); +#endif storedExtNav.reset(); storedExtNavVel.reset(); @@ -672,8 +678,10 @@ void NavEKF3_core::UpdateFilter(bool predict) // Update states using optical flow data SelectFlowFusion(); +#if EK3_FEATURE_BODY_ODOM // Update states using body frame odometry data SelectBodyOdomFusion(); +#endif // Update states using airspeed data SelectTasFusion(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 6aa5a15ee1..271236dced 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -35,6 +35,7 @@ #include #include "AP_NavEKF/EKFGSF_yaw.h" +#include "AP_NavEKF3_feature.h" // GPS pre-flight check bit locations #define MASK_GPS_NSATS (1<<0) @@ -1189,9 +1190,11 @@ private: bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference // body frame odometry fusion +#if EK3_FEATURE_BODY_ODOM EKF_obs_buffer_t storedBodyOdm; // body velocity data buffer vel_odm_elements bodyOdmDataNew; // Body frame odometry data at the current time horizon vel_odm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon +#endif uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innovation consistency checks (msec) Vector3 bodyVelTestRatio; // Innovation test ratios for body velocity XYZ measurements Vector3 varInnovBodyVel; // Body velocity XYZ innovation variances (m/sec)^2 @@ -1201,9 +1204,11 @@ private: bool bodyVelFusionDelayed; // true when body frame velocity fusion has been delayed bool bodyVelFusionActive; // true when body frame velocity fusion is active +#if EK3_FEATURE_BODY_ODOM // wheel sensor fusion EKF_obs_buffer_t storedWheelOdm; // body velocity data buffer wheel_odm_elements wheelOdmDataDelayed; // Body frame odometry data at the fusion time horizon +#endif // GPS yaw sensor fusion uint32_t yawMeasTime_ms; // system time GPS yaw angle was last input to the data buffer diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h new file mode 100644 index 0000000000..591d8d8073 --- /dev/null +++ b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h @@ -0,0 +1,13 @@ +/* + feature selection for EKF3 + */ + +#pragma once + +#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) + +// body odomotry (which includes wheel encoding) on rover or 2M boards +#define EK3_FEATURE_BODY_ODOM EK3_FEATURE_ALL || APM_BUILD_TYPE(APM_BUILD_Rover) || BOARD_FLASH_SIZE > 1024