Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: adapt auto disarm check to use a timer

Browse Source
This commit is contained in:
Jonathan Challinger 2015-11-06 01:30:11 -08:00 committed by Randy Mackay
parent e8305c5653
commit 6524222397
2 changed files with 14 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ArduCopter.cpp
View File

@ -108,6 +108,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(update_batt_compass, 40, 120),
SCHED_TASK(read_aux_switches, 40, 50),
SCHED_TASK(arm_motors_check, 40, 50),
SCHED_TASK(auto_disarm_check, 40, 50),
SCHED_TASK(auto_trim, 40, 75),
SCHED_TASK(update_altitude, 40, 140),
SCHED_TASK(run_nav_updates, 8, 100),
@ -475,9 +476,6 @@ void Copter::one_hz_loop()
pre_arm_checks(false);
}
// auto disarm checks
auto_disarm_check();
if (!motors.armed()) {
// make it possible to change ahrs orientation at runtime during initial config
ahrs.set_orientation();

30
ArduCopter/motors.cpp
View File

@ -7,7 +7,7 @@
#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds
#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second
static uint8_t auto_disarming_counter;
static uint32_t auto_disarm_begin;
// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
@ -43,7 +43,7 @@ void Copter::arm_motors_check()
if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) {
auto_trim_counter = 250;
// ensure auto-disarm doesn't trigger immediately
auto_disarming_counter = 0;
auto_disarm_begin = millis();
}
// full left
@ -70,25 +70,24 @@ void Copter::arm_motors_check()
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
// called at 1hz
void Copter::auto_disarm_check()
{
uint8_t disarm_delay = constrain_int16(g.disarm_delay, 0, 127);
uint32_t tnow_ms = millis();
uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127);
// exit immediately if we are already disarmed, or if auto
// disarming is disabled
if (!motors.armed() || disarm_delay == 0) {
auto_disarming_counter = 0;
if (!motors.armed() || disarm_delay_ms == 0) {
auto_disarm_begin = tnow_ms;
return;
}
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
auto_disarming_counter++;
#if FRAME_CONFIG != HELI_FRAME
// 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 /= 2;
disarm_delay_ms /= 2;
#endif
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
@ -100,19 +99,16 @@ void Copter::auto_disarm_check()
thr_low = g.rc_3.control_in <= deadband_top;
}
if (thr_low && ap.land_complete) {
// increment counter
auto_disarming_counter++;
} else {
// reset counter
auto_disarming_counter = 0;
if (!thr_low || !ap.land_complete) {
// reset timer
auto_disarm_begin = tnow_ms;
}
}
// disarm once counter expires
if (auto_disarming_counter >= disarm_delay) {
// disarm once timer expires
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
init_disarm_motors();
auto_disarming_counter = 0;
auto_disarm_begin = tnow_ms;
}
}