From d3de77f74cd116f78c96e5d0a0c855f5465d0cc4 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Thu, 6 Dec 2012 20:18:33 +0900
Subject: [PATCH] ArduCopter: fix do_set_servo so that it works again

---
 ArduCopter/commands_logic.pde | 44 ++++++++++++++++++++++++++++++++++-
 1 file changed, 43 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index e08ea162ad..7fb5619821 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -790,7 +790,49 @@ static void do_set_home()
 
 static void do_set_servo()
 {
-    APM_RC.OutputCh(command_cond_queue.p1 - 1, command_cond_queue.alt);
+    uint8_t channel_num = -1;
+
+    switch( command_cond_queue.p1 ) {
+        case 1:
+            channel_num = CH_1;
+            break;
+        case 2:
+            channel_num = CH_2;
+            break;
+        case 3:
+            channel_num = CH_3;
+            break;
+        case 4:
+            channel_num = CH_4;
+            break;
+        case 5:
+            channel_num = CH_5;
+            break;
+        case 6:
+            channel_num = CH_6;
+            break;
+        case 7:
+            channel_num = CH_7;
+            break;
+        case 8:
+            channel_num = CH_8;
+            break;
+        case 9:
+            // not used
+            break;
+        case 10:
+            channel_num = CH_10;
+            break;
+        case 11:
+            channel_num = CH_11;
+            break;
+    }
+
+    // send output to channel
+    if (channel_num != -1) {
+        APM_RC.enable_out(channel_num);
+        APM_RC.OutputCh(channel_num, command_cond_queue.alt);
+    }
 }
 
 static void do_set_relay()