@ -15,66 +15,44 @@
@@ -15,66 +15,44 @@
# define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds
#endif
// Enumerator for types of check
enum EKFCheckType {
CHECK_NONE = 0,
CHECK_DCM = 1,
CHECK_EKF = 2
};
////////////////////////////////////////////////////////////////////////////////
// EKF_check strucutre
////////////////////////////////////////////////////////////////////////////////
static struct {
uint8_t fail_count_compass; // number of iterations ekf or dcm have been out of tolerances
uint8_t bad_compass : 1; // true if dcm or ekf should be considered untrusted (fail_count_compass has exceeded EKF_CHECK_ITERATIONS_MAX)
uint8_t fail_count; // number of iterations ekf or dcm have been out of tolerances
uint8_t bad_variance : 1; // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX)
uint32_t last_warn_time; // system time of last warning in milliseconds. Used to throttle text warnings sent to GCS
} ekf_check_state;
// ekf_dcm_ check - detects if ekf variances or dcm yaw errors that are out of tolerance and triggers failsafe
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
// should be called at 10hz
void ekf_dcm_ check()
void ekf_check()
{
EKFCheckType check_type = CHECK_NONE;
// decide if we should check ekf or dcm
if (ahrs.have_inertial_nav() && g.ekfcheck_thresh > 0.0f) {
check_type = CHECK_EKF;
} else if (g.dcmcheck_thresh > 0.0f) {
check_type = CHECK_DCM;
}
// return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
if (!motors.armed() || ap.usb_connected || check_type == CHECK_NONE ) {
ekf_check_state.fail_count_compass = 0;
ekf_check_state.bad_compass = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass ;
if (!motors.armed() || ap.usb_connected || (g.ekfcheck_thresh <= 0.0f)) {
ekf_check_state.fail_count = 0;
ekf_check_state.bad_variance = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
failsafe_ekf_off_event(); // clear failsafe
return;
}
// compare compass and velocity variance vs threshold
if ((check_type == CHECK_EKF && ekf_over_threshold()) || (check_type == CHECK_DCM && dcm_over_threshold() )) {
if (ekf_over_threshold()) {
// if compass is not yet flagged as bad
if (!ekf_check_state.bad_compass ) {
if (!ekf_check_state.bad_variance ) {
// increase counter
ekf_check_state.fail_count_compass ++;
ekf_check_state.fail_count++;
// if counter above max then trigger failsafe
if (ekf_check_state.fail_count_compass >= EKF_CHECK_ITERATIONS_MAX) {
if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
// limit count from climbing too high
ekf_check_state.fail_count_compass = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_compass = true;
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_variance = true;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_ CHECK, ERROR_CODE_EKFINAV_ CHECK_BAD_VARIANCE);
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
// send message to gcs
if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
if (check_type == CHECK_EKF) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance"));
} else {
gcs_send_text_P(SEVERITY_HIGH,PSTR("DCM bad heading"));
}
ekf_check_state.last_warn_time = hal.scheduler->millis();
}
failsafe_ekf_event();
@ -82,14 +60,14 @@ void ekf_dcm_check()
@@ -82,14 +60,14 @@ void ekf_dcm_check()
}
} else {
// reduce counter
if (ekf_check_state.fail_count_compass > 0) {
ekf_check_state.fail_count_compass --;
if (ekf_check_state.fail_count > 0) {
ekf_check_state.fail_count--;
// if compass is flagged as bad and the counter reaches zero then clear flag
if (ekf_check_state.bad_compass && ekf_check_state.fail_count_compass == 0) {
ekf_check_state.bad_compass = false;
if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
ekf_check_state.bad_variance = false;
// log recovery in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_ CHECK, ERROR_CODE_EKFINAV_ CHECK_VARIANCE_CLEARED);
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_VARIANCE_CLEARED);
// clear failsafe
failsafe_ekf_off_event();
}
@ -97,18 +75,11 @@ void ekf_dcm_check()
@@ -97,18 +75,11 @@ void ekf_dcm_check()
}
// set AP_Notify flags
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass ;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance ;
// To-Do: add ekf variances to extended status
}
// dcm_over_threshold - returns true if the dcm yaw error is over the tolerance
static bool dcm_over_threshold()
{
// return true if yaw error is over the threshold
return (g.dcmcheck_thresh > 0.0f && ahrs.get_error_yaw() > g.dcmcheck_thresh);
}
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
static bool ekf_over_threshold()
{
Randy Mackay
10 years ago