RTL optionally use planned mission landing (#8487)

- adds new RTL_LAND_TYPE parameter

src/modules/navigator/mission.cpp

@@ -123,6 +123,9 @@ Mission::on_inactive()
 			_offboard_mission.dataman_id = mission_state.dataman_id;
 			_offboard_mission.count = mission_state.count;
 			_current_offboard_mission_index = mission_state.current_seq;
+
+			// find and store landing start marker (if available)
+			find_offboard_land_start();
 		}
 
 		/* On init let's check the mission, maybe there is already one available. */
@@ -251,18 +254,19 @@ Mission::on_active()
 }
 
 bool
-Mission::set_current_offboard_mission_index(unsigned index)
+Mission::set_current_offboard_mission_index(uint16_t index)
 {
-	if (index < _offboard_mission.count) {
+	if (_navigator->get_mission_result()->valid &&
+	    (index != _current_offboard_mission_index) && (index < _offboard_mission.count)) {
 
 		_current_offboard_mission_index = index;
-		set_current_offboard_mission_item();
 
 		// update mission items if already in active mission
 		if (_navigator->is_planned_mission()) {
 			// prevent following "previous - current" line
 			_navigator->get_position_setpoint_triplet()->previous.valid = false;
 			_navigator->get_position_setpoint_triplet()->current.valid = false;
+			_navigator->get_position_setpoint_triplet()->next.valid = false;
 			set_mission_items();
 		}
 
@@ -272,11 +276,11 @@ Mission::set_current_offboard_mission_index(unsigned index)
 	return false;
 }
 
-int
+bool
 Mission::find_offboard_land_start()
 {
-	/* find the first MAV_CMD_DO_LAND_START and return the index
+	/* return true if a MAV_CMD_DO_LAND_START is found and internally save the index
-	 *  return -1 if not found
+	 *  return false if not found
 	 *
 	 * TODO: implement full spec and find closest landing point geographically
 	 */
@@ -289,15 +293,48 @@ Mission::find_offboard_land_start()
 
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return -1;
+			PX4_ERR("dataman read failure");
+			break;
 		}
 
 		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
-			return i;
+			_land_start_available = true;
+			_land_start_index = i;
+			return true;
 		}
 	}
 
-	return -1;
+	_land_start_available = false;
+	return false;
+}
+
+bool
+Mission::land_start()
+{
+	// if not currently landing, jump to do_land_start
+	if (_land_start_available) {
+		if (landing()) {
+			return true;
+
+		} else {
+			set_current_offboard_mission_index(get_land_start_index());
+			return landing();
+		}
+	}
+
+	return false;
+}
+
+bool
+Mission::landing()
+{
+	// vehicle is currently landing if
+	//  mission valid, still flying, and in the landing portion of mission
+
+	const bool mission_valid = _navigator->get_mission_result()->valid;
+	const bool on_landing_stage = _land_start_available && (_current_offboard_mission_index >= get_land_start_index());
+
+	return mission_valid && on_landing_stage;
 }
 
 void
@@ -405,6 +442,9 @@ Mission::update_offboard_mission()
 		PX4_ERR("mission check failed");
 	}
 
+	// find and store landing start marker (if available)
+	find_offboard_land_start();
+
 	set_current_offboard_mission_item();
 }
 
@@ -1128,10 +1168,8 @@ Mission::altitude_sp_foh_update()
 		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
 	}
 
-
 	// we set altitude directly so we can run this in parallel to the heading update
 	_navigator->set_position_setpoint_triplet_updated();
-
 }
 
 void
@@ -1192,10 +1230,8 @@ Mission::do_abort_landing()
 				     (int)(alt_sp - alt_landing));
 
 	// reset mission index to start of landing
-	int land_start_index = find_offboard_land_start();
+	if (_land_start_available) {
-
+		_current_offboard_mission_index = get_land_start_index();
-	if (land_start_index != -1) {
-		_current_offboard_mission_index = land_start_index;
 
 	} else {
 		// move mission index back (landing approach point)
@@ -1437,12 +1473,15 @@ Mission::check_mission_valid(bool force)
 			_missionFeasibilityChecker.checkMissionFeasible(_offboard_mission,
 					_param_dist_1wp.get(),
 					_param_dist_between_wps.get(),
-					false);
+					_navigator->mission_landing_required());
 
 		_navigator->get_mission_result()->seq_total = _offboard_mission.count;
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
 		_home_inited = _navigator->home_position_valid();
+
+		// find and store landing start marker (if available)
+		find_offboard_land_start();
 	}
 }
 

