Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 01:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: adapt auto disarm for sprung throttle stick copters

Browse Source
This commit is contained in:
Jonathan Challinger 2015-08-14 12:08:03 -07:00 committed by Randy Mackay
parent 51ff905ba8
commit 49126e83e2
1 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
ArduCopter/motors.cpp
View File

@ -75,33 +75,42 @@ void Copter::arm_motors_check()
// called at 1hz // called at 1hz
void Copter::auto_disarm_check() void Copter::auto_disarm_check()
{ {
uint8_t disarm_delay = AUTO_DISARMING_DELAY_LONG;
uint8_t disarm_delay; // exit immediately if we are already disarmed
if (!motors.armed()) {
// exit immediately if we are already disarmed or throttle output is not zero,
if (!motors.armed() || !ap.throttle_zero) {
auto_disarming_counter = 0; auto_disarming_counter = 0;
return; return;
} }
// allow auto disarm in manual flight modes or Loiter/AltHold if we're landed
// always allow auto disarm if using interlock switch or motors are Emergency Stopped // always allow auto disarm if using interlock switch or motors are Emergency Stopped
if (mode_has_manual_throttle(control_mode) || ap.land_complete || (ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) { if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
auto_disarming_counter++; auto_disarming_counter++;
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less // 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 // obvious the copter is armed as the motors will not be spinning
if (ap.using_interlock || ap.motor_emergency_stop){ disarm_delay = AUTO_DISARMING_DELAY_SHORT;
disarm_delay = AUTO_DISARMING_DELAY_SHORT; } else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;
if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else { } else {
disarm_delay = AUTO_DISARMING_DELAY_LONG; float deadband_top = g.rc_3.get_control_mid() + g.throttle_deadzone;
thr_low = g.rc_3.control_in <= deadband_top;
} }
if(auto_disarming_counter >= disarm_delay) { if (thr_low && ap.land_complete) {
init_disarm_motors(); // increment counter
auto_disarming_counter++;
} else {
// reset counter
auto_disarming_counter = 0; auto_disarming_counter = 0;
} }
}else{ }
// disarm once counter expires
if (auto_disarming_counter >= disarm_delay) {
init_disarm_motors();
auto_disarming_counter = 0; auto_disarming_counter = 0;
} }
} }