diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 5e52ceeb72..2d579eb29e 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -535,6 +535,10 @@ static float sin_pitch;
 // or in SuperSimple mode when the copter leaves a 20m radius from home.
 static int32_t initial_simple_bearing;
 
+// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable
+static int32_t initial_armed_bearing;
+
+
 ////////////////////////////////////////////////////////////////////////////////
 // Rate contoller targets
 ////////////////////////////////////////////////////////////////////////////////
@@ -1523,7 +1527,7 @@ void update_yaw_mode(void)
 
     case YAW_RESETTOARMEDYAW:
         // changes yaw to be same as when quad was armed
-        nav_yaw = get_yaw_slew(nav_yaw, initial_simple_bearing, AUTO_YAW_SLEW_RATE);
+        nav_yaw = get_yaw_slew(nav_yaw, initial_armed_bearing, AUTO_YAW_SLEW_RATE);
         get_stabilize_yaw(nav_yaw);
 
         // if there is any pilot input, switch to YAW_HOLD mode for the next iteration
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index f6683504a4..a781885a9c 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -136,6 +136,8 @@ static void init_arm_motors()
     // --------------------
     init_simple_bearing();
 
+    initial_armed_bearing = ahrs.yaw_sensor;
+
     // Reset home position
     // -------------------
     if(ap.home_is_set)