Navigator: Fix RTL state sequence

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) {
+				// use current heading to home
-					/* if previous setpoint is valid then use it to calculate heading to home */
+				_mission_item.yaw = get_bearing_to_next_waypoint(
-					_mission_item.yaw = get_bearing_to_next_waypoint(
+							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-								    pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
+							    _navigator->get_home_position()->lat, _navigator->get_home_position()->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);
-				}
 			}
 
 			_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;

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;