AP_NavEKF3: make drag fusion optional

This commit is contained in:
Andrew Tridgell 2021-01-19 15:36:39 +11:00
parent 1ccda938cb
commit ddc4d08e8f
6 changed files with 17 additions and 0 deletions

View File

@ -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 *

View File

@ -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
}

View File

@ -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

View File

@ -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,

View File

@ -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

View File

@ -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