From fbe43dba5c0bd227803c7fa5ddeeb6f07c3b2cf9 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tomp@AionRobotics.com>
Date: Wed, 1 May 2019 13:43:27 -0700
Subject: [PATCH] RC_Channel: refactor stick_mixing to library

---
 libraries/RC_Channel/RC_Channel.cpp | 20 ++++++++++++++++++++
 libraries/RC_Channel/RC_Channel.h   |  2 ++
 2 files changed, 22 insertions(+)

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index b216c8873a..38be977c79 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -359,6 +359,26 @@ bool RC_Channel::has_override() const
     return is_positive(override_timeout_ms) && ((AP_HAL::millis() - last_override_time) < (uint32_t)override_timeout_ms);
 }
 
+/*
+  perform stick mixing on one channel
+  This type of stick mixing reduces the influence of the auto
+  controller as it increases the influence of the users stick input,
+  allowing the user full deflection if needed
+ */
+int16_t RC_Channel::stick_mixing(const int16_t servo_in)
+{
+    float ch_inf = (float)(radio_in - radio_trim);
+    ch_inf = fabsf(ch_inf);
+    ch_inf = MIN(ch_inf, 400.0f);
+    ch_inf = ((400.0f - ch_inf) / 400.0f);
+
+    int16_t servo_out = servo_in;
+    servo_out *= ch_inf;
+    servo_out += control_in;
+
+    return servo_out;
+}
+
 //
 // support for auxillary switches:
 //
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 21fd569537..d23d596c1f 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -71,6 +71,8 @@ public:
     void       set_override(const uint16_t v, const uint32_t timestamp_us);
     bool       has_override() const;
 
+    int16_t    stick_mixing(const int16_t servo_in);
+
     // get control input with zero deadzone
     int16_t    get_control_in_zero_dz(void) const;