From 1c26ed0fca3be5ee5c7574bd88deee4b2a67ddf1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 27 Jul 2015 12:49:03 +1000
Subject: [PATCH] Copter: fixed deadzone handling for external tail gyro

when using an external tail gyro on a flybar heli the stick input
should be directly passed to output. This patch fixes the use of
deadzone in that passthrough.

It also makes the tail handling consistent with roll and pitch
handling, by not using ACRO_YAW_P when in tail pass-through.

Finally it also fixes deadzone handling for roll and pitch, and
removes the unnecessary get_pilot_desired_yaw_rate() that has a
different prototype from the one used in the rest of the code
---
 ArduCopter/Copter.h              |  1 -
 ArduCopter/heli_control_acro.cpp | 35 +++++++++++++++++++-------------
 2 files changed, 21 insertions(+), 15 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 0b306bd8fc..cf3c73da55 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -826,7 +826,6 @@ private:
     void heli_radio_passthrough();
     bool heli_acro_init(bool ignore_checks);
     void heli_acro_run();
-    void get_pilot_desired_yaw_rate(int16_t yaw_in, float &yaw_out);
     bool heli_stabilize_init(bool ignore_checks);
     void heli_stabilize_run();
     void read_inertia();
diff --git a/ArduCopter/heli_control_acro.cpp b/ArduCopter/heli_control_acro.cpp
index 9cc9c0a697..6111c3e211 100644
--- a/ArduCopter/heli_control_acro.cpp
+++ b/ArduCopter/heli_control_acro.cpp
@@ -51,25 +51,32 @@ void Copter::heli_acro_run()
         // run attitude controller
         attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
     }else{
-        // flybar helis only need yaw rate control
-        get_pilot_desired_yaw_rate(channel_yaw->control_in, target_yaw);
+        /*
+          for fly-bar passthrough use control_in values with no
+          deadzone. This gives true pass-through.
+         */
+        float roll_in = channel_roll->pwm_to_angle_dz(0);
+        float pitch_in = channel_pitch->pwm_to_angle_dz(0);
+        float yaw_in;
+        
+        if (motors.supports_yaw_passthrough()) {
+            // if the tail on a flybar heli has an external gyro then
+            // also use no deadzone for the yaw control and
+            // pass-through the input direct to output.
+            yaw_in = channel_yaw->pwm_to_angle_dz(0);
+        } else {
+            // if there is no external gyro then run the usual
+            // ACRO_YAW_P gain on the input control, including
+            // deadzone
+            yaw_in = get_pilot_desired_yaw_rate(channel_yaw->control_in);
+        }
+
         // run attitude controller
-        attitude_control.passthrough_bf_roll_pitch_rate_yaw(channel_roll->control_in, channel_pitch->control_in, target_yaw);
+        attitude_control.passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
     }
 
     // output pilot's throttle without angle boost
     attitude_control.set_throttle_out(channel_throttle->control_in, false, g.throttle_filt);
 }
 
-// get_pilot_desired_yaw_rate - transform pilot's yaw input into a desired yaw angle rate
-// returns desired yaw rate in centi-degrees-per-second
-void Copter::get_pilot_desired_yaw_rate(int16_t yaw_in, float &yaw_out)
-{
-    // calculate rate request
-    float rate_bf_yaw_request = yaw_in * g.acro_yaw_p;
-
-    // hand back rate request
-    yaw_out = rate_bf_yaw_request;
-}
-
 #endif  //HELI_FRAME