various fixed for bugs encountered during testing

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/mission.cpp

@@ -403,8 +403,8 @@ Mission::find_mission_land_start()
 			_land_start_index = i;
 		}
 
-		if (_land_start_available && (missionitem.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
-			// use the loiter to altitude item of the mission landing as target landing location
+		if (_land_start_available && i > _land_start_index  && item_contains_position(missionitem)) {
+			// use the position of any waypoint after the land start marker which specifies a position.
 			_landing_lat = missionitem.lat;
 			_landing_lon = missionitem.lon;
 			_landing_alt = missionitem.altitude_is_relative ?	missionitem.altitude +

src/modules/navigator/navigator_main.cpp

@@ -564,10 +564,11 @@ Navigator::run()
 					}
 
 
-					if (!rtl_activated && _rtl.initialClimbDone() && get_mission_start_land_available()) {
+					if (!rtl_activated && _rtl.getClimbAndReturnDone() && get_mission_start_land_available()) {
 						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
 
-						if (!getMissionLandingInProgress()) {
+						if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED
+						    && !get_land_detected()->landed) {
 							start_mission_landing();
 						}
 

src/modules/navigator/rtl.cpp

@@ -231,8 +231,9 @@ void RTL::on_activation()
 		_rtl_state = RTL_STATE_LANDED;
 
 	} else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) {
-		// RTL straight to RETURN state, but mission will takeover for landing.
-		_rtl_state = RTL_STATE_RETURN;
+		// we were just on a mission landing, set _rtl_state past RTL_STATE_RETURN such that navigator will engage mission mode,
+		// which will continue executing the landing
+		_rtl_state = RTL_STATE_DESCEND;
 
 
 	} else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
@@ -246,7 +247,7 @@ void RTL::on_activation()
 		_rtl_state = RTL_STATE_RETURN;
 	}
 
-	setInitialClimbDone(_rtl_state != RTL_STATE_CLIMB);
+	setClimbAndReturnDone(_rtl_state > RTL_STATE_RETURN);
 
 	set_rtl_item();
 
@@ -273,7 +274,7 @@ void RTL::set_rtl_item()
 	// Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination.
 	// After reaching DO_LAND_START, do nothing, let navigator takeover with mission landing.
 	if (_destination.type == RTL_DESTINATION_MISSION_LANDING) {
-		if (_rtl_state > RTL_STATE_CLIMB) {
+		if (_rtl_state > RTL_STATE_RETURN) {
 			if (_navigator->start_mission_landing()) {
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing");
 				return;
@@ -486,11 +487,11 @@ void RTL::advance_rtl()
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB:
-		setInitialClimbDone(true);
 		_rtl_state = RTL_STATE_RETURN;
 		break;
 
 	case RTL_STATE_RETURN:
+		setClimbAndReturnDone(true);
 
 		if (vtol_in_fw_mode || descend_and_loiter) {
 			_rtl_state = RTL_STATE_DESCEND;

src/modules/navigator/rtl.h

@@ -83,9 +83,9 @@ public:
 
 	int rtl_destination();
 
-	void setInitialClimbDone(bool done) { _initial_climb_done = done; }
+	void setClimbAndReturnDone(bool done) { _climb_and_return_done = done; }
 
-	bool initialClimbDone() { return _initial_climb_done; }
+	bool getClimbAndReturnDone() { return _climb_and_return_done; }
 
 private:
 	/**
@@ -138,7 +138,7 @@ private:
 
 	float _rtl_alt{0.0f};	// AMSL altitude at which the vehicle should return to the home position
 	bool _rtl_alt_min{false};
-	bool _initial_climb_done{false};	// this flag is set to true if RTL is active and we are past the climb state
+	bool _climb_and_return_done{false};	// this flag is set to true if RTL is active and we are past the climb state and return state
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,