From 74b98b89d0198d022050023ac3a30c31cbf5c3c1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 26 Mar 2015 23:23:17 -0700 Subject: [PATCH] Copter: remove dcm check --- ArduCopter/ArduCopter.pde | 4 +-- ArduCopter/Parameters.h | 5 ++- ArduCopter/Parameters.pde | 7 ---- ArduCopter/config.h | 5 +-- ArduCopter/defines.h | 6 ++-- ArduCopter/ekf_check.pde | 73 ++++++++++++--------------------------- 6 files changed, 30 insertions(+), 70 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index cdb4de8866..23235fda06 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -740,7 +740,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { #endif { update_notify, 8, 10 }, { one_hz_loop, 400, 42 }, - { ekf_dcm_check, 40, 2 }, + { ekf_check, 40, 2 }, { crash_check, 40, 2 }, { landinggear_update, 40, 1 }, { gcs_check_input, 8, 550 }, @@ -815,7 +815,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { #endif { update_notify, 2, 100 }, { one_hz_loop, 100, 420 }, - { ekf_dcm_check, 10, 20 }, + { ekf_check, 10, 20 }, { crash_check, 10, 20 }, { landinggear_update, 10, 10 }, { gcs_check_input, 2, 550 }, diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4819322b1c..0fe42bf06a 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -125,9 +125,9 @@ public: k_param_acro_expo, k_param_throttle_deadzone, k_param_optflow, - k_param_dcmcheck_thresh, // 59 + k_param_dcmcheck_thresh, // deprecated - remove k_param_log_bitmask, - k_param_cli_enabled, + k_param_cli_enabled, // 61 // 65: AP_Limits Library k_param_limits = 65, // deprecated - remove @@ -404,7 +404,6 @@ public: AP_Int8 land_repositioning; AP_Float ekfcheck_thresh; - AP_Float dcmcheck_thresh; #if FRAME_CONFIG == HELI_FRAME // Heli diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index c539efc4eb..66c8542d1b 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -441,13 +441,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(ekfcheck_thresh, "EKF_CHECK_THRESH", EKFCHECK_THRESHOLD_DEFAULT), - // @Param: DCM_CHECK_THRESH - // @DisplayName: DCM yaw error threshold - // @Description: Allows setting the maximum acceptable yaw error as a sin of the yaw error (0 to disable check) - // @Values: 0:Disabled, 0.6:Strict, 0.8:Default, 0.98:Relaxed - // @User: Advanced - GSCALAR(dcmcheck_thresh, "DCM_CHECK_THRESH", DCMCHECK_THRESHOLD_DEFAULT), - #if FRAME_CONFIG == HELI_FRAME // @Group: HS1_ // @Path: ../libraries/RC_Channel/RC_Channel.cpp diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 46ac5a1881..f4a9534761 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -259,13 +259,10 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// EKF & DCM Checker +// EKF Checker #ifndef EKFCHECK_THRESHOLD_DEFAULT # define EKFCHECK_THRESHOLD_DEFAULT 0.8f // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad #endif -#ifndef DCMCHECK_THRESHOLD_DEFAULT - # define DCMCHECK_THRESHOLD_DEFAULT 0.8f // DCM checker's default yaw error threshold above which we will abandon horizontal position hold. The units are sin(angle) so 0.8 = about 60degrees of error -#endif #ifndef EKF_ORIGIN_MAX_DIST_M # define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 2ee110ae72..bd4e57a04e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -331,7 +331,7 @@ enum FlipState { #define ERROR_SUBSYSTEM_FLIP 13 #define ERROR_SUBSYSTEM_AUTOTUNE 14 #define ERROR_SUBSYSTEM_PARACHUTE 15 -#define ERROR_SUBSYSTEM_EKFINAV_CHECK 16 +#define ERROR_SUBSYSTEM_EKFCHECK 16 #define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17 #define ERROR_SUBSYSTEM_BARO 18 #define ERROR_SUBSYSTEM_CPU 19 @@ -357,8 +357,8 @@ enum FlipState { // parachute failed to deploy because of low altitude #define ERROR_CODE_PARACHUTE_TOO_LOW 2 // EKF check definitions -#define ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE 2 -#define ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED 0 +#define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2 +#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0 // Baro specific error codes #define ERROR_CODE_BARO_GLITCH 2 diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index da73ac74ea..8f0726d4ee 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -15,66 +15,44 @@ # 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 or dcm have been out of tolerances - - uint8_t bad_compass : 1; // true if dcm or ekf should be considered untrusted (fail_count_compass has exceeded EKF_CHECK_ITERATIONS_MAX) - + 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_dcm_check - detects if ekf variances or dcm yaw errors that are out of tolerance and triggers failsafe +// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe // should be called at 10hz -void ekf_dcm_check() +void ekf_check() { - 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() || ap.usb_connected || check_type == CHECK_NONE) { - ekf_check_state.fail_count_compass = 0; - ekf_check_state.bad_compass = false; - AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; + if (!motors.armed() || ap.usb_connected || (g.ekfcheck_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 ((check_type == CHECK_EKF && ekf_over_threshold()) || (check_type == CHECK_DCM && dcm_over_threshold())) { + if (ekf_over_threshold()) { // if compass is not yet flagged as bad - if (!ekf_check_state.bad_compass) { + if (!ekf_check_state.bad_variance) { // increase counter - ekf_check_state.fail_count_compass++; + ekf_check_state.fail_count++; // if counter above max then trigger failsafe - if (ekf_check_state.fail_count_compass >= EKF_CHECK_ITERATIONS_MAX) { + if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) { // limit count from climbing too high - ekf_check_state.fail_count_compass = EKF_CHECK_ITERATIONS_MAX; - ekf_check_state.bad_compass = true; + ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX; + ekf_check_state.bad_variance = true; // log an error in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE); + Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE); // send message to gcs if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { - 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")); - } + gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); ekf_check_state.last_warn_time = hal.scheduler->millis(); } failsafe_ekf_event(); @@ -82,14 +60,14 @@ void ekf_dcm_check() } } else { // reduce counter - if (ekf_check_state.fail_count_compass > 0) { - ekf_check_state.fail_count_compass--; + 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_compass && ekf_check_state.fail_count_compass == 0) { - ekf_check_state.bad_compass = false; + if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) { + ekf_check_state.bad_variance = false; // log recovery in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED); + Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_VARIANCE_CLEARED); // clear failsafe failsafe_ekf_off_event(); } @@ -97,18 +75,11 @@ void ekf_dcm_check() } // set AP_Notify flags - AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; + AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance; // 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() {