From 5394c661c7bf2b660a113999bcd7e0509f07845a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 23 Oct 2015 20:22:50 +0200
Subject: [PATCH] Navigator: Fix RTL to return in a straight line

---
 src/modules/navigator/rtl.cpp | 10 +++++++++-
 src/modules/navigator/rtl.h   |  2 ++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 8a4d89ef35..6e754588bb 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -58,6 +58,7 @@
 RTL::RTL(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
+	_rtl_start_lock(false),
 	_param_return_alt(this, "RTL_RETURN_ALT", false),
 	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
 	_param_land_delay(this, "RTL_LAND_DELAY", false)
@@ -95,6 +96,7 @@ RTL::on_activation()
 		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
 			   + _param_return_alt.get()) {
 			_rtl_state = RTL_STATE_CLIMB;
+			_rtl_start_lock = false;
 
 		/* otherwise go straight to return */
 		} else {
@@ -102,6 +104,7 @@ RTL::on_activation()
 			_rtl_state = RTL_STATE_RETURN;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.altitude = _navigator->get_global_position()->alt;
+			_rtl_start_lock = false;
 		}
 
 	}
@@ -126,7 +129,10 @@ RTL::set_rtl_item()
 	/* make sure we have the latest params */
 	updateParams();
 
-	set_previous_pos_setpoint();
+	if (!_rtl_start_lock) {
+		set_previous_pos_setpoint();
+	}
+
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
@@ -182,6 +188,8 @@ RTL::set_rtl_item()
 		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
 			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
+
+		_rtl_start_lock = true;
 		break;
 	}
 
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 3506065802..464a1d4ee4 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -87,6 +87,8 @@ private:
 		RTL_STATE_LANDED,
 	} _rtl_state;
 
+	bool _rtl_start_lock;
+
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;