mirror of https://github.com/ArduPilot/ardupilot
Copter: add DCM check of yaw error
Triggers an "ekf" failsafe if the DCM yaw error is > 60deg
This commit is contained in:
parent
470fcc2077
commit
7e1c975c54
|
@ -807,7 +807,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
|||
#endif
|
||||
{ update_notify, 8, 10 },
|
||||
{ one_hz_loop, 400, 42 },
|
||||
{ ekf_check, 40, 2 },
|
||||
{ ekf_dcm_check, 40, 2 },
|
||||
{ crash_check, 40, 2 },
|
||||
{ gcs_check_input, 8, 550 },
|
||||
{ gcs_send_heartbeat, 400, 150 },
|
||||
|
@ -875,7 +875,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
|||
#endif
|
||||
{ update_notify, 2, 100 },
|
||||
{ one_hz_loop, 100, 420 },
|
||||
{ ekf_check, 10, 20 },
|
||||
{ ekf_dcm_check, 10, 20 },
|
||||
{ crash_check, 10, 20 },
|
||||
{ gcs_check_input, 2, 550 },
|
||||
{ gcs_send_heartbeat, 100, 150 },
|
||||
|
|
|
@ -15,42 +15,48 @@
|
|||
# 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's compass variance has been out of tolerances
|
||||
uint8_t fail_count_compass; // number of iterations ekf or dcm have been out of tolerances
|
||||
|
||||
uint8_t bad_compass : 1; // true if compass variance is bad
|
||||
uint8_t bad_compass : 1; // true if dcm or ekf should be considered untrusted (fail_count_compass 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_check - detects ekf variances that are out of tolerance
|
||||
// ekf_dcm_check - detects if ekf variances or dcm yaw errors that are out of tolerance and triggers failsafe
|
||||
// should be called at 10hz
|
||||
void ekf_check()
|
||||
void ekf_dcm_check()
|
||||
{
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
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() || g.ekfcheck_thresh == 0.0f || !ahrs.have_inertial_nav() || ap.usb_connected) {
|
||||
if (!motors.armed() || ap.usb_connected || check_type == CHECK_NONE) {
|
||||
ekf_check_state.fail_count_compass = 0;
|
||||
ekf_check_state.bad_compass = 0;
|
||||
ekf_check_state.bad_compass = false;
|
||||
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
|
||||
failsafe_ekf_off_event(); // clear failsafe
|
||||
return;
|
||||
}
|
||||
|
||||
// use EKF to get variance
|
||||
float posVar, hgtVar, tasVar;
|
||||
Vector3f magVar;
|
||||
Vector2f offset;
|
||||
float compass_variance;
|
||||
float vel_variance;
|
||||
ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
|
||||
compass_variance = magVar.length();
|
||||
|
||||
// compare compass and velocity variance vs threshold
|
||||
if (compass_variance >= g.ekfcheck_thresh && vel_variance > g.ekfcheck_thresh) {
|
||||
if ((check_type == CHECK_EKF && ekf_over_threshold()) || (check_type == CHECK_DCM && dcm_over_threshold())) {
|
||||
// if compass is not yet flagged as bad
|
||||
if (!ekf_check_state.bad_compass) {
|
||||
// increase counter
|
||||
|
@ -64,7 +70,11 @@ void ekf_check()
|
|||
Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE);
|
||||
// send message to gcs
|
||||
if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance"));
|
||||
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();
|
||||
|
@ -90,19 +100,46 @@ void ekf_check()
|
|||
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
|
||||
|
||||
// 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()
|
||||
{
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
// return false immediately if disabled
|
||||
if (g.ekfcheck_thresh <= 0.0f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// use EKF to get variance
|
||||
float posVar, hgtVar, tasVar;
|
||||
Vector3f magVar;
|
||||
Vector2f offset;
|
||||
float compass_variance;
|
||||
float vel_variance;
|
||||
ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
|
||||
compass_variance = magVar.length();
|
||||
|
||||
// return true if compass and velocity variance over the threshold
|
||||
return (compass_variance >= g.ekfcheck_thresh && vel_variance >= g.ekfcheck_thresh);
|
||||
#else
|
||||
// if no EKF available then never trigger failure
|
||||
ekf_check_state.bad_compass = 0;
|
||||
failsafe.ekf = false;
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
|
||||
// failsafe_ekf_event - perform ekf failsafe
|
||||
static void failsafe_ekf_event()
|
||||
{
|
||||
// return immediately if ekf failsafe already triggered or disabled
|
||||
if (failsafe.ekf || g.ekfcheck_thresh <= 0.0f) {
|
||||
// return immediately if ekf failsafe already triggered
|
||||
if (failsafe.ekf) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -138,4 +175,3 @@ static void failsafe_ekf_off_event(void)
|
|||
failsafe.ekf = false;
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED);
|
||||
}
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue