From 8170df7ba883aa064c8281c407df210ea7f1030a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 11 Oct 2016 22:02:20 +1100
Subject: [PATCH] Plane: added servo remapping object

---
 ArduPlane/Parameters.cpp | 4 ++++
 ArduPlane/Parameters.h   | 3 +++
 ArduPlane/Plane.h        | 1 +
 ArduPlane/servos.cpp     | 8 ++++++++
 4 files changed, 16 insertions(+)

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index 6fcdcf9825..21c476bf51 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -1378,6 +1378,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Group: ICE_
     // @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp
     AP_SUBGROUPINFO(ice_control, "ICE_", 2, ParametersG2, AP_ICEngine),
+
+    // @Group: SERVO_
+    // @Path: ../libraries/RC_Channel/SRV_Channel.cpp
+    AP_SUBGROUPINFO(servo_channels, "SERVO", 3, ParametersG2, SRV_Channels),
     
     AP_GROUPEND
 };
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 27fa704602..5bb6b5c2f0 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -585,6 +585,9 @@ public:
 
     // internal combustion engine control
     AP_ICEngine ice_control;
+
+    // control over servo output ranges
+    SRV_Channels servo_channels;
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 0b1e20e8bc..eb36fb0a10 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -44,6 +44,7 @@
 #include <AP_AccelCal/AP_AccelCal.h>                // interface and maths for accelerometer calibration
 #include <AP_AHRS/AP_AHRS.h>         // ArduPilot Mega DCM Library
 #include <RC_Channel/RC_Channel.h>     // RC Channel Library
+#include <RC_Channel/SRV_Channel.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>     // Range finder library
 #include <Filter/Filter.h>                     // Filter library
 #include <AP_Buffer/AP_Buffer.h>      // APM FIFO Buffer
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index 87e1529783..1b75c86612 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -798,6 +798,11 @@ void Plane::set_servos(void)
  */
 void Plane::servos_output(void)
 {
+    hal.rcout->cork();
+    
+    // remap servo output to SERVO* ranges if enabled
+    g2.servo_channels.remap_servo_output();
+
     if (g.rudder_only == 0) {
         // when in RUDDER_ONLY mode we don't send the channel_roll
         // output and instead rely on KFF_RDDRMIX. That allows the yaw
@@ -808,5 +813,8 @@ void Plane::servos_output(void)
     channel_pitch->output();
     channel_throttle->output();
     channel_rudder->output();
+
     RC_Channel_aux::output_ch_all();
+    
+    hal.rcout->push();
 }