Grouping of mission items

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

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:

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);

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<uint32_t>(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<uint32_t>(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;
+				}
+			}
+		}
+	}
+}

src/modules/navigator/mission.h

@@ -65,6 +65,8 @@
 #include <uORB/topics/vehicle_roi.h>
 #include <uORB/uORB.h>
 
+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<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
 		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,

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<uint32_t>(_mission_item.params[0]));
+		return true;
+
+	case NAV_CMD_GROUP_END:
+		_navigator->set_group_end(static_cast<uint32_t>(_mission_item.params[0]));
+		return true;
+
 	case NAV_CMD_DO_VTOL_TRANSITION:
 
 		if (int(_mission_item.params[0]) == 3) {

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),

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,

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(

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()
 {