mirror of https://github.com/ArduPilot/ardupilot
Copter: added FS_CRASH_CHECK parameter
this allows automatic crash detection to be disabled
This commit is contained in:
parent
90909f2b4a
commit
ff934d5bca
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue