10 #ifndef EKF_CHECK_ITERATIONS_MAX 11 # define EKF_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure 14 #ifndef EKF_CHECK_WARNING_TIME 15 # define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds 38 if (!
motors->
armed() ||
ap.usb_connected || (g.fs_ekf_thresh <= 0.0f)) {
42 failsafe_ekf_off_event();
47 if (ekf_over_threshold()) {
78 failsafe_ekf_off_event();
93 if (g.fs_ekf_thresh <= 0.0f) {
98 float position_variance, vel_variance, height_variance, tas_variance;
101 ahrs.
get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset);
104 uint8_t over_thresh_count = 0;
105 if (mag_variance.
length() >= g.fs_ekf_thresh) {
108 if (vel_variance >= g.fs_ekf_thresh) {
111 if (position_variance >= g.fs_ekf_thresh) {
115 if (over_thresh_count >= 2) {
120 if (optflow_position_ok() || ekf_position_ok()) {
140 if (control_mode ==
LAND && landing_with_GPS()) {
141 mode_land.do_not_use_GPS();
151 switch (g.fs_ekf_action) {
175 failsafe.ekf =
false;
183 float yaw_angle_change_rad;
185 if (new_ekfYawReset_ms != ekfYawReset_ms) {
186 attitude_control->inertial_frame_reset();
187 ekfYawReset_ms = new_ekfYawReset_ms;
191 #if AP_AHRS_NAVEKF_AVAILABLE 193 if ((EKF2.getPrimaryCoreIndex() != ekf_primary_core) && (EKF2.getPrimaryCoreIndex() != -1)) {
194 attitude_control->inertial_frame_reset();
195 ekf_primary_core = EKF2.getPrimaryCoreIndex();
197 gcs().
send_text(MAV_SEVERITY_WARNING,
"EKF primary changed:%d", (
unsigned)ekf_primary_core);
#define DATA_EKF_YAW_RESET
#define ERROR_CODE_FAILSAFE_RESOLVED
#define ERROR_CODE_FAILSAFE_OCCURRED
bool ekf_over_threshold()
#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV
#define EKF_CHECK_WARNING_TIME
#define ERROR_SUBSYSTEM_EKFCHECK
bool get_origin(Location &ret) const override
AP_MotorsMatrix motors(400)
uint32_t getLastYawResetAngle(float &yawAng) const override
void failsafe_ekf_event()
#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const override
void failsafe_ekf_off_event(void)
void send_text(MAV_SEVERITY severity, const char *fmt,...)
static struct @10 ekf_check_state
static struct notify_flags_and_values_type flags
AP_HAL_MAIN_CALLBACKS & copter
#define EKF_CHECK_ITERATIONS_MAX
#define ERROR_SUBSYSTEM_EKF_PRIMARY
#define FS_EKF_ACTION_ALTHOLD
#define FS_EKF_ACTION_LAND
#define FS_EKF_ACTION_LAND_EVEN_STABILIZE
#define ERROR_CODE_EKFCHECK_BAD_VARIANCE