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()