From 33e4f4443453d51e18c1d80c0f0a0f7a829d684d Mon Sep 17 00:00:00 2001
From: "Kirill A. Kornilov" <kirill@tp>
Date: Tue, 25 Nov 2014 08:19:27 +1100
Subject: [PATCH] RC_Channel: added set_radio_trimmed()

take into account trim value in set_radio()
---
 libraries/RC_Channel/RC_Channel_aux.cpp | 18 ++++++++++++++++++
 libraries/RC_Channel/RC_Channel_aux.h   |  3 +++
 2 files changed, 21 insertions(+)

diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp
index 545d7fd7b4..69300856af 100644
--- a/libraries/RC_Channel/RC_Channel_aux.cpp
+++ b/libraries/RC_Channel/RC_Channel_aux.cpp
@@ -162,6 +162,24 @@ RC_Channel_aux::set_radio(RC_Channel_aux::Aux_servo_function_t function, int16_t
     }
 }
 
+/*
+  set radio_out for all channels matching the given function type, allow radio_trim to center servo
+ */
+void
+RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::Aux_servo_function_t function, int16_t value)
+{
+    if (!function_assigned(function)) {
+        return;
+    }
+    for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) {
+        if (_aux_channels[i] && _aux_channels[i]->function.get() == function) {
+        	int16_t value2 = value - 1500 + _aux_channels[i]->radio_trim;
+			_aux_channels[i]->radio_out = constrain_int16(value2,_aux_channels[i]->radio_min,_aux_channels[i]->radio_max);
+            _aux_channels[i]->output();
+		}
+    }
+}
+
 /*
   set and save the trim value to radio_in for all channels matching
   the given function type
diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h
index 2eac1927dd..44fcc5d439 100644
--- a/libraries/RC_Channel/RC_Channel_aux.h
+++ b/libraries/RC_Channel/RC_Channel_aux.h
@@ -82,6 +82,9 @@ public:
 	// set radio_out for a function channel
 	static void set_radio(Aux_servo_function_t function, int16_t value);
 
+	// set radio_out for all channels matching the given function type, allow radio_trim to center servo
+	static void set_radio_trimmed(Aux_servo_function_t function, int16_t value);
+
 	// set and save the trim for a function channel to radio_in
 	static void set_radio_trim(Aux_servo_function_t function);