aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: Prevent copter from switching to DCM unless EKF has severe errors

Browse Source
This commit is contained in:
Paul Riseborough 2015-05-13 15:37:17 +10:00 committed by Randy Mackay
parent 9dbef1c6ee
commit c2ac80cc63
3 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -118,8 +118,8 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
#if AP_AHRS_NAVEKF_AVAILABLE && !AHRS_EKF_USE_ALWAYS #if AP_AHRS_NAVEKF_AVAILABLE && !AHRS_EKF_USE_ALWAYS
// @Param: EKF_USE // @Param: EKF_USE
// @DisplayName: Use NavEKF Kalman filter for attitude and position estimation // @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
// @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation // @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation and whether fallback to the DCM algorithm is allowed
// @Values: 0:Disabled,1:Enabled // @Values: 0:Disabled,1:Enabled, 2:Enabled - No Fallback
// @User: Advanced // @User: Advanced
AP_GROUPINFO("EKF_USE", 13, AP_AHRS, _ekf_use, AHRS_EKF_USE_DEFAULT), AP_GROUPINFO("EKF_USE", 13, AP_AHRS, _ekf_use, AHRS_EKF_USE_DEFAULT),
#endif #endif

6
libraries/AP_AHRS/AP_AHRS.h
View File

@ -46,6 +46,10 @@
#define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter #define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter
#define AP_AHRS_YAW_P_MIN 0.05f // minimum value for AHRS_YAW_P parameter #define AP_AHRS_YAW_P_MIN 0.05f // minimum value for AHRS_YAW_P parameter
#define EKF_DO_NOT_USE 0 // Prevents the EKF from being used by the flight controllers
#define EKF_USE_WITH_FALLBACK 1 // Uses the EKF unless its solution is unhealthy or not initialised. This allows sensor errors to cause reversion.
#define EKF_USE_WITHOUT_FALLBACK 2 // Uses the EKF unless it encounters numerical processing errors or isn't iniitalised. Sensor errors will not cause reversion.
enum AHRS_VehicleClass { enum AHRS_VehicleClass {
AHRS_VEHICLE_UNKNOWN, AHRS_VEHICLE_UNKNOWN,
AHRS_VEHICLE_GROUND, AHRS_VEHICLE_GROUND,
@ -367,7 +371,7 @@ protected:
AP_Int8 _gps_delay; AP_Int8 _gps_delay;
#if AHRS_EKF_USE_ALWAYS #if AHRS_EKF_USE_ALWAYS
static const int8_t _ekf_use = 1; static const int8_t _ekf_use = EKF_USE_WITHOUT_FALLBACK;
#else #else
AP_Int8 _ekf_use; AP_Int8 _ekf_use;
#endif #endif

12
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -312,7 +312,12 @@ bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
bool AP_AHRS_NavEKF::using_EKF(void) const bool AP_AHRS_NavEKF::using_EKF(void) const
{ {
bool ret = ekf_started && _ekf_use && EKF.healthy(); uint8_t ekf_faults;
EKF.getFilterFaults(ekf_faults);
// If EKF is started we switch away if it reports unhealthy. This could be due to bad
// sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters
// an internal processing error, but not for bad sensor data.
bool ret = ekf_started && ((_ekf_use == EKF_USE_WITH_FALLBACK && EKF.healthy()) || (_ekf_use == EKF_USE_WITHOUT_FALLBACK && ekf_faults == 0));
if (!ret) { if (!ret) {
return false; return false;
} }
@ -338,7 +343,10 @@ bool AP_AHRS_NavEKF::using_EKF(void) const
*/ */
bool AP_AHRS_NavEKF::healthy(void) const bool AP_AHRS_NavEKF::healthy(void) const
{ {
if (_ekf_use) { // If EKF is started we switch away if it reports unhealthy. This could be due to bad
// sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters
// an internal processing error, but not for bad sensor data.
if (_ekf_use != EKF_DO_NOT_USE) {
return ekf_started && EKF.healthy(); return ekf_started && EKF.healthy();
} }
return AP_AHRS_DCM::healthy(); return AP_AHRS_DCM::healthy();