From c76621bdb2f56a804bb992f4f890e9784d133996 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@px4.io>
Date: Fri, 23 Mar 2018 11:01:47 +0100
Subject: [PATCH] RLT: set yaw to current yaw during climb

---
 src/modules/navigator/mission.cpp | 3 +--
 src/modules/navigator/rtl.cpp     | 2 +-
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index ddbd24c9b7..f4e26a8c50 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -54,7 +54,6 @@
 #include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <lib/ecl/geo/geo.h>
-#include <lib/mathlib/mathlib.h>
 #include <navigator/navigation.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -1227,7 +1226,7 @@ Mission::heading_sp_update()
 				  point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]);
 
 		if (d_current > _navigator->get_acceptance_radius()) {
-			float yaw = _wrap_pi(
+			float yaw = wrap_pi(
 					    get_bearing_to_next_waypoint(point_from_latlon[0],
 							    point_from_latlon[1], point_to_latlon[0],
 							    point_to_latlon[1]) + yaw_offset);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 682afc6bbf..47208b3aa3 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -155,7 +155,7 @@ RTL::set_rtl_item()
 			_mission_item.lon = gpos.lon;
 			_mission_item.altitude = return_alt;
 			_mission_item.altitude_is_relative = false;
-			_mission_item.yaw = NAN;
+			_mission_item.yaw = _navigator->get_local_position()->yaw;
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;