mission: publish sub flight mode for flightmode manager

msg/NavigatorMissionItem.msg

@@ -6,6 +6,11 @@ uint16 sequence_current          # Sequence of the current mission item
 
 uint16 nav_cmd
 
+uint8 MISSION_SUBMODE_NONE = 0
+uint8 MISSION_SUBMODE_PRECLAND = 1
+
+uint8 nav_sub_cmd	          # sub-command, for example AUTO_PRECLAND in main command AUTO_MISSION
+
 float32 latitude
 float32 longitude
 

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -107,6 +107,7 @@ void FlightModeManager::Run()
 		_vehicle_control_mode_sub.update();
 		_vehicle_land_detected_sub.update();
 		_vehicle_status_sub.update();
+		_navigator_mission_item_sub.update();
 
 		start_flight_task();
 
@@ -139,6 +140,14 @@ void FlightModeManager::start_flight_task()
 	bool task_failure = false;
 	bool should_disable_task = true;
 
+	const bool land_should_be_precland = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
+					      ||
+					      _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)
+					     && _param_rtl_pld_md.get() > 0;
+	const bool precland_mission_item_active = _vehicle_status_sub.get().nav_state ==
+			vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION &&
+			_navigator_mission_item_sub.get().nav_sub_cmd == navigator_mission_item_s::MISSION_SUBMODE_PRECLAND;
+
 	// Do not run any flight task for VTOLs in fixed-wing mode
 	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 		switchTask(FlightTaskIndex::None);
@@ -164,11 +173,8 @@ void FlightModeManager::start_flight_task()
 			task_failure = true;
 		}
 
+	} else if (land_should_be_precland || precland_mission_item_active) {
 		// Take-over landing from navigator if precision landing is enabled
-
-	} else if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL ||
-		    _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)
-		   && _param_rtl_pld_md.get() > 0) {
 		should_disable_task = false;
 
 		if (switchTask(FlightTaskIndex::AutoPrecisionLanding) != FlightTaskError::NoError) {

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -44,6 +44,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/topics/navigator_mission_item.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/takeoff_status.h>
@@ -150,6 +151,7 @@ private:
 	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
 
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionData<navigator_mission_item_s> _navigator_mission_item_sub{ORB_ID(navigator_mission_item)};
 
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};

src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp

@@ -121,20 +121,24 @@ bool FlightTaskAutoPrecisionLanding::update()
 void
 FlightTaskAutoPrecisionLanding::run_state_start()
 {
-	_position_setpoint = _target; // Follow navigator triplet
+	// In this state simply track the navigator setpoint triplet
+	// This ensures that the behaviour for precision landing during RTL / LAND
+	// is the same as without precision landing.
+	// This flight task will generate independent setpoints once the vehicle
+	// is in the vertical landing phase.
+	_position_setpoint = _target;
 
-	// check if target visible and go to horizontal approach directly
 	if (switch_to_state_horizontal_approach()) {
+		// If target visible and go to horizontal approach directly
 		return;
 
 	} else if ((PrecLandMode)_param_rtl_pld_md.get() == PrecLandMode::Opportunistic) {
-
 		// could not see the target immediately, so just fall back to normal landing
 		switch_to_state_fallback();
 
 	} else if (_type == WaypointType::land) {
-
 		// Navigator already entered land stage. Take over with precision landing
+		// This is where the vehicle starts to behave differently than in regular land!
 		switch_to_state_search();
 	}
 }

src/modules/navigator/mission.cpp

@@ -746,7 +746,7 @@ Mission::set_mission_items()
 			user_feedback_done = true;
 		}
 
-		publish_navigator_mission_item(); // for logging
+		publish_navigator_mission_item();
 		_navigator->set_position_setpoint_triplet_updated();
 
 		return;
@@ -967,6 +967,20 @@ Mission::set_mission_items()
 					// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
 					pos_sp_triplet->previous.valid = false;
 
+				} else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+					if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
+						new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
+					}
+				}
+
+				/* we just moved to the landing waypoint, now descend */
+				if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND &&
+				    new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+
+					if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
+						new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
+					}
+
 				}
 
 				/* ignore yaw for landing items */
@@ -1100,8 +1114,10 @@ Mission::set_mission_items()
 	} else {
 		// The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items)
 		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
-		mission_apply_limitation(_mission_item);
+		if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
-		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+			mission_apply_limitation(_mission_item);
+			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+		}
 
 		// ELSE: The current position setpoint stays unchanged.
 	}
@@ -1119,6 +1135,13 @@ Mission::set_mission_items()
 	/* set current work item type */
 	_work_item_type = new_work_item_type;
 
+	if (new_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
+		_mission_sub_mode = navigator_mission_item_s::MISSION_SUBMODE_PRECLAND;
+
+	} else {
+		_mission_sub_mode = navigator_mission_item_s::MISSION_SUBMODE_NONE;
+	}
+
 	/* require takeoff after landing or idle */
 	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
 	    || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
@@ -1171,7 +1194,7 @@ Mission::set_mission_items()
 		}
 	}
 
-	publish_navigator_mission_item(); // for logging
+	publish_navigator_mission_item();
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -1418,7 +1441,7 @@ Mission::do_abort_landing()
 	_navigator->get_position_setpoint_triplet()->next.lon = (double)NAN;
 	_navigator->get_position_setpoint_triplet()->next.alt = NAN;
 
-	publish_navigator_mission_item(); // for logging
+	publish_navigator_mission_item();
 	_navigator->set_position_setpoint_triplet_updated();
 
 	mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t",
@@ -1862,6 +1885,7 @@ void Mission::publish_navigator_mission_item()
 	navigator_mission_item.instance_count = _navigator->mission_instance_count();
 	navigator_mission_item.sequence_current = _current_mission_index;
 	navigator_mission_item.nav_cmd = _mission_item.nav_cmd;
+	navigator_mission_item.nav_sub_cmd = _mission_sub_mode;
 	navigator_mission_item.latitude = _mission_item.lat;
 	navigator_mission_item.longitude = _mission_item.lon;
 	navigator_mission_item.altitude = _mission_item.altitude;

src/modules/navigator/mission.h

@@ -288,9 +288,12 @@ private:
 		WORK_ITEM_TYPE_MOVE_TO_LAND,	/**< move to land waypoint before descent */
 		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
 		WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
-		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION
+		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
+		WORK_ITEM_TYPE_PRECISION_LAND
 	} _work_item_type{WORK_ITEM_TYPE_DEFAULT};	/**< current type of work to do (sub mission item) */
 
 	uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL};	/**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
 	bool _execution_mode_changed{false};
+
+	uint8_t _mission_sub_mode{navigator_mission_item_s::MISSION_SUBMODE_NONE};
 };