diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 545892f48e..1e463055b6 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -126,6 +126,7 @@ set(msg_files Mission.msg MissionResult.msg MountOrientation.msg + ModeCompleted.msg NavigatorMissionItem.msg NpfgStatus.msg ObstacleDistance.msg diff --git a/msg/ModeCompleted.msg b/msg/ModeCompleted.msg new file mode 100644 index 0000000000..98f331ed64 --- /dev/null +++ b/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 + diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 38fba5136a..ee2c7115f8 100644 --- a/src/modules/navigator/land.cpp +++ b/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(); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1a5a2d65b7..1442fe1f72 100644 --- a/src/modules/navigator/mission.cpp +++ b/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 diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dcff5643d5..9e0edc4e87 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -77,6 +77,7 @@ #include #include #include +#include #include 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_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_roi_pub{ORB_ID(vehicle_roi)}; + uORB::Publication _mode_completed_pub{ORB_ID(mode_completed)}; orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7d76bc44b6..31ff6a7d8a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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) { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 028ddc3be1..17e64090bb 100644 --- a/src/modules/navigator/rtl.cpp +++ b/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: diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 802756cb67..8c39a2eecc 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/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(); diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index faf2812d0e..73b8c46bb7 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/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; }