navigator: add ModeCompleted signalling topic

msg/CMakeLists.txt

@@ -126,6 +126,7 @@ set(msg_files
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg
+	ModeCompleted.msg
 	NavigatorMissionItem.msg
 	NpfgStatus.msg
 	ObstacleDistance.msg

msg/ModeCompleted.msg

@@ -0,0 +1,14 @@
+# Mode completion result, published by an active mode.
+# Note that this is not always published (e.g. when a user switches modes or on
+# failsafe activation)
+uint64 timestamp				 # time since system start (microseconds)
+
+
+uint8 RESULT_SUCCESS = 0
+# [1-99]: reserved
+uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error)
+
+uint8 result                     # One of RESULT_*
+
+uint8 nav_state                  # Source mode
+

src/modules/navigator/land.cpp

@@ -87,6 +87,7 @@ Land::on_active()
 	if (_navigator->get_land_detected()->landed) {
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
+		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND);
 		set_idle_item(&_mission_item);
 
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

src/modules/navigator/mission.cpp

@@ -735,6 +735,7 @@ Mission::set_mission_items()
 		// set mission finished
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
+		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
 
 		if (!user_feedback_done) {
 			/* only tell users that we got no mission if there has not been any

src/modules/navigator/navigator.h

@@ -77,6 +77,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/mode_completed.h>
 #include <uORB/uORB.h>
 
 using namespace time_literals;
@@ -323,6 +324,8 @@ public:
 	void 		calculate_breaking_stop(double &lat, double &lon, float &yaw);
 	void        	stop_capturing_images();
 
+	void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS);
+
 private:
 
 	struct traffic_buffer_s {
@@ -352,6 +355,7 @@ private:
 	uORB::Publication<vehicle_command_ack_s>	_vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_command_s>		_vehicle_cmd_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_roi_s>		_vehicle_roi_pub{ORB_ID(vehicle_roi)};
+	uORB::Publication<mode_completed_s> _mode_completed_pub{ORB_ID(mode_completed)};
 
 	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */
 

src/modules/navigator/navigator_main.cpp

@@ -1697,6 +1697,15 @@ void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
 	yaw = get_local_position()->heading;
 }
 
+void Navigator::mode_completed(uint8_t nav_state, uint8_t result)
+{
+	mode_completed_s mode_completed{};
+	mode_completed.timestamp = hrt_absolute_time();
+	mode_completed.result = result;
+	mode_completed.nav_state = nav_state;
+	_mode_completed_pub.publish(mode_completed);
+}
+
 int Navigator::print_usage(const char *reason)
 {
 	if (reason) {

src/modules/navigator/rtl.cpp

@@ -668,6 +668,7 @@ void RTL::advance_rtl()
 
 	case RTL_STATE_LAND:
 		_rtl_state = RTL_STATE_LANDED;
+		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
 		break;
 
 	default:

src/modules/navigator/takeoff.cpp

@@ -65,6 +65,7 @@ Takeoff::on_active()
 	} else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) {
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
+		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF);
 
 		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 

src/modules/navigator/vtol_takeoff.cpp

@@ -149,9 +149,10 @@ VtolTakeoff::on_active()
 				_navigator->reset_position_setpoint(reposition_triplet->current);
 				_navigator->reset_position_setpoint(reposition_triplet->next);
 
-				// the VTOL takeoff is done, proceed loitering and update the navigation state to LOITER
+				// the VTOL takeoff is done
 				_navigator->get_mission_result()->finished = true;
 				_navigator->set_mission_result_updated();
+				_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);
 
 				break;
 			}