From a07a8aece1955bd4ce48617ab31c79e89b3e1aac Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 26 Dec 2015 21:58:26 +1100
Subject: [PATCH] RC_Channel: added pwm_to_angle_dz_trim()

---
 libraries/RC_Channel/RC_Channel.cpp | 16 +++++++++++++---
 libraries/RC_Channel/RC_Channel.h   |  2 ++
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 68d3cf19ab..689033b711 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -312,10 +312,10 @@ RC_Channel::update_min_max()
   the current radio_in value using the specified dead_zone
  */
 int16_t
-RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
+RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim)
 {
-    int16_t radio_trim_high = radio_trim + dead_zone;
-    int16_t radio_trim_low  = radio_trim - dead_zone;
+    int16_t radio_trim_high = _trim + dead_zone;
+    int16_t radio_trim_low  = _trim - dead_zone;
 
     // prevent div by 0
     if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)
@@ -330,6 +330,16 @@ RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
         return 0;
 }
 
+/*
+  return an "angle in centidegrees" (normally -4500 to 4500) from
+  the current radio_in value using the specified dead_zone
+ */
+int16_t
+RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
+{
+    return pwm_to_angle_dz_trim(dead_zone, radio_trim);
+}
+
 /*
   return an "angle in centidegrees" (normally -4500 to 4500) from
   the current radio_in value
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index af5856882e..ff010dbae6 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -62,6 +62,7 @@ public:
     void        set_reverse(bool reverse);
     bool        get_reverse(void) const;
     void        set_default_dead_zone(int16_t dzone);
+    uint16_t    get_dead_zone(void) const { return _dead_zone; }
     
     // get the channel number
     uint8_t     get_ch_out(void) const { return _ch_out; };
@@ -105,6 +106,7 @@ public:
     // includes offset from PWM
     //int16_t   get_radio_out(void);
 
+    int16_t                                         pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim);
     int16_t                                         pwm_to_angle_dz(uint16_t dead_zone);
     int16_t                                         pwm_to_angle();