From 9375fc8947e972c97a8f6c9704cf2aa573b59b7c Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 17 Nov 2014 14:00:31 -0800
Subject: [PATCH] RC_Channel: add get_control_mid function

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

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 014daee89b..3de5f72684 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -224,6 +224,28 @@ RC_Channel::calc_pwm(void)
     radio_out = constrain_int16(radio_out, radio_min.get(), radio_max.get());
 }
 
+
+/*
+  return the center stick position expressed as a control_in value
+  used for thr_mid in copter
+ */
+int16_t
+RC_Channel::get_control_mid() {
+    if (_type == RC_CHANNEL_TYPE_RANGE) {
+        int16_t r_in = (radio_min.get()+radio_max.get())/2;
+
+        if (_reverse == -1) {
+            r_in = radio_max.get() - (r_in - radio_min.get());
+        }
+
+        int16_t radio_trim_low  = radio_min + _dead_zone;
+
+        return (_low + ((int32_t)(_high - _low) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low));
+    } else {
+        return 0;
+    }
+}
+
 // ------------------------------------------
 
 void
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index b16bfad791..16a34c104d 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -61,6 +61,9 @@ public:
     // get the channel number
     uint8_t     get_ch_out(void) const { return _ch_out; };
 
+    // get the center stick position expressed as a control_in value
+    int16_t     get_control_mid();
+
     // read input from APM_RC - create a control_in value
     void        set_pwm(int16_t pwm);
     static void set_pwm_all(void);