From 12720bbbe17808aecf086f46f5da074597068682 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 26 May 2014 16:00:25 +0900
Subject: [PATCH] Copter: accept condition-yaw commands in guided

---
 ArduCopter/commands_logic.pde | 26 +++++++++++++++++++++++---
 1 file changed, 23 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index ca68da8088..f67a1acc97 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -856,14 +856,34 @@ static bool verify_yaw()
 // do_guided - start guided mode
 static bool do_guided(const AP_Mission::Mission_Command& cmd)
 {
+    Vector3f pos;       // target location
+
     // only process guided waypoint if we are in guided mode
     if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
         return false;
     }
 
-    // set wp_nav's destination
-    Vector3f pos = pv_location_to_vector(cmd.content.location);
-    guided_set_destination(pos);
+    // switch to handle different commands
+    switch (cmd.id) {
+
+        case MAV_CMD_NAV_WAYPOINT:
+            // set wp_nav's destination
+            pos = pv_location_to_vector(cmd.content.location);
+            guided_set_destination(pos);
+            return true;
+            break;
+
+        case MAV_CMD_CONDITION_YAW:
+            do_yaw(cmd);
+            return true;
+            break;
+
+        default:
+            // reject unrecognised command
+            return false;
+            break;
+    }
+
     return true;
 }