ardupilot/ArduCopter/ekf_check.cpp

280 lines
10 KiB
C++

#include "Copter.h"
/**
*
* 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 structure
////////////////////////////////////////////////////////////////////////////////
static struct {
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_check - detects if ekf variance are out of tolerance and triggers failsafe
// should be called at 10hz
void Copter::ekf_check()
{
// exit immediately if ekf has no origin yet - this assumes the origin can never become unset
Location temp_loc;
if (!ahrs.get_origin(temp_loc)) {
return;
}
// return immediately if motors are not armed, or ekf check is disabled
if (!motors->armed() || (g.fs_ekf_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 (ekf_over_threshold()) {
// if compass is not yet flagged as bad
if (!ekf_check_state.bad_variance) {
// increase counter
ekf_check_state.fail_count++;
#if EKF_CHECK_ITERATIONS_MAX > 2
if (ekf_check_state.fail_count == EKF_CHECK_ITERATIONS_MAX-1) {
// we are just about to declare a EKF failsafe, ask the EKF if we can change lanes
// to resolve the issue
ahrs.check_lane_switch();
}
#endif
// if counter above max then trigger failsafe
if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
// limit count from climbing too high
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_variance = true;
AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
// send message to gcs
if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
ekf_check_state.last_warn_time = AP_HAL::millis();
}
failsafe_ekf_event();
}
}
} else {
// reduce counter
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_variance && ekf_check_state.fail_count == 0) {
ekf_check_state.bad_variance = false;
AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED);
// clear failsafe
failsafe_ekf_off_event();
}
}
}
// set AP_Notify flags
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
// To-Do: add ekf variances to extended status
}
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
bool Copter::ekf_over_threshold()
{
// return false immediately if disabled
if (g.fs_ekf_thresh <= 0.0f) {
return false;
}
// use EKF to get variance
float position_variance, vel_variance, height_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset);
// return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold
uint8_t over_thresh_count = 0;
if (mag_variance.length() >= g.fs_ekf_thresh) {
over_thresh_count++;
}
bool optflow_healthy = false;
#if OPTFLOW == ENABLED
optflow_healthy = optflow.healthy();
#endif
if (!optflow_healthy && (vel_variance >= (2.0f * g.fs_ekf_thresh))) {
over_thresh_count += 2;
} else if (vel_variance >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (position_variance >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (over_thresh_count >= 2) {
return true;
}
// either optflow relative or absolute position estimate OK
if (optflow_position_ok() || ekf_position_ok()) {
return false;
}
return true;
}
// failsafe_ekf_event - perform ekf failsafe
void Copter::failsafe_ekf_event()
{
// return immediately if ekf failsafe already triggered
if (failsafe.ekf) {
return;
}
// EKF failsafe event has occurred
failsafe.ekf = true;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
// sometimes LAND *does* require GPS so ensure we are in non-GPS land
if (control_mode == Mode::Number::LAND && landing_with_GPS()) {
mode_land.do_not_use_GPS();
return;
}
// does this mode require position?
if (!copter.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
return;
}
// take action based on fs_ekf_action parameter
switch (g.fs_ekf_action) {
case FS_EKF_ACTION_ALTHOLD:
// AltHold
if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)) {
set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
}
break;
case FS_EKF_ACTION_LAND:
case FS_EKF_ACTION_LAND_EVEN_STABILIZE:
default:
set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
break;
}
}
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
void Copter::failsafe_ekf_off_event(void)
{
// return immediately if not in ekf failsafe
if (!failsafe.ekf) {
return;
}
failsafe.ekf = false;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
}
// check for ekf yaw reset and adjust target heading, also log position reset
void Copter::check_ekf_reset()
{
// check for yaw reset
float yaw_angle_change_rad;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) {
attitude_control->inertial_frame_reset();
ekfYawReset_ms = new_ekfYawReset_ms;
AP::logger().Write_Event(LogEvent::EKF_YAW_RESET);
}
#if AP_AHRS_NAVEKF_AVAILABLE && HAL_NAVEKF2_AVAILABLE
// check for change in primary EKF (log only, AC_WPNav handles position target adjustment)
if ((ahrs.EKF2.getPrimaryCoreIndex() != ekf_primary_core) && (ahrs.EKF2.getPrimaryCoreIndex() != -1)) {
attitude_control->inertial_frame_reset();
ekf_primary_core = ahrs.EKF2.getPrimaryCoreIndex();
AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core));
gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core);
}
#endif
}
// check for high vibrations affecting altitude control
void Copter::check_vibration()
{
uint32_t now = AP_HAL::millis();
// assume checks will succeed
bool checks_succeeded = true;
// check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
Vector3f vel_innovation;
Vector3f pos_innovation;
Vector3f mag_innovation;
float tas_innovation;
float yaw_innovation;
if (!ahrs.get_innovations(vel_innovation, pos_innovation, mag_innovation, tas_innovation, yaw_innovation)) {
checks_succeeded = false;
}
const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z);
// check if EKF's NKF4.SH and NK4.SV > 1.0
float position_variance, vel_variance, height_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset)) {
checks_succeeded = false;
}
// if no failure
if ((g2.fs_vibe_enabled == 0) || !checks_succeeded || !motors->armed() || !innov_velD_posD_positive || (vel_variance < 1.0f)) {
if (vibration_check.high_vibes) {
// start clear time
if (vibration_check.clear_ms == 0) {
vibration_check.clear_ms = now;
return;
}
// turn off vibration compensation after 15 seconds
if (now - vibration_check.clear_ms > 15000) {
// restore ekf gains, reset timers and update user
vibration_check.high_vibes = false;
pos_control->set_vibe_comp(false);
vibration_check.clear_ms = 0;
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
}
}
vibration_check.start_ms = 0;
return;
}
// start timer
if (vibration_check.start_ms == 0) {
vibration_check.start_ms = now;
vibration_check.clear_ms = 0;
return;
}
// check if failure has persisted for at least 1 second
if (now - vibration_check.start_ms > 1000) {
if (!vibration_check.high_vibes) {
// switch ekf to use resistant gains
vibration_check.high_vibes = true;
pos_control->set_vibe_comp(true);
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
}
}
}