Copter: add configurable arming delay

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;

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");
         }

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
 //

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;
     }

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:

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;

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();
     }
 }

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: