5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-25 01:03:59 -04:00

Copter: added DISARM_DELAY parameter

this allows automatic disarming to be disabled, or set to a shorter or
longer time as appropriate for the user
This commit is contained in:
Andrew Tridgell 2015-08-31 11:44:08 +10:00
parent f6f2973acd
commit 90909f2b4a
4 changed files with 19 additions and 6 deletions

View File

@ -432,6 +432,14 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
// @User: Standard
GSCALAR(arming_check, "ARMING_CHECK", ARMING_CHECK_ALL),
// @Param: DISARM_DELAY
// @DisplayName: Disarm delay
// @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm.
// @Units: Seconds
// @Range: 0 127
// @User: Advanced
GSCALAR(disarm_delay, "DISARM_DELAY", AUTO_DISARMING_DELAY),
// @Param: ANGLE_MAX
// @DisplayName: Angle Max
// @Description: Maximum lean angle in all flight modes

View File

@ -166,6 +166,7 @@ public:
// 90: Motors
//
k_param_motors = 90,
k_param_disarm_delay,
// 97: RSSI
k_param_rssi = 97,
@ -424,6 +425,7 @@ public:
AP_Int8 ch11_option;
AP_Int8 ch12_option;
AP_Int8 arming_check;
AP_Int8 disarm_delay;
AP_Int8 land_repositioning;
AP_Int8 fs_ekf_action;

View File

@ -695,6 +695,10 @@
# define ALT_HOLD_ACCEL_MAX 250 // if you change this you must also update the duplicate declaration in AC_WPNav.h
#endif
#ifndef AUTO_DISARMING_DELAY
# define AUTO_DISARMING_DELAY 10
#endif
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//

View File

@ -5,8 +5,6 @@
#define ARM_DELAY 20 // called at 10hz so 2 seconds
#define DISARM_DELAY 20 // called at 10hz so 2 seconds
#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds
#define AUTO_DISARMING_DELAY_LONG 10 // called at 1hz so 10 seconds
#define AUTO_DISARMING_DELAY_SHORT 5 // called at 1hz so 5 seconds
#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second
static uint8_t auto_disarming_counter;
@ -75,10 +73,11 @@ void Copter::arm_motors_check()
// called at 1hz
void Copter::auto_disarm_check()
{
uint8_t disarm_delay = AUTO_DISARMING_DELAY_LONG;
uint8_t disarm_delay = constrain_int16(g.disarm_delay, 0, 127);
// exit immediately if we are already disarmed
if (!motors.armed()) {
// exit immediately if we are already disarmed, or if auto
// disarming is disabled
if (!motors.armed() || disarm_delay == 0) {
auto_disarming_counter = 0;
return;
}
@ -88,7 +87,7 @@ void Copter::auto_disarm_check()
auto_disarming_counter++;
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
// obvious the copter is armed as the motors will not be spinning
disarm_delay = AUTO_DISARMING_DELAY_SHORT;
disarm_delay /= 2;
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;