diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 704c06c0fd..604e08d22f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/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 * diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index f91e6cb0fd..4e37ed94bf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/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 } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index e0441e883b..4d6c7272aa 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index ca7e8eefe2..eaaa5175ce 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/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, diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 9e1d64e52a..efe1a9429b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/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 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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h index 0952c23eff..809511eba2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h +++ b/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 +