Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -239,10 +239,12 @@ void NavEKF3_core::SelectBetaDragFusion()
prevBetaDragStep_ms = imuSampleTime_ms; 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 // 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)) { if (!inhibitWindStates && storedDrag.recall(dragSampleDelayed,imuDataDelayed.time_ms)) {
FuseDragForces(); FuseDragForces();
} }
#endif
} }
/* /*
@ -449,6 +451,7 @@ void NavEKF3_core::FuseSideslip()
ConstrainVariances(); ConstrainVariances();
} }
#if EK3_FEATURE_DRAG_FUSION
/* /*
* Fuse X and Y body axis specific forces using explicit algebraic equations generated with SymPy. * Fuse X and Y body axis specific forces using explicit algebraic equations generated with SymPy.
* See AP_NavEKF3/derivation/main.py for derivation * See AP_NavEKF3/derivation/main.py for derivation
@ -718,6 +721,7 @@ void NavEKF3_core::FuseDragForces()
} }
} }
} }
#endif // EK3_FEATURE_DRAG_FUSION
/******************************************************** /********************************************************
* MISC FUNCTIONS * * MISC FUNCTIONS *

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

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

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

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

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
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)) { if(!storedOutput.init(imu_buffer_length)) {
return false; return false;
} }
#if EK3_FEATURE_DRAG_FUSION
if (!storedDrag.init(obs_buffer_length)) { if (!storedDrag.init(obs_buffer_length)) {
return false; return false;
} }
#endif
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u buffs IMU=%u OBS=%u OF=%u EN:%u dt=%.4f", GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u buffs IMU=%u OBS=%u OF=%u EN:%u dt=%.4f",
(unsigned)imu_index, (unsigned)imu_index,

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

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

5
libraries/AP_NavEKF3/AP_NavEKF3_feature.h
View File

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