diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 54a0c6272a..223bc2d637 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -223,7 +223,7 @@ static void do_RTL(void)
 
     // set roll, pitch and yaw modes
     set_roll_pitch_mode(RTL_RP);
-    set_yaw_mode(YAW_LOOK_AT_HOME);
+    set_yaw_mode(RTL_YAW);
     set_throttle_mode(RTL_THR);
 
     // set navigation mode
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index e4de48d315..9dad842a49 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -618,6 +618,10 @@
 
 
 // RTL Mode
+#ifndef RTL_YAW
+ # define RTL_YAW                   YAW_LOOK_AT_NEXT_WP
+#endif
+
 #ifndef RTL_RP
  # define RTL_RP                    ROLL_PITCH_AUTO
 #endif