Copter: fix tradheli RSC RC passthrough mode

Copter: heli get_pilot_desired_rotor_speed converts interlock input to desired rotor speed

ArduCopter/Copter.h

@@ -759,6 +759,7 @@ private:
     void check_dynamic_flight(void);
     void update_heli_control_dynamics(void);
     void heli_update_landing_swash();
+    float get_pilot_desired_rotor_speed() const;
     void heli_update_rotor_speed_targets();
     void heli_update_autorotation();
 #if MODE_AUTOROTATE_ENABLED == ENABLED

ArduCopter/RC_Channel.cpp

@@ -349,9 +349,15 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
             break;
 
         case AUX_FUNC::MOTOR_INTERLOCK:
-            // Turn on when above LOW, because channel will also be used for speed
+#if FRAME_CONFIG == HELI_FRAME
-            // control signal in tradheli
+            // The interlock logic for ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH is handled 
+            // in heli_update_rotor_speed_targets.  Otherwise turn on when above low.
+            if (copter.motors->get_rsc_mode() != ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH) {
+                copter.ap.motor_interlock_switch = (ch_flag == HIGH || ch_flag == MIDDLE);
+            }
+#else
             copter.ap.motor_interlock_switch = (ch_flag == HIGH || ch_flag == MIDDLE);
+#endif
             break;
 
         case AUX_FUNC::BRAKE:

ArduCopter/RC_Channel.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <RC_Channel/RC_Channel.h>
+#include <AP_Motors/AP_Motors.h>
 #include "mode.h"
 
 class RC_Channel_Copter : public RC_Channel

ArduCopter/heli.cpp

@@ -131,6 +131,17 @@ void Copter::heli_update_landing_swash()
     }
 }
 
+// convert motor interlock switch's position to desired rotor speed expressed as a value from 0 to 1
+// returns zero if motor interlock auxiliary switch hasn't been defined
+float Copter::get_pilot_desired_rotor_speed() const
+{
+    RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK);
+    if (rc_ptr != nullptr) {
+        return (float)rc_ptr->get_control_in() * 0.001f;
+    }
+    return 0.0f;
+}
+
 // heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
 void Copter::heli_update_rotor_speed_targets()
 {
@@ -139,17 +150,15 @@ void Copter::heli_update_rotor_speed_targets()
 
     // get rotor control method
     uint8_t rsc_control_mode = motors->get_rsc_mode();
-    float rsc_control_deglitched = 0.0f;
+
-    RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK);
-    if (rc_ptr != nullptr) {
-        rsc_control_deglitched = rotor_speed_deglitch_filter.apply((float)rc_ptr->get_control_in()) * 0.001f;
-    }
     switch (rsc_control_mode) {
         case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
             // pass through pilot desired rotor speed from the RC
-            if (motors->get_interlock()) {
+            if (get_pilot_desired_rotor_speed() > 0.01) {
-                motors->set_desired_rotor_speed(rsc_control_deglitched);
+                ap.motor_interlock_switch = true;
+                motors->set_desired_rotor_speed(get_pilot_desired_rotor_speed());
             } else {
+                ap.motor_interlock_switch = false;
                 motors->set_desired_rotor_speed(0.0f);
             }
             break;