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

Copter: remove inav check 

the EKF check works reliably but attempts to check the inertial nav for
errors has not been successful.  I could not find a way to reliably
catch flyaways without also introducing false positives (and thus
unwanted LANDings)
This commit is contained in:
Randy Mackay 2014-08-20 14:38:53 +09:00
parent 43e8b36e5b
commit c87283af98
1 changed files with 19 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
ArduCopter/ekf_check.pde
View File

@ -34,8 +34,9 @@ static struct {
// should be called at 10hz // should be called at 10hz
void ekf_check() void ekf_check()
{ {
// return immediately if motors are not armed, ekf check is disabled, no inertial-nav position yet or usb is connected #if AP_AHRS_NAVEKF_AVAILABLE
if (!motors.armed() || g.ekfcheck_thresh == 0.0f || !inertial_nav.position_ok() || ap.usb_connected) { // return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
if (!motors.armed() || g.ekfcheck_thresh == 0.0f || !ahrs.have_inertial_nav() || ap.usb_connected) {
ekf_check_state.fail_count_compass = 0; ekf_check_state.fail_count_compass = 0;
ekf_check_state.bad_compass = 0; ekf_check_state.bad_compass = 0;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
@ -43,26 +44,14 @@ void ekf_check()
return; return;
} }
// variances // use EKF to get variance
float compass_variance = 0; float posVar, hgtVar, tasVar;
float vel_variance = 9.0; // default set high to enable failsafe trigger if not using EKF Vector3f magVar;
Vector2f offset;
#if AP_AHRS_NAVEKF_AVAILABLE float compass_variance;
if (ahrs.have_inertial_nav()) { float vel_variance;
// use EKF to get variance ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
float posVar, hgtVar, tasVar; compass_variance = magVar.length();
Vector3f magVar;
Vector2f offset;
ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
compass_variance = magVar.length();
} else {
// use complementary filter's acceleration corrections multiplied by conversion factor to make them general in the same range as the EKF's variances
compass_variance = safe_sqrt(inertial_nav.accel_correction_hbf.x * inertial_nav.accel_correction_hbf.x + inertial_nav.accel_correction_hbf.y * inertial_nav.accel_correction_hbf.y) * EKF_CHECK_COMPASS_INAV_CONVERSION;
}
#else
// use complementary filter's acceleration corrections multiplied by conversion factor to make them general in the same range as the EKF's variances
compass_variance = safe_sqrt(inertial_nav.accel_correction_hbf.x * inertial_nav.accel_correction_hbf.x + inertial_nav.accel_correction_hbf.y * inertial_nav.accel_correction_hbf.y) * EKF_CHECK_COMPASS_INAV_CONVERSION;
#endif
// compare compass and velocity variance vs threshold // compare compass and velocity variance vs threshold
if (compass_variance >= g.ekfcheck_thresh && vel_variance > g.ekfcheck_thresh) { if (compass_variance >= g.ekfcheck_thresh && vel_variance > g.ekfcheck_thresh) {
@ -79,15 +68,7 @@ void ekf_check()
Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE); Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE);
// send message to gcs // send message to gcs
if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
#if AP_AHRS_NAVEKF_AVAILABLE gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance"));
if (ahrs.have_inertial_nav()) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance"));
} else {
gcs_send_text_P(SEVERITY_HIGH,PSTR("INAV variance"));
}
#else
gcs_send_text_P(SEVERITY_HIGH,PSTR("INAV variance"));
#endif
ekf_check_state.last_warn_time = hal.scheduler->millis(); ekf_check_state.last_warn_time = hal.scheduler->millis();
} }
failsafe_ekf_event(); failsafe_ekf_event();
@ -112,13 +93,15 @@ void ekf_check()
// set AP_Notify flags // set AP_Notify flags
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
// To-Do: add check for althold when vibrations are high
// To-Do: add ekf variances to extended status // To-Do: add ekf variances to extended status
// To-Do: add counter measures to try and recover from bad EKF #else
// To-Do: add check into GPS position_ok() to return false if ekf xy not healthy? // if no EKF available then never trigger failure
// To-Do: ensure it compiles for AVR ekf_check_state.bad_compass = 0;
failsafe.ekf = false;
#endif
} }
#if AP_AHRS_NAVEKF_AVAILABLE
// failsafe_ekf_event - perform ekf failsafe // failsafe_ekf_event - perform ekf failsafe
static void failsafe_ekf_event() static void failsafe_ekf_event()
{ {
@ -159,3 +142,4 @@ static void failsafe_ekf_off_event(void)
failsafe.ekf = false; failsafe.ekf = false;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED); Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED);
} }
#endif