mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 14:48:28 -04:00
ArduPlane: Improve EKF failsafe in VTOL modes
Replicates Copter behaviour with a three step process if the EKF sustains a loss of navigation as detected by high GPS innovation test ratios: 1) Attempts a yaw reset using the GSF estimate if available 2) Attempts a lane switch 3) Falls back to a non-position mode
This commit is contained in:
parent
318811210f
commit
40cc5a5006
@ -51,6 +51,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
|
|||||||
#if ADVANCED_FAILSAFE == ENABLED
|
#if ADVANCED_FAILSAFE == ENABLED
|
||||||
SCHED_TASK(afs_fs_check, 10, 100),
|
SCHED_TASK(afs_fs_check, 10, 100),
|
||||||
#endif
|
#endif
|
||||||
|
SCHED_TASK(ekf_check, 10, 75),
|
||||||
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500),
|
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500),
|
||||||
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 500),
|
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 500),
|
||||||
SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150),
|
SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150),
|
||||||
|
@ -1271,6 +1271,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0),
|
AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0),
|
||||||
|
|
||||||
|
// @Param: FS_EKF_THRESH
|
||||||
|
// @DisplayName: EKF failsafe variance threshold
|
||||||
|
// @Description: Allows setting the maximum acceptable compass and velocity variance used to check navigation health in VTOL modes
|
||||||
|
// @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed
|
||||||
|
// @User: Advanced
|
||||||
|
AP_GROUPINFO("FS_EKF_THRESH", 26, ParametersG2, fs_ekf_thresh, FS_EKF_THRESHOLD_DEFAULT),
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -575,6 +575,8 @@ public:
|
|||||||
// EFI Engine Monitor
|
// EFI Engine Monitor
|
||||||
AP_EFI efi;
|
AP_EFI efi;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
AP_Float fs_ekf_thresh;
|
||||||
};
|
};
|
||||||
|
|
||||||
extern const AP_Param::Info var_info[];
|
extern const AP_Param::Info var_info[];
|
||||||
|
@ -1036,6 +1036,14 @@ private:
|
|||||||
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
|
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
|
||||||
"_failsafe_priorities is missing the sentinel");
|
"_failsafe_priorities is missing the sentinel");
|
||||||
|
|
||||||
|
// EKF checks for loss of navigation performed in ekf_check.cpp
|
||||||
|
// These are specific to VTOL operation
|
||||||
|
void ekf_check();
|
||||||
|
bool ekf_over_threshold();
|
||||||
|
void failsafe_ekf_event();
|
||||||
|
void failsafe_ekf_off_event(void);
|
||||||
|
bool ekf_position_ok() const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
void failsafe_check(void);
|
void failsafe_check(void);
|
||||||
bool set_target_location(const Location& target_loc) override;
|
bool set_target_location(const Location& target_loc) override;
|
||||||
|
@ -308,3 +308,9 @@
|
|||||||
#ifndef LANDING_GEAR_ENABLED
|
#ifndef LANDING_GEAR_ENABLED
|
||||||
#define LANDING_GEAR_ENABLED !HAL_MINIMIZE_FEATURES
|
#define LANDING_GEAR_ENABLED !HAL_MINIMIZE_FEATURES
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
// EKF Failsafe
|
||||||
|
#ifndef FS_EKF_THRESHOLD_DEFAULT
|
||||||
|
# define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
|
||||||
|
#endif
|
||||||
|
203
ArduPlane/ekf_check.cpp
Normal file
203
ArduPlane/ekf_check.cpp
Normal file
@ -0,0 +1,203 @@
|
|||||||
|
#include "Plane.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
|
||||||
|
bool failsafe_on; // true when the loss of navigation failsafe is on
|
||||||
|
} ekf_check_state;
|
||||||
|
|
||||||
|
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
|
||||||
|
// should be called at 10hz
|
||||||
|
void Plane::ekf_check()
|
||||||
|
{
|
||||||
|
// ensure EKF_CHECK_ITERATIONS_MAX is at least 7
|
||||||
|
static_assert(EKF_CHECK_ITERATIONS_MAX >= 7, "EKF_CHECK_ITERATIONS_MAX must be at least 7");
|
||||||
|
|
||||||
|
// 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 (!plane.arming.is_armed() || quadplane.in_vtol_posvel_mode() || (g2.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_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2)) {
|
||||||
|
// we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset
|
||||||
|
// yaw to resolve the issue
|
||||||
|
ahrs.request_yaw_reset();
|
||||||
|
}
|
||||||
|
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();
|
||||||
|
}
|
||||||
|
// 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 Plane::ekf_over_threshold()
|
||||||
|
{
|
||||||
|
// return false immediately if disabled
|
||||||
|
if (g2.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);
|
||||||
|
|
||||||
|
const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);
|
||||||
|
|
||||||
|
// 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_max >= g2.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 * g2.fs_ekf_thresh))) {
|
||||||
|
over_thresh_count += 2;
|
||||||
|
} else if (vel_variance >= g2.fs_ekf_thresh) {
|
||||||
|
over_thresh_count++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((position_variance >= g2.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ekf_position_ok()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// failsafe_ekf_event - perform ekf failsafe
|
||||||
|
void Plane::failsafe_ekf_event()
|
||||||
|
{
|
||||||
|
// return immediately if ekf failsafe already triggered
|
||||||
|
if (ekf_check_state.failsafe_on) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// EKF failsafe event has occurred
|
||||||
|
ekf_check_state.failsafe_on = true;
|
||||||
|
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
|
||||||
|
|
||||||
|
// if not in a VTOL mode requring position, then nothing needs to be done
|
||||||
|
if (!quadplane.in_vtol_posvel_mode()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (quadplane.in_vtol_auto()) {
|
||||||
|
// the pilot is not controlling via sticks so switch to QLAND
|
||||||
|
plane.set_mode(mode_qland, ModeReason::EKF_FAILSAFE);
|
||||||
|
} else {
|
||||||
|
// the pilot is controlling via sticks so fallbacl to QHOVER
|
||||||
|
plane.set_mode(mode_qhover, ModeReason::EKF_FAILSAFE);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
|
||||||
|
void Plane::failsafe_ekf_off_event(void)
|
||||||
|
{
|
||||||
|
// return immediately if not in ekf failsafe
|
||||||
|
if (!ekf_check_state.failsafe_on) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
ekf_check_state.failsafe_on = false;
|
||||||
|
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
|
||||||
|
}
|
||||||
|
|
||||||
|
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
|
||||||
|
bool Plane::ekf_position_ok() const
|
||||||
|
{
|
||||||
|
if (!ahrs.have_inertial_nav()) {
|
||||||
|
// do not allow navigation with dcm position
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// with EKF use filter status and ekf check
|
||||||
|
nav_filter_status filt_status;
|
||||||
|
if(ahrs.get_filter_status(filt_status)) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// if disarmed we accept a predicted horizontal position
|
||||||
|
if (!plane.arming.is_armed()) {
|
||||||
|
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
|
||||||
|
} else {
|
||||||
|
// once armed we require a good absolute position and EKF must not be in const_pos_mode
|
||||||
|
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
|
||||||
|
}
|
||||||
|
}
|
@ -2189,6 +2189,21 @@ bool QuadPlane::in_vtol_mode(void) const
|
|||||||
in_vtol_auto());
|
in_vtol_auto());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
are we in a VTOL mode that needs position and velocity estimates?
|
||||||
|
*/
|
||||||
|
bool QuadPlane::in_vtol_posvel_mode(void) const
|
||||||
|
{
|
||||||
|
if (!available()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return (plane.control_mode == &plane.mode_qloiter ||
|
||||||
|
plane.control_mode == &plane.mode_qland ||
|
||||||
|
plane.control_mode == &plane.mode_qrtl ||
|
||||||
|
plane.control_mode == &plane.mode_qautotune ||
|
||||||
|
((plane.control_mode == &plane.mode_guided || plane.control_mode == &plane.mode_avoidADSB) && plane.auto_state.vtol_loiter) ||
|
||||||
|
in_vtol_auto());
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
main landing controller. Used for landing and RTL.
|
main landing controller. Used for landing and RTL.
|
||||||
|
@ -89,6 +89,7 @@ public:
|
|||||||
bool verify_vtol_land(void);
|
bool verify_vtol_land(void);
|
||||||
bool in_vtol_auto(void) const;
|
bool in_vtol_auto(void) const;
|
||||||
bool in_vtol_mode(void) const;
|
bool in_vtol_mode(void) const;
|
||||||
|
bool in_vtol_posvel_mode(void) const;
|
||||||
void update_throttle_hover();
|
void update_throttle_hover();
|
||||||
|
|
||||||
// vtol help for is_flying()
|
// vtol help for is_flying()
|
||||||
|
Loading…
Reference in New Issue
Block a user