From e3a0f1568d24c9b3f41eef82d0ea534772eb3d92 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Wed, 8 Jul 2015 16:24:43 -0400
Subject: [PATCH] Copter: Tradheli to check rotor speed control input before
 arming

Also, force rsc_control input to 0 when disarmed.  This prevents condition where AP_MotorsHeli can receive a rotor speed command greater than zero while disarmed, which was causing the ColYaw function to move the rudder servo.  These two changes are somewhat tied together as it required changing the arming_check to check the RSC_Control not desired_speed from AP_MotorsHeli.
---
 ArduCopter/Copter.h   |  2 ++
 ArduCopter/heli.cpp   | 40 +++++++++++++++++++++++-----------------
 ArduCopter/motors.cpp |  6 +++---
 3 files changed, 28 insertions(+), 20 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 825fbeaa68..a249885c21 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -501,6 +501,8 @@ private:
     // governor.  Even a single "off" frame can cause the rotor to slow dramatically and take a long time to restart.
     ModeFilterInt16_Size5 rotor_speed_deglitch_filter {4};
 
+    int16_t rsc_control_deglitched;
+
     // Tradheli flags
     struct {
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp
index 6ddee2c534..b2b2a818f3 100644
--- a/ArduCopter/heli.cpp
+++ b/ArduCopter/heli.cpp
@@ -137,24 +137,30 @@ void Copter::heli_update_rotor_speed_targets()
 
     // get rotor control method
     uint8_t rsc_control_mode = motors.get_rsc_mode();
-    int16_t rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in);
 
-    switch (rsc_control_mode) {
-        case AP_MOTORS_HELI_RSC_MODE_NONE:
-            // even though pilot passes rotors speed directly to rotor ESC via receiver, motor lib needs to know if
-            // rotor is spinning in case we are using direct drive tailrotor which must be spun up at same time
-        case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH:
-            // pass through pilot desired rotor speed
-            motors.set_desired_rotor_speed(rsc_control_deglitched);
-            break;
-        case AP_MOTORS_HELI_RSC_MODE_SETPOINT:
-            // pass setpoint through as desired rotor speed
-            if (rsc_control_deglitched > 0) {
-                motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
-            }else{
-                motors.set_desired_rotor_speed(0);
-            }
-            break;
+    rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in);
+
+    if (motors.armed()) {
+        switch (rsc_control_mode) {
+            case AP_MOTORS_HELI_RSC_MODE_NONE:
+                // even though pilot passes rotors speed directly to rotor ESC via receiver, motor lib needs to know if
+                // rotor is spinning in case we are using direct drive tailrotor which must be spun up at same time
+            case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH:
+                // pass through pilot desired rotor speed
+                motors.set_desired_rotor_speed(rsc_control_deglitched);
+                break;
+            case AP_MOTORS_HELI_RSC_MODE_SETPOINT:
+                // pass setpoint through as desired rotor speed
+                if (rsc_control_deglitched > 0) {
+                    motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
+                }else{
+                    motors.set_desired_rotor_speed(0);
+                }
+                break;
+        }
+    } else {
+        // if disarmed, force desired_rotor_speed to Zero
+        motors.set_desired_rotor_speed(0);
     }
 
     // when rotor_runup_complete changes to true, log event
diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp
index 264ae69a4e..99e879c46f 100644
--- a/ArduCopter/motors.cpp
+++ b/ArduCopter/motors.cpp
@@ -702,15 +702,15 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
     }
 
     // always check if rotor is spinning on heli
-    #if FRAME_CONFIG == HELI_FRAME
+#if FRAME_CONFIG == HELI_FRAME
     // heli specific arming check
-    if (!motors.allow_arming()){
+    if ((rsc_control_deglitched > 0) || !motors.allow_arming()){
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor Control Engaged"));
         }
         return false;
     }
-    #endif  // HELI_FRAME
+#endif  // HELI_FRAME
 
     // succeed if arming checks are disabled
     if (g.arming_check == ARMING_CHECK_NONE) {