2014-07-21 07:06:20 -03:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2014-07-21 07:06:20 -03:00
|
|
|
/**
|
|
|
|
*
|
|
|
|
* ekf_check.pde - detects failures of the ekf or inertial nav system
|
|
|
|
* triggers an alert to the pilot and helps take countermeasures
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
#ifndef EKF_CHECK_ITERATIONS_MAX
|
|
|
|
# define EKF_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#ifndef EKF_CHECK_WARNING_TIME
|
|
|
|
# define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds
|
|
|
|
#endif
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
// EKF_check strucutre
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
static struct {
|
2015-03-27 03:23:17 -03:00
|
|
|
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)
|
2014-07-21 07:06:20 -03:00
|
|
|
uint32_t last_warn_time; // system time of last warning in milliseconds. Used to throttle text warnings sent to GCS
|
|
|
|
} ekf_check_state;
|
|
|
|
|
2015-03-27 03:23:17 -03:00
|
|
|
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
|
2014-07-21 07:06:20 -03:00
|
|
|
// should be called at 10hz
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::ekf_check()
|
2014-07-21 07:06:20 -03:00
|
|
|
{
|
2015-07-16 02:52:17 -03:00
|
|
|
// exit immediately if ekf has no origin yet - this assumes the origin can never become unset
|
|
|
|
Location temp_loc;
|
2015-10-17 03:00:32 -03:00
|
|
|
if (!ahrs.get_origin(temp_loc)) {
|
2015-07-16 02:52:17 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-08-20 02:38:53 -03:00
|
|
|
// return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
|
2015-06-09 22:16:52 -03:00
|
|
|
if (!motors.armed() || ap.usb_connected || (g.fs_ekf_thresh <= 0.0f)) {
|
2015-03-27 03:23:17 -03:00
|
|
|
ekf_check_state.fail_count = 0;
|
|
|
|
ekf_check_state.bad_variance = false;
|
|
|
|
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
|
2014-07-21 07:06:20 -03:00
|
|
|
failsafe_ekf_off_event(); // clear failsafe
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-07-31 10:43:04 -03:00
|
|
|
// compare compass and velocity variance vs threshold
|
2015-03-27 03:23:17 -03:00
|
|
|
if (ekf_over_threshold()) {
|
2014-07-21 07:06:20 -03:00
|
|
|
// if compass is not yet flagged as bad
|
2015-03-27 03:23:17 -03:00
|
|
|
if (!ekf_check_state.bad_variance) {
|
2014-07-21 07:06:20 -03:00
|
|
|
// increase counter
|
2015-03-27 03:23:17 -03:00
|
|
|
ekf_check_state.fail_count++;
|
2014-07-21 07:06:20 -03:00
|
|
|
// if counter above max then trigger failsafe
|
2015-03-27 03:23:17 -03:00
|
|
|
if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
|
2014-07-21 07:06:20 -03:00
|
|
|
// limit count from climbing too high
|
2015-03-27 03:23:17 -03:00
|
|
|
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
|
|
|
|
ekf_check_state.bad_variance = true;
|
2014-07-21 07:06:20 -03:00
|
|
|
// log an error in the dataflash
|
2015-03-27 03:23:17 -03:00
|
|
|
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
|
2014-07-21 07:06:20 -03:00
|
|
|
// send message to gcs
|
|
|
|
if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
|
2015-08-11 20:50:34 -03:00
|
|
|
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("EKF variance"));
|
2014-07-21 07:06:20 -03:00
|
|
|
ekf_check_state.last_warn_time = hal.scheduler->millis();
|
|
|
|
}
|
|
|
|
failsafe_ekf_event();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
} else {
|
2014-10-27 00:37:56 -03:00
|
|
|
// reduce counter
|
2015-03-27 03:23:17 -03:00
|
|
|
if (ekf_check_state.fail_count > 0) {
|
|
|
|
ekf_check_state.fail_count--;
|
2014-10-27 00:37:56 -03:00
|
|
|
|
|
|
|
// if compass is flagged as bad and the counter reaches zero then clear flag
|
2015-03-27 03:23:17 -03:00
|
|
|
if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
|
|
|
|
ekf_check_state.bad_variance = false;
|
2014-07-21 07:06:20 -03:00
|
|
|
// log recovery in the dataflash
|
2015-03-27 03:23:17 -03:00
|
|
|
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_VARIANCE_CLEARED);
|
2014-07-21 07:06:20 -03:00
|
|
|
// clear failsafe
|
|
|
|
failsafe_ekf_off_event();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// set AP_Notify flags
|
2015-03-27 03:23:17 -03:00
|
|
|
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
|
2014-07-21 07:06:20 -03:00
|
|
|
|
|
|
|
// To-Do: add ekf variances to extended status
|
2014-10-16 04:18:37 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
|
2015-05-29 23:12:49 -03:00
|
|
|
bool Copter::ekf_over_threshold()
|
2014-10-16 04:18:37 -03:00
|
|
|
{
|
|
|
|
// return false immediately if disabled
|
2015-06-09 22:16:52 -03:00
|
|
|
if (g.fs_ekf_thresh <= 0.0f) {
|
2014-10-16 04:18:37 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-03-27 03:24:26 -03:00
|
|
|
// return true immediately if position is bad
|
2015-06-15 03:53:41 -03:00
|
|
|
if (!ekf_position_ok() && !optflow_position_ok()) {
|
2015-03-27 03:24:26 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2014-10-16 04:18:37 -03:00
|
|
|
// use EKF to get variance
|
|
|
|
float posVar, hgtVar, tasVar;
|
|
|
|
Vector3f magVar;
|
|
|
|
Vector2f offset;
|
|
|
|
float compass_variance;
|
|
|
|
float vel_variance;
|
2015-10-17 02:57:12 -03:00
|
|
|
ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
|
2014-10-16 04:18:37 -03:00
|
|
|
compass_variance = magVar.length();
|
|
|
|
|
|
|
|
// return true if compass and velocity variance over the threshold
|
2015-06-09 22:16:52 -03:00
|
|
|
return (compass_variance >= g.fs_ekf_thresh && vel_variance >= g.fs_ekf_thresh);
|
2014-07-21 07:06:20 -03:00
|
|
|
}
|
|
|
|
|
2014-10-16 04:18:37 -03:00
|
|
|
|
2014-07-21 07:06:20 -03:00
|
|
|
// failsafe_ekf_event - perform ekf failsafe
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::failsafe_ekf_event()
|
2014-07-21 07:06:20 -03:00
|
|
|
{
|
2014-10-16 04:18:37 -03:00
|
|
|
// return immediately if ekf failsafe already triggered
|
|
|
|
if (failsafe.ekf) {
|
2014-07-21 07:06:20 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-06-09 23:39:06 -03:00
|
|
|
// do nothing if motors disarmed
|
|
|
|
if (!motors.armed()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize
|
|
|
|
if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
|
2014-07-21 07:06:20 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// EKF failsafe event has occurred
|
|
|
|
failsafe.ekf = true;
|
2014-08-06 12:59:19 -03:00
|
|
|
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
|
2014-07-21 07:06:20 -03:00
|
|
|
|
2015-06-09 23:39:06 -03:00
|
|
|
// take action based on fs_ekf_action parameter
|
|
|
|
switch (g.fs_ekf_action) {
|
|
|
|
case FS_EKF_ACTION_ALTHOLD:
|
|
|
|
// AltHold
|
2015-10-17 22:05:37 -03:00
|
|
|
if (failsafe.radio || !set_mode(ALT_HOLD)) {
|
2015-06-09 23:39:06 -03:00
|
|
|
set_mode_land_with_pause();
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
set_mode_land_with_pause();
|
|
|
|
break;
|
2014-07-21 07:06:20 -03:00
|
|
|
}
|
|
|
|
|
2015-06-09 23:39:06 -03:00
|
|
|
// if flight mode is already LAND ensure it's not the GPS controlled LAND
|
2014-07-21 07:06:20 -03:00
|
|
|
if (control_mode == LAND) {
|
|
|
|
land_do_not_use_GPS();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::failsafe_ekf_off_event(void)
|
2014-07-21 07:06:20 -03:00
|
|
|
{
|
|
|
|
// return immediately if not in ekf failsafe
|
|
|
|
if (!failsafe.ekf) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// clear flag and log recovery
|
|
|
|
failsafe.ekf = false;
|
2014-08-06 12:59:19 -03:00
|
|
|
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED);
|
2014-07-21 07:06:20 -03:00
|
|
|
}
|