diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp
index 545d7fd7b46e27206c3b04c379b37dfcd4b4fc02..69300856af17bc1fa39bf07cefcc7540c86da635 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 2eac1927ddc1c8aea131cb77d380dc2ef27d795b..44fcc5d4390d5399f3a23a0a5caee5e010b6ace1 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);