From 64f98622c0461b04e69724472d9c4fe109b6218f Mon Sep 17 00:00:00 2001
From: bnsgeyer <bnsgeyer@aol.com>
Date: Tue, 7 Jan 2020 00:12:54 -0500
Subject: [PATCH] Copter: fix tradheli RSC RC passthrough mode

Copter: heli get_pilot_desired_rotor_speed converts interlock input to desired rotor speed
---
 ArduCopter/Copter.h       |  1 +
 ArduCopter/RC_Channel.cpp | 10 ++++++++--
 ArduCopter/RC_Channel.h   |  1 +
 ArduCopter/heli.cpp       | 23 ++++++++++++++++-------
 4 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 3e5dec6632..74a77a7a5b 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -749,6 +749,7 @@ private:
     bool should_use_landing_swash() const;
     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
diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp
index cd62e30533..d157d85be8 100644
--- a/ArduCopter/RC_Channel.cpp
+++ b/ArduCopter/RC_Channel.cpp
@@ -357,9 +357,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
-            // control signal in tradheli
+#if FRAME_CONFIG == HELI_FRAME
+            // 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:
diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h
index 7395d4c597..2216e58f4f 100644
--- a/ArduCopter/RC_Channel.h
+++ b/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
diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp
index 31e23da4d7..3a72de1e8c 100644
--- a/ArduCopter/heli.cpp
+++ b/ArduCopter/heli.cpp
@@ -126,6 +126,17 @@ void Copter::heli_update_landing_swash()
     motors->set_collective_for_landing(should_use_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()
 {
@@ -134,17 +145,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()) {
-                motors->set_desired_rotor_speed(rsc_control_deglitched);
+            if (get_pilot_desired_rotor_speed() > 0.01) {
+                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;