From 79bd8b4fd0e3b2daa8ae91af9ba4cf48be58dd3d Mon Sep 17 00:00:00 2001
From: Dr Gareth Owen <drgowen@gmail.com>
Date: Wed, 29 May 2013 21:22:46 +0100
Subject: [PATCH] rtl resets yaw to same as when armed - unless user specifies
 otherwise

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

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index c2f0a76807..c687582b5e 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -637,7 +637,11 @@ static bool verify_RTL()
                 rtl_loiter_start_time = millis();
 
                 // give pilot back control of yaw
-                set_yaw_mode(YAW_HOLD);
+                if(get_wp_yaw_mode(true) != YAW_HOLD) {
+                    set_yaw_mode(YAW_RESETTOARMEDYAW); // yaw back to initial yaw on take off
+                } else {
+                    set_yaw_mode(YAW_HOLD);
+                }
 
                 // advance to next rtl state
                 rtl_state = RTL_STATE_LOITERING_AT_HOME;