From 85b074f8d84c8be30d6d93ed5e00d850292f1fae Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Sun, 9 Apr 2017 23:16:50 +0200
Subject: [PATCH] Navigator: Fix RTL state sequence

---
 src/modules/navigator/rtl.cpp | 29 ++++++-----------------------
 src/modules/navigator/rtl.h   |  2 --
 2 files changed, 6 insertions(+), 25 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 61a5346a75..70d6b56f3d 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -60,7 +60,6 @@
 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_min_loiter_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
@@ -94,8 +93,6 @@ RTL::get_rtl_altitude()
 void
 RTL::on_activation()
 {
-	/* reset starting point so we override what the triplet contained from the previous navigation state */
-	_rtl_start_lock = false;
 	set_current_position_item(&_mission_item);
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
@@ -139,10 +136,6 @@ RTL::set_rtl_item()
 {
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	if (!_rtl_start_lock) {
-		set_previous_pos_setpoint();
-	}
-
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
@@ -158,7 +151,7 @@ RTL::set_rtl_item()
 
 			// we are close to home, limit climb to min
 			if (home_dist < _param_rtl_min_dist.get()) {
-				climb_alt = _navigator->get_home_position()->alt + _param_min_loiter_alt.get();
+				climb_alt = _navigator->get_home_position()->alt + _param_descend_alt.get();
 			}
 
 			_mission_item.lat = _navigator->get_global_position()->lat;
@@ -194,18 +187,10 @@ RTL::set_rtl_item()
 				_mission_item.yaw = _navigator->get_home_position()->yaw;
 
 			} else {
-				if (pos_sp_triplet->previous.valid) {
-					/* if previous setpoint is valid then use it to calculate heading to home */
-					_mission_item.yaw = get_bearing_to_next_waypoint(
-								    pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
-								    _mission_item.lat, _mission_item.lon);
-
-				} else {
-					/* else use current position */
-					_mission_item.yaw = get_bearing_to_next_waypoint(
-								    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-								    _mission_item.lat, _mission_item.lon);
-				}
+				// use current heading to home
+				_mission_item.yaw = get_bearing_to_next_waypoint(
+							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+							    _navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
 			}
 
 			_mission_item.loiter_radius = _navigator->get_loiter_radius();
@@ -218,8 +203,6 @@ RTL::set_rtl_item()
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "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;
 		}
 
@@ -297,8 +280,8 @@ RTL::set_rtl_item()
 		}
 
 	case RTL_STATE_LAND: {
-			_mission_item.yaw = _navigator->get_home_position()->yaw;
 			set_land_item(&_mission_item, false);
+			_mission_item.yaw = _navigator->get_home_position()->yaw;
 
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
 			break;
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index a11101732f..b7b552cdda 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -94,8 +94,6 @@ private:
 		RTL_STATE_LANDED,
 	} _rtl_state;
 
-	bool _rtl_start_lock;
-
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_min_loiter_alt;
 	control::BlockParamFloat _param_descend_alt;