From ff934d5bca9915edd640739c75a65d6c889a5de1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Aug 2015 12:15:46 +1000 Subject: [PATCH] Copter: added FS_CRASH_CHECK parameter this allows automatic crash detection to be disabled --- ArduCopter/Parameters.cpp | 7 +++++++ ArduCopter/Parameters.h | 4 +++- ArduCopter/crash_check.cpp | 4 ++-- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b6b311c964..a42c9f4fa0 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -496,6 +496,13 @@ const AP_Param::Info Copter::var_info[] PROGMEM = { // @User: Advanced GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT), + // @Param: FS_CRASH_CHECK + // @DisplayName: Crash check enable + // @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected. + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1), + #if FRAME_CONFIG == HELI_FRAME // @Group: HS1_ // @Path: ../libraries/RC_Channel/RC_Channel.cpp diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index d54948bedb..30f8a9d17f 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -163,10 +163,11 @@ public: k_param_heli_stab_col_max, // 88 // - // 90: Motors + // 90: misc2 // k_param_motors = 90, k_param_disarm_delay, + k_param_fs_crash_check, // 97: RSSI k_param_rssi = 97, @@ -429,6 +430,7 @@ public: AP_Int8 land_repositioning; AP_Int8 fs_ekf_action; + AP_Int8 fs_crash_check; AP_Float fs_ekf_thresh; AP_Int16 gcs_pid_mask; diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index cb3f178322..fe6d994028 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -14,8 +14,8 @@ void Copter::crash_check() { static uint16_t crash_counter; // number of iterations vehicle may have been crashed - // return immediately if disarmed - if (!motors.armed() || ap.land_complete) { + // return immediately if disarmed, or crash checking disabled + if (!motors.armed() || ap.land_complete || g.fs_crash_check == 0) { crash_counter = 0; return; }