src/modules/navigator/mission.h

@@ -91,9 +91,12 @@ public:
 		MISSION_YAWMODE_MAX = 5
 	};
 
-	bool set_current_offboard_mission_index(unsigned index);
+	bool set_current_offboard_mission_index(uint16_t index);
 
-	int find_offboard_land_start();
+	bool land_start();
+	bool landing();
+
+	uint16_t get_land_start_index() const { return _land_start_index; }
 
 private:
 	/**
@@ -236,6 +239,11 @@ private:
 	 */
 	void generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw);
 
+	/**
+	 * Find and store the index of the landing sequence (DO_LAND_START)
+	 */
+	bool find_offboard_land_start();
+
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
@@ -247,8 +255,13 @@ private:
 	struct mission_s _onboard_mission {};
 	struct mission_s _offboard_mission {};
 
-	int _current_onboard_mission_index{-1};
+	int32_t _current_onboard_mission_index{-1};
-	int _current_offboard_mission_index{-1};
+	int32_t _current_offboard_mission_index{-1};
+
+	// track location of planned mission landing
+	bool	_land_start_available{false};
+	uint16_t _land_start_index{UINT16_MAX};		/**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */
+
 	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 
 	enum {

src/modules/navigator/mission_block.cpp

@@ -664,22 +664,6 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 	item->origin = ORIGIN_ONBOARD;
 }
 
