From 46ef5172131da411d9cbc3a633403f69b8e9cbc7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Matej=20Fran=C4=8De=C5=A1kin?= Date: Fri, 1 Oct 2021 13:51:25 +0200 Subject: [PATCH] Grouping of mission items --- msg/mission_result.msg | 5 ++ src/modules/mavlink/mavlink_mission.cpp | 48 ++++++++++ src/modules/mavlink/mavlink_mission.h | 3 + src/modules/navigator/mission.cpp | 89 ++++++++++++++++++- src/modules/navigator/mission.h | 4 + src/modules/navigator/mission_block.cpp | 8 ++ .../navigator/mission_feasibility_checker.cpp | 4 +- src/modules/navigator/navigation.h | 2 + src/modules/navigator/navigator.h | 2 + src/modules/navigator/navigator_main.cpp | 36 ++++++++ 10 files changed, 199 insertions(+), 2 deletions(-) diff --git a/msg/mission_result.msg b/msg/mission_result.msg index c1ad449a23..49719fd148 100644 --- a/msg/mission_result.msg +++ b/msg/mission_result.msg @@ -21,4 +21,9 @@ bool item_do_jump_changed # true if the number of do jumps remaining has changed uint16 item_changed_index # indicate which item has changed uint16 item_do_jump_remaining # set to the number of do jumps remaining for that item +uint32[3] groups_started # mission item group ids to be started +uint8 groups_started_num # number of items in groups_started +uint32[3] groups_ended # mission item group ids to be ended +uint8 groups_ended_num # number of items in groups_ended + uint8 execution_mode # indicates the mode in which the mission is executed diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 473e0f7bd5..ed58d8d1f0 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -474,6 +474,32 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) PX4_DEBUG("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } +void +MavlinkMissionManager::send_group_start(uint32_t group_id, uint64_t timestamp) +{ + mavlink_group_start_t group_start{}; + + group_start.group_id = group_id; + group_start.time_usec = timestamp; + + mavlink_msg_group_start_send_struct(_mavlink->get_channel(), &group_start); + + PX4_DEBUG("WPM: Send GROUP_START id %u", group_start.group_id); +} + +void +MavlinkMissionManager::send_group_end(uint32_t group_id, uint64_t timestamp) +{ + mavlink_group_end_t group_end{}; + + group_end.group_id = group_id; + group_end.time_usec = timestamp; + + mavlink_msg_group_end_send_struct(_mavlink->get_channel(), &group_end); + + PX4_DEBUG("WPM: Send GROUP_END id %u", group_end.group_id); +} + void MavlinkMissionManager::send() { @@ -515,6 +541,14 @@ MavlinkMissionManager::send() } } + for (uint8_t i = 0; i < mission_result.groups_ended_num; i++) { + send_group_end(mission_result.groups_ended[i], mission_result.timestamp); + } + + for (uint8_t i = 0; i < mission_result.groups_started_num; i++) { + send_group_start(mission_result.groups_started[i], mission_result.timestamp); + } + } else { if (_slow_rate_limiter.check(hrt_absolute_time())) { send_mission_current(_current_seq); @@ -1459,6 +1493,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; break; + case MAV_CMD_GROUP_START: + case MAV_CMD_GROUP_END: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->params[0] = mavlink_mission_item->param1; + break; + default: mission_item->nav_cmd = NAV_CMD_INVALID; @@ -1546,6 +1586,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; break; + case MAV_CMD_GROUP_START: + case MAV_CMD_GROUP_END: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->params[0] = mavlink_mission_item->param1; + break; + default: mission_item->nav_cmd = NAV_CMD_INVALID; @@ -1635,6 +1681,8 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_SET_CAMERA_ZOOM: case NAV_CMD_SET_CAMERA_FOCUS: case NAV_CMD_DO_VTOL_TRANSITION: + case MAV_CMD_GROUP_START: + case MAV_CMD_GROUP_END: break; default: diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 6740f0c697..2d7385eef2 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -204,6 +204,9 @@ private: */ void send_mission_item_reached(uint16_t seq); + void send_group_start(uint32_t group_id, uint64_t timestamp); + void send_group_end(uint32_t group_id, uint64_t timestamp); + void handle_mission_ack(const mavlink_message_t *msg); void handle_mission_set_current(const mavlink_message_t *msg); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index aefc407392..4387cac09d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -492,6 +492,10 @@ Mission::update_mission() const mission_s old_mission = _mission; + uint32_t old_started_groups[max_started_groups]; + size_t old_started_groups_num; + get_started_groups(_current_mission_index, old_started_groups, old_started_groups_num); + if (_mission_sub.copy(&_mission)) { /* determine current index */ if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { @@ -555,6 +559,42 @@ Mission::update_mission() find_mission_land_start(); set_current_mission_item(); + + uint32_t new_started_groups[max_started_groups]; + size_t new_started_groups_num; + get_started_groups(_current_mission_index, new_started_groups, new_started_groups_num); + + // End all previously started groups that are not started anymore + for (size_t i = 0; i < old_started_groups_num; i++) { + bool found = false; + + for (size_t j = 0; j < new_started_groups_num; j++) { + if (old_started_groups[i] == new_started_groups[j]) { + found = true; + break; + } + } + + if (!found) { + _navigator->set_group_end(old_started_groups[i]); + } + } + + // Start all groups that were not started before + for (size_t i = 0; i < new_started_groups_num; i++) { + bool found = false; + + for (size_t j = 0; j < old_started_groups_num; j++) { + if (new_started_groups[i] == old_started_groups[j]) { + found = true; + break; + } + } + + if (!found) { + _navigator->set_group_start(new_started_groups[i]); + } + } } @@ -1929,7 +1969,6 @@ void Mission::publish_navigator_mission_item() _navigator_mission_item_pub.publish(navigator_mission_item); } - void Mission::calculate_mission_checksums() { union { @@ -2092,3 +2131,51 @@ void Mission::calculate_mission_checksums() mission_checksum_pub.publish(csum); } } + +void +Mission::get_started_groups(size_t current_item, uint32_t started_groups[], size_t &num) +{ + dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); + num = 0; + + for (size_t i = 0; i < current_item; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(missionitem); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + PX4_ERR("dataman read failure"); + break; + } + + if (missionitem.nav_cmd == NAV_CMD_GROUP_START) { + uint32_t group_id = static_cast(missionitem.params[0]); + bool found = false; + + for (size_t n = 0; n < num; n++) { + if (started_groups[n] == group_id) { + found = true; + break; + } + } + + if (!found && num < max_started_groups) { + started_groups[num++] = group_id; + } + + } else if (missionitem.nav_cmd == NAV_CMD_GROUP_END) { + uint32_t group_id = static_cast(missionitem.params[0]); + + for (size_t n = 0; n < num; n++) { + if (started_groups[n] == group_id) { + for (size_t m = n + 1; m < num; m++) { + started_groups[m - 1] = started_groups[m]; + } + + num--; + break; + } + } + } + } +} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6b4fd9f4cf..1b760fcb03 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -65,6 +65,8 @@ #include #include +const int max_started_groups = 3; + class Navigator; class Mission : public MissionBlock, public ModuleParams @@ -240,6 +242,8 @@ private: void calculate_mission_checksums(); + void get_started_groups(size_t current_item, uint32_t started_groups[], size_t &num); + DEFINE_PARAMETERS( (ParamFloat) _param_mis_dist_1wp, (ParamFloat) _param_mis_dist_wps, diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 6d6ba8da38..13a23113d5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -110,6 +110,14 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_SET_CAMERA_FOCUS: return true; + case NAV_CMD_GROUP_START: + _navigator->set_group_start(static_cast(_mission_item.params[0])); + return true; + + case NAV_CMD_GROUP_END: + _navigator->set_group_end(static_cast(_mission_item.params[0])); + return true; + case NAV_CMD_DO_VTOL_TRANSITION: if (int(_mission_item.params[0]) == 3) { diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 2df3f922d8..e8b8c90e05 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -278,7 +278,9 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && - missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { + missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION && + missionitem.nav_cmd != NAV_CMD_GROUP_START && + missionitem.nav_cmd != NAV_CMD_GROUP_END) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d\t", (int)(i + 1), diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 5c6251d603..4386ff9b51 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -84,6 +84,8 @@ enum NAV_CMD { NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214, NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206, NAV_CMD_OBLIQUE_SURVEY = 260, + NAV_CMD_GROUP_START = 301, + NAV_CMD_GROUP_END = 302, NAV_CMD_SET_CAMERA_MODE = 530, NAV_CMD_SET_CAMERA_ZOOM = 531, NAV_CMD_SET_CAMERA_FOCUS = 532, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index f585275b24..e028e11782 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -307,6 +307,8 @@ public: void acquire_gimbal_control(); void release_gimbal_control(); + void set_group_start(uint32_t id); + void set_group_end(uint32_t id); private: DEFINE_PARAMETERS( diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cdece459de..a62adc792d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1463,12 +1463,48 @@ Navigator::publish_mission_result() /* reset some of the flags */ _mission_result.item_do_jump_changed = false; + _mission_result.groups_started_num = 0; + _mission_result.groups_ended_num = 0; _mission_result.item_changed_index = 0; _mission_result.item_do_jump_remaining = 0; _mission_result_updated = false; } +void +Navigator::set_group_start(uint32_t id) +{ + if (id == 0 || _mission_result.groups_started_num >= max_started_groups) { + return; + } + + for (uint8_t i = 0; i < _mission_result.groups_started_num; i++) { + if (_mission_result.groups_started[_mission_result.groups_started_num] == id) { + return; + } + } + + _mission_result.groups_started[_mission_result.groups_started_num++] = id; + _mission_result_updated = true; +} + +void +Navigator::set_group_end(uint32_t id) +{ + if (id == 0 || _mission_result.groups_ended_num >= max_started_groups) { + return; + } + + for (uint8_t i = 0; i < _mission_result.groups_ended_num; i++) { + if (_mission_result.groups_ended[_mission_result.groups_ended_num] == id) { + return; + } + } + + _mission_result.groups_ended[_mission_result.groups_ended_num++] = id; + _mission_result_updated = true; +} + void Navigator::set_mission_failure_heading_timeout() {