From 80f3541933349a4b3bc2e946b8ddc768fe28edf3 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 21 Jan 2016 18:59:47 -0800 Subject: [PATCH] Copter: add configurable arming delay --- ArduCopter/Copter.h | 4 ++++ ArduCopter/arming_checks.cpp | 5 +++-- ArduCopter/config.h | 4 ++++ ArduCopter/control_throw.cpp | 2 +- ArduCopter/events.cpp | 4 ++++ ArduCopter/heli.cpp | 8 ++++---- ArduCopter/motors.cpp | 28 ++++++++++++++++++++++------ ArduCopter/switches.cpp | 9 +-------- 8 files changed, 43 insertions(+), 21 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 391e4f18d4..87b524cd18 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -248,6 +248,8 @@ private: uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use uint8_t motor_emergency_stop : 1; // 21 // motor estop switch, shuts off motors when enabled uint8_t land_repo_active : 1; // 22 // true if the pilot is overriding the landing position + uint8_t motor_interlock_switch : 1; // 23 // true if pilot is requesting motor interlock enable + uint8_t in_arming_delay : 1; // 24 // true while we are armed but waiting to spin motors }; uint32_t value; } ap; @@ -489,6 +491,8 @@ private: uint16_t mainLoop_count; // Loiter timer - Records how long we have been in loiter uint32_t rtl_loiter_start_time; + // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. + uint32_t arm_time_ms; // Used to exit the roll and pitch auto trim function uint8_t auto_trim_counter; diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 8b18c8fc0d..f981064719 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -24,6 +24,8 @@ bool Copter::all_arming_checks_passing(bool arming_from_gcs) { if (pre_arm_checks(true)) { set_pre_arm_check(true); + } else { + return false; } return ap.pre_arm_check && arm_checks(true, arming_from_gcs); @@ -51,8 +53,7 @@ bool Copter::pre_arm_checks(bool display_failure) // if it is, switch needs to be in disabled position to arm // otherwise exit immediately. This check to be repeated, // as state can change at any time. - // skip check in Throw mode which takes control of the motor interlock - if (ap.using_interlock && motors.get_interlock()) { + if (ap.using_interlock && ap.motor_interlock_switch) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 99505d0f8e..d24cf7abd7 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -54,6 +54,10 @@ # define MAIN_LOOP_SECONDS 0.0025f # define MAIN_LOOP_MICROS 2500 +#ifndef ARMING_DELAY_SEC + # define ARMING_DELAY_SEC 2.0f +#endif + ////////////////////////////////////////////////////////////////////////////// // FRAME_CONFIG // diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp index aa6e42410c..7af15840e2 100644 --- a/ArduCopter/control_throw.cpp +++ b/ArduCopter/control_throw.cpp @@ -11,7 +11,7 @@ bool Copter::throw_init(bool ignore_checks) return false; #endif - // do not enter the mode when already armed + // do not enter the mode when already armed or when flying if (motors.armed()) { return false; } diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 0d80e59b9d..dcb76ee6b6 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -201,6 +201,10 @@ void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason) } bool Copter::should_disarm_on_failsafe() { + if (ap.in_arming_delay) { + return true; + } + switch(control_mode) { case STABILIZE: case ACRO: diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 139cd905bb..2b6ecdc073 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -147,10 +147,10 @@ void Copter::heli_update_rotor_speed_targets() case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH: // pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom if (rsc_control_deglitched > 0.01f) { - motors.set_interlock(true); + ap.motor_interlock_switch = true; motors.set_desired_rotor_speed(rsc_control_deglitched); } else { - motors.set_interlock(false); + ap.motor_interlock_switch = false; motors.set_desired_rotor_speed(0.0f); } break; @@ -160,10 +160,10 @@ void Copter::heli_update_rotor_speed_targets() // pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode // other than being used to create a crude estimate of rotor speed if (rsc_control_deglitched > 0.0f) { - motors.set_interlock(true); + ap.motor_interlock_switch = true; motors.set_desired_rotor_speed(motors.get_rsc_setpoint()); }else{ - motors.set_interlock(false); + ap.motor_interlock_switch = false; motors.set_desired_rotor_speed(0.0f); } break; diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index f4723ac9a0..9061a85362 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -202,6 +202,12 @@ bool Copter::init_arm_motors(bool arming_from_gcs) // flag exiting this function in_arm_motors = false; + // Log time stamp of arming event + arm_time_ms = millis(); + + // Start the arming delay + ap.in_arming_delay = true; + // return success return true; } @@ -254,22 +260,32 @@ void Copter::init_disarm_motors() // disable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(false); hal.util->set_soft_armed(false); + + ap.in_arming_delay = false; } // motors_output - send output to motors library which will adjust and send to ESCs and servos void Copter::motors_output() { + // Update arming delay state + if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) { + ap.in_arming_delay = false; + } + // check if we are performing the motor test if (ap.motor_test) { motor_test_output(); } else { - if (!ap.using_interlock){ - // if not using interlock switch, set according to Emergency Stop status - // where Emergency Stop is forced false during arming if Emergency Stop switch - // is not used. Interlock enabled means motors run, so we must - // invert motor_emergency_stop status for motors to run. - motors.set_interlock(!ap.motor_emergency_stop); + bool interlock = motors.armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop; + if (!motors.get_interlock() && interlock) { + motors.set_interlock(true); + Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED); + } else if (motors.get_interlock() && !interlock) { + motors.set_interlock(false); + Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED); } + + // send output signals to motors motors.output(); } } diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index d19c6bae7e..ce9ab07869 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -565,14 +565,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_MOTOR_INTERLOCK: // Turn on when above LOW, because channel will also be used for speed // control signal in tradheli - motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); - - // Log new status - if (motors.get_interlock()){ - Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED); - } else { - Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED); - } + ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); break; case AUXSW_BRAKE: