From 67c2bc7d0b9d0641c116eb56e5559ba8afc6a9c9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 15 Jan 2014 14:02:26 +0900
Subject: [PATCH] Copter: add support for MAV_CMD_DO_SET_SERVO

---
 ArduCopter/GCS_Mavlink.pde |  5 +++++
 ArduCopter/motors.pde      | 32 ++++++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 757bff27a6..f909fcf43f 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1215,6 +1215,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             }
             break;
 
+        case MAV_CMD_DO_SET_SERVO:
+            servo_write(packet.param1 - 1, packet.param2);
+            result = MAV_RESULT_ACCEPTED;
+            break;
+
         case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
             if (packet.param1 == 1 || packet.param1 == 3) {
                 // when packet.param1 == 3 we reboot to hold in bootloader
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 75ca1ee06b..3f0096ae15 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -523,3 +523,35 @@ set_servos_4()
     motors.output();
 }
 
+// servo_write - writes to a servo after checking the channel is not used for a motor
+static void servo_write(uint8_t ch, uint16_t pwm)
+{
+    bool servo_ok = false;
+
+    #if (FRAME_CONFIG == QUAD_FRAME)
+        // Quads can use RC5 and higher as servos
+        if (ch >= CH_5) servo_ok = true;
+    #elif (FRAME_CONFIG == TRI_FRAME || FRAME_CONFIG == SINGLE_FRAME)
+        // Tri's and Singles can use RC5, RC6, RC8 and higher
+        if (ch == CH_5 || ch == CH_6 || ch >= CH_8) servo_ok = true;
+    #elif (FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME)
+        // Hexa and Y6 can use RC7 and higher
+        if (ch >= CH_7) servo_ok = true;
+    #elif (FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME)
+        // Octa and X8 can use RC9 and higher
+        if (ch >= CH_9) servo_ok = true;
+    #elif (FRAME_CONFIG == HELI_FRAME)
+        // Heli's can use RC5, RC6, RC7, not RC8, and higher
+        if (ch == CH_5 || ch == CH_6 || ch == CH_7 || ch >= CH_9) servo_ok = true;
+    #else
+        // throw compile error if frame type is unrecognise
+        #error Unrecognised frame type
+    #endif
+
+    // debug -- remove me!
+    cliSerial->printf_P(PSTR("\nCh:%d %d %d\n"),(int)ch, (int)pwm, (int)servo_ok);
+    if (servo_ok) {
+        hal.rcout->enable_ch(ch);
+        hal.rcout->write(ch, pwm);
+    }
+}