If already in landing, go straight to state RTL_LAND. Also cleaned up the comments

src/modules/navigator/rtl.cpp

@@ -59,7 +59,7 @@ RTL::RTL(Navigator *navigator) :
 void
 RTL::on_inactive()
 {
-	// reset RTL state
+	// Reset RTL state.
 	_rtl_state = RTL_STATE_NONE;
 }
 
@@ -72,23 +72,26 @@ RTL::rtl_type() const
 void
 RTL::on_activation()
 {
-	if (_navigator->get_land_detected()->landed
+	if (_navigator->get_land_detected()->landed) {
-	    || (_navigator->get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
+		// For safety reasons don't go into RTL if landed or currently in landing.
-		// for safety reasons don't go into RTL if landed or currently in landing
 		_rtl_state = RTL_STATE_LANDED;
 
+	} else if (_navigator->get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+		// Do not go into full RTL if already performing a land.
+		_rtl_state = RTL_STATE_LAND;
+
 	} else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_landing()) {
-		// RTL straight to RETURN state, but mission will takeover for landing
+		// RTL straight to RETURN state, but mission will takeover for landing.
 
 	} else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get())
 		   || _rtl_alt_min) {
 
-		// if lower than return altitude, climb up first
+		// If lower than return altitude, climb up first.
-		// if rtl_alt_min is true then forcing altitude change even if above
+		// If rtl_alt_min is true then forcing altitude change even if above.
 		_rtl_state = RTL_STATE_CLIMB;
 
 	} else {
-		// otherwise go straight to return
+		// Otherwise go straight to return
 		_rtl_state = RTL_STATE_RETURN;
 	}
 
@@ -113,9 +116,9 @@ RTL::set_return_alt_min(bool min)
 void
 RTL::set_rtl_item()
 {
-	// RTL_TYPE: mission landing
+	// RTL_TYPE: mission landing.
-	// landing using planned mission landing, fly to DO_LAND_START instead of returning HOME
+	// Landing using planned mission landing, fly to DO_LAND_START instead of returning HOME.
-	// do nothing, let navigator takeover with mission landing
+	// Do nothing, let navigator takeover with mission landing.
 	if (rtl_type() == RTL_LAND) {
 		if (_rtl_state > RTL_STATE_CLIMB) {
 			if (_navigator->start_mission_landing()) {
@@ -123,7 +126,7 @@ RTL::set_rtl_item()
 				return;
 
 			} else {
-				// otherwise use regular RTL
+				// Otherwise use regular RTL.
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing");
 			}
 		}
@@ -136,18 +139,18 @@ RTL::set_rtl_item()
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	// check if we are pretty close to home already
+	// Check if we are pretty close to home already.
 	const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
 
-	// compute the return altitude
+	// Compute the return altitude.
 	float return_alt = max(home.alt + _param_return_alt.get(), gpos.alt);
 
-	// we are close to home, limit climb to min
+	// We are close to home, limit climb to min.
 	if (home_dist < _param_rtl_min_dist.get()) {
 		return_alt = home.alt + _param_descend_alt.get();
 	}
 
-	// compute the loiter altitude
+	// Compute the loiter altitude.
 	const float loiter_altitude = min(home.alt + _param_descend_alt.get(), gpos.alt);
 
 	switch (_rtl_state) {
@@ -171,20 +174,20 @@ RTL::set_rtl_item()
 
 	case RTL_STATE_RETURN: {
 
-			// don't change altitude
+			// Don't change altitude.
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.lat = home.lat;
 			_mission_item.lon = home.lon;
 			_mission_item.altitude = return_alt;
 			_mission_item.altitude_is_relative = false;
 
-			// use home yaw if close to home
+			// Use home yaw if close to home.
-			/* check if we are pretty close to home already */
+			// Check if we are pretty close to home already.
 			if (home_dist < _param_rtl_min_dist.get()) {
 				_mission_item.yaw = home.yaw;
 
 			} else {
-				// use current heading to home
+				// Use current heading to home.
 				_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, home.lat, home.lon);
 			}
 
@@ -211,7 +214,7 @@ RTL::set_rtl_item()
 			_mission_item.altitude = loiter_altitude;
 			_mission_item.altitude_is_relative = false;
 
-			// except for vtol which might be still off here and should point towards this location
+			// Except for vtol which might be still off here and should point towards this location.
 			const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
 
 			if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
@@ -226,7 +229,7 @@ RTL::set_rtl_item()
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
 
-			/* disable previous setpoint to prevent drift */
+			// Disable previous setpoint to prevent drift.
 			pos_sp_triplet->previous.valid = false;
 
 			mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
@@ -237,7 +240,7 @@ RTL::set_rtl_item()
 	case RTL_STATE_LOITER: {
 			const bool autoland = (_param_land_delay.get() > FLT_EPSILON);
 
-			// don't change altitude
+			// Don't change altitude.
 			_mission_item.lat = home.lat;
 			_mission_item.lon = home.lon;
 			_mission_item.altitude = loiter_altitude;
@@ -265,7 +268,7 @@ RTL::set_rtl_item()
 		}
 
 	case RTL_STATE_LAND: {
-			// land at home position
+			// Land at home position.
 			_mission_item.nav_cmd = NAV_CMD_LAND;
 			_mission_item.lat = home.lat;
 			_mission_item.lon = home.lon;
@@ -293,12 +296,12 @@ RTL::set_rtl_item()
 
 	reset_mission_item_reached();
 
-	/* execute command if set. This is required for commands like VTOL transition */
+	// Execute command if set. This is required for commands like VTOL transition.
 	if (!item_contains_position(_mission_item)) {
 		issue_command(_mission_item);
 	}
 
-	/* convert mission item to current position setpoint and make it valid */
+	// Convert mission item to current position setpoint and make it valid.
 	mission_apply_limitation(_mission_item);
 
 	if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
@@ -329,7 +332,7 @@ RTL::advance_rtl()
 
 	case RTL_STATE_DESCEND:
 
-		/* only go to land if autoland is enabled */
+		// Only go to land if autoland is enabled.
 		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
 			_rtl_state = RTL_STATE_LOITER;