AP_NavEKF3: make drag fusion optional

libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

@@ -239,10 +239,12 @@ void NavEKF3_core::SelectBetaDragFusion()
         prevBetaDragStep_ms = imuSampleTime_ms;
     }
 
+#if EK3_FEATURE_DRAG_FUSION
     // fusion of XY body frame aero specific forces is done at a slower rate and only if alternative methods of wind estimation are not available
     if (!inhibitWindStates && storedDrag.recall(dragSampleDelayed,imuDataDelayed.time_ms)) {
         FuseDragForces();
     }
+#endif
 }
 
 /*
@@ -449,6 +451,7 @@ void NavEKF3_core::FuseSideslip()
     ConstrainVariances();
 }
 
+#if EK3_FEATURE_DRAG_FUSION
 /*
  * Fuse X and Y body axis specific forces using explicit algebraic equations generated with SymPy.
  * See AP_NavEKF3/derivation/main.py for derivation
@@ -718,6 +721,7 @@ void NavEKF3_core::FuseDragForces()
         }
     }
 }
+#endif // EK3_FEATURE_DRAG_FUSION
 
 /********************************************************
 *                   MISC FUNCTIONS                      *

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -1377,6 +1377,7 @@ void NavEKF3_core::updateMovementCheck(void)
 
 void NavEKF3_core::SampleDragData(const imu_elements &imu)
 {
+#if EK3_FEATURE_DRAG_FUSION
     // Average and down sample to 5Hz
     const float bcoef_x = frontend->_ballisticCoef_x;
     const float bcoef_y = frontend->_ballisticCoef_y;
@@ -1419,4 +1420,5 @@ void NavEKF3_core::SampleDragData(const imu_elements &imu)
         dragDownSampled.time_ms = 0;
         dragSampleTimeDelta = 0.0f;
     }
+#endif // EK3_FEATURE_DRAG_FUSION
 }

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -440,9 +440,11 @@ void  NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vecto
 // return the synthetic air data drag and sideslip innovations
 void NavEKF3_core::getSynthAirDataInnovations(Vector2f &dragInnov, float &betaInnov) const
 {
+#if EK3_FEATURE_DRAG_FUSION
     dragInnov.x = innovDrag[0];
     dragInnov.y = innovDrag[1];
     betaInnov   = innovBeta;
+#endif
 }
 
 // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -153,9 +153,11 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
     if(!storedOutput.init(imu_buffer_length)) {
         return false;
     }
+#if EK3_FEATURE_DRAG_FUSION
     if (!storedDrag.init(obs_buffer_length)) {
         return false;
     }
+#endif
 
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u buffs IMU=%u OBS=%u OF=%u EN:%u dt=%.4f",
                     (unsigned)imu_index,

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1267,6 +1267,7 @@ private:
         Vector3f beaconPosNED; // beacon NED position
     } *rngBcnFusionReport;
 
+#if EK3_FEATURE_DRAG_FUSION
     // drag fusion for multicopter wind estimation
     EKF_obs_buffer_t<drag_elements> storedDrag;
     drag_elements dragSampleDelayed;
@@ -1276,6 +1277,7 @@ private:
     Vector2f innovDrag;		            // multirotor drag measurement innovation (m/sec**2)
 	Vector2f innovDragVar;	            // multirotor drag measurement innovation variance ((m/sec**2)**2)
 	Vector2f dragTestRatio;		        // drag innovation consistency check ratio
+#endif
     bool dragFusionEnabled;
 
     // height source selection logic

libraries/AP_NavEKF3/AP_NavEKF3_feature.h

@@ -19,3 +19,8 @@
 #define EK3_FEATURE_EXTERNAL_NAV EK3_FEATURE_ALL || BOARD_FLASH_SIZE > 1024
 #endif
 
+// drag fusion on 2M boards
+#ifndef EK3_FEATURE_DRAG_FUSION
+#define EK3_FEATURE_DRAG_FUSION EK3_FEATURE_ALL || BOARD_FLASH_SIZE > 1024
+#endif
+