AP_NavEKF3: make body odomotry build depend on vehicle type

saves about 11k of flash

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

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);

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.

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

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

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();

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -35,6 +35,7 @@
 #include <AP_DAL/AP_DAL.h>
 
 #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<vel_odm_elements> 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<wheel_odm_elements> 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

libraries/AP_NavEKF3/AP_NavEKF3_feature.h

@@ -0,0 +1,13 @@
+/*
+  feature selection for EKF3
+ */
+
+#pragma once
+
+#include <AP_Vehicle/AP_Vehicle_Type.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)
+
+// 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