-void
-MissionBlock::set_current_position_item(struct mission_item_s *item)
-{
-	item->nav_cmd = NAV_CMD_WAYPOINT;
-	item->lat = _navigator->get_global_position()->lat;
-	item->lon = _navigator->get_global_position()->lon;
-	item->altitude_is_relative = false;
-	item->altitude = _navigator->get_global_position()->alt;
-	item->yaw = NAN;
-	item->loiter_radius = _navigator->get_loiter_radius();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
-	item->time_inside = 0.0f;
-	item->autocontinue = true;
-	item->origin = ORIGIN_ONBOARD;
-}
-
 void
 MissionBlock::set_idle_item(struct mission_item_s *item)
 {

src/modules/navigator/mission_block.h

@@ -106,8 +106,6 @@ protected:
 	 */
 	void set_land_item(struct mission_item_s *item, bool at_current_location);
 
-	void set_current_position_item(struct mission_item_s *item);
-
 	/**
 	 * Set idle mission item
 	 */

src/modules/navigator/navigator.h

@@ -238,12 +238,18 @@ public:
 
 	void 		set_mission_failure(const char *reason);
 
-	bool		is_planned_mission() { return _navigation_mode == &_mission; }
+	// MISSION
+	bool		is_planned_mission() const { return _navigation_mode == &_mission; }
+	bool		on_mission_landing() { return _mission.landing(); }
+	bool		start_mission_landing() { return _mission.land_start(); }
+
+	// RTL
+	bool		mission_landing_required() { return _rtl.mission_landing_required(); }
 
 	bool		abort_landing();
 
+	// Param access
 	float		get_loiter_min_alt() const { return _param_loiter_min_alt.get(); }
-
 	bool		force_vtol() const { return _vstatus.is_vtol && !_vstatus.is_rotary_wing && _param_force_vtol.get(); }
 
 private:
@@ -261,9 +267,9 @@ private:
 	int		_onboard_mission_sub{-1};	/**< onboard mission subscription */
 	int		_param_update_sub{-1};		/**< param update subscription */
 	int		_sensor_combined_sub{-1};	/**< sensor combined subscription */
+	int		_traffic_sub{-1};		/**< traffic subscription */
 	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
-	int		_vstatus_sub{-1};			/**< vehicle status subscription */
+	int		_vstatus_sub{-1};		/**< vehicle status subscription */
-	int		_traffic_sub{-1};			/**< traffic subscription */
 
 	orb_advert_t	_geofence_result_pub{nullptr};
 	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */

src/modules/navigator/navigator_main.cpp

@@ -476,25 +476,23 @@ Navigator::task_main()
 				/* find NAV_CMD_DO_LAND_START in the mission and
 				 * use MAV_CMD_MISSION_START to start the mission there
 				 */
-				int land_start = _mission.find_offboard_land_start();
+				if (_mission.land_start()) {
-
-				if (land_start != -1) {
 					vehicle_command_s vcmd = {};
 					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
-					vcmd.param1 = land_start;
+					vcmd.param1 = _mission.get_land_start_index();
 					publish_vehicle_cmd(&vcmd);
 
 				} else {
-					PX4_WARN("planned landing not available");
+					PX4_WARN("planned mission landing not available");
 				}
 
 				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
-				if (_mission_result.valid &&
+				if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) {
-				    PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
+					if (!_mission.set_current_offboard_mission_index(cmd.param1)) {
-
+						PX4_WARN("CMD_MISSION_START failed");
-					_mission.set_current_offboard_mission_index(cmd.param1);
+					}
 				}
 
 				// CMD_MISSION_START is acknowledged by commander
@@ -625,7 +623,15 @@ Navigator::task_main()
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 			_pos_sp_triplet_published_invalid_once = false;
-			navigation_mode_new = &_rtl;
+
+			// if RTL is set to use a mission landing and mission has a planned landing, then use MISSION
+			if (mission_landing_required() && on_mission_landing()) {
+				navigation_mode_new = &_mission;
+
+			} else {
+				navigation_mode_new = &_rtl;
+			}
+
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:

src/modules/navigator/rtl.cpp

@@ -55,7 +55,8 @@ RTL::RTL(Navigator *navigator, const char *name) :
 	_param_return_alt(this, "RTL_RETURN_ALT", false),
 	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
 	_param_land_delay(this, "RTL_LAND_DELAY", false),
-	_param_rtl_min_dist(this, "RTL_MIN_DIST", false)
+	_param_rtl_min_dist(this, "RTL_MIN_DIST", false),
+	_param_rtl_land_type(this, "RTL_LAND_TYPE", false)
 {
 }
 
@@ -66,6 +67,13 @@ RTL::on_inactive()
 	_rtl_state = RTL_STATE_NONE;
 }
 
+bool
+RTL::mission_landing_required()
+{
+	// returns true if navigator should use planned mission landing
+	return (_param_rtl_land_type.get() == 1);
+}
+
 void
 RTL::on_activation()
 {
@@ -73,6 +81,10 @@ RTL::on_activation()
 		// for safety reasons don't go into RTL if landed
 		_rtl_state = RTL_STATE_LANDED;
 
+	} else if (mission_landing_required() && _navigator->on_mission_landing()) {
+		// RTL straight to RETURN state, but mission will takeover for landing
+		_rtl_state = RTL_STATE_RETURN;
+
 	} else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get())
 		   || _rtl_alt_min) {
 
@@ -106,6 +118,22 @@ RTL::set_return_alt_min(bool min)
 void
 RTL::set_rtl_item()
 {
+	// RTL_TYPE: mission landing
+	// landing using planned mission landing, fly to DO_LAND_START instead of returning HOME
+	// do nothing, let navigator takeover with mission landing
+	if (mission_landing_required()) {
+		if (_rtl_state > RTL_STATE_CLIMB) {
+			if (_navigator->start_mission_landing()) {
+				mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing");
+				return;
+
+			} else {
+				// otherwise use regular RTL
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing");
+			}
+		}
+	}
+
 	_navigator->set_can_loiter_at_sp(false);
 
 	const home_position_s &home = *_navigator->get_home_position();

src/modules/navigator/rtl.h

@@ -58,6 +58,8 @@ public:
 
 	void set_return_alt_min(bool min);
 
+	bool mission_landing_required();
+
 private:
 	/**
 	 * Set the RTL item
@@ -86,6 +88,7 @@ private:
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
 	control::BlockParamFloat _param_rtl_min_dist;
+	control::BlockParamInt _param_rtl_land_type;
 };
 
 #endif

src/modules/navigator/rtl_params.c

@@ -103,3 +103,14 @@ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
  * @group Return To Land
  */
 PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f);
+
+/**
+ * RTL land location
+ *
+ * Land at the home location or planned mission landing
+ *
+ * @value 0 Home Position
+ * @value 1 Planned Landing (Mission)
+ * @group Return To Land
+ */
+PARAM_DEFINE_INT32(RTL_LAND_TYPE, 0);