Add mission checksum to group messages

Store all command parameters that are defined in mavlink

This makes checksum calculation possible on both sides.

msg/CMakeLists.txt

@@ -107,6 +107,7 @@ set(msg_files
 	manual_control_switches.msg
 	mavlink_log.msg
 	mission.msg
+	mission_checksum.msg
 	mission_result.msg
 	mount_orientation.msg
 	navigator_mission_item.msg

msg/mission_checksum.msg

@@ -0,0 +1,5 @@
+uint64 timestamp	# time since system start (microseconds)
+uint32 mission_checksum	# main mission checksum
+uint32 fence_checksum	# geo fence checksum
+uint32 rally_checksum	# rally points checksum
+uint32 all_checksum	# checksum of all mission types

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_messages.cpp

@@ -90,6 +90,7 @@
 #include "streams/LOCAL_POSITION_NED.hpp"
 #include "streams/MAG_CAL_REPORT.hpp"
 #include "streams/MANUAL_CONTROL.hpp"
+#include "streams/MISSION_CHECKSUM.hpp"
 #include "streams/MOUNT_ORIENTATION.hpp"
 #include "streams/NAV_CONTROLLER_OUTPUT.hpp"
 #include "streams/OBSTACLE_DISTANCE.hpp"
@@ -546,8 +547,11 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamEfiStatus>(),
 #endif // EFI_STATUS_HPP
 #if defined(GPS_RTCM_DATA_HPP)
-	create_stream_list_item<MavlinkStreamGPSRTCMData>()
+	create_stream_list_item<MavlinkStreamGPSRTCMData>(),
 #endif // GPS_RTCM_DATA_HPP
+#if defined(MISSION_CHECKSUM_HPP)
+	create_stream_list_item<MavlinkStreamMissionChecksum>()
+#endif // MISSION_CHECKSUM_HPP
 };
 
 const char *get_stream_name(const uint16_t msg_id)

src/modules/mavlink/mavlink_mission.cpp

@@ -52,6 +52,7 @@
 #include <matrix/math.hpp>
 #include <navigator/navigation.h>
 #include <uORB/topics/mission.h>
+#include <uORB/topics/mission_checksum.h>
 #include <uORB/topics/mission_result.h>
 
 using matrix::wrap_2pi;
@@ -474,6 +475,34 @@ 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, uint32_t mission_checksum)
+{
+	mavlink_group_start_t group_start{};
+
+	group_start.group_id = group_id;
+	group_start.time_usec = timestamp;
+	group_start.mission_checksum = mission_checksum;
+
+	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, uint32_t mission_checksum)
+{
+	mavlink_group_end_t group_end{};
+
+	group_end.group_id = group_id;
+	group_end.time_usec = timestamp;
+	group_end.mission_checksum = mission_checksum;
+
+	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 +544,23 @@ MavlinkMissionManager::send()
 			}
 		}
 
+		uORB::Subscription csum_sub{ORB_ID(mission_checksum)};
+		mission_checksum_s csum{};
+
+		uint32_t checksum = 0;
+
+		if (csum_sub.advertised() && csum_sub.copy(&csum)) {
+			checksum = csum.all_checksum;
+		}
+
+		for (uint8_t i = 0; i < mission_result.groups_ended_num; i++) {
+			send_group_end(mission_result.groups_ended[i], mission_result.timestamp, checksum);
+		}
+
+		for (uint8_t i = 0; i < mission_result.groups_started_num; i++) {
+			send_group_start(mission_result.groups_started[i], mission_result.timestamp, checksum);
+		}
+
 	} else {
 		if (_slow_rate_limiter.check(hrt_absolute_time())) {
 			send_mission_current(_current_seq);
@@ -1389,8 +1435,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 
 		case MAV_CMD_NAV_TAKEOFF:
 			mission_item->nav_cmd = NAV_CMD_TAKEOFF;
+			mission_item->params[0] = mavlink_mission_item->param1;  // Pitch
 			mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
-
 			break;
 
 		case MAV_CMD_NAV_LOITER_TO_ALT:
@@ -1424,8 +1470,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			break;
 
 		case MAV_CMD_NAV_VTOL_TAKEOFF:
+			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
+			break;
+
 		case MAV_CMD_NAV_VTOL_LAND:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->params[0] = mavlink_mission_item->param1;  // Land options
+			mission_item->params[2] = mavlink_mission_item->param3;  // Approach altitude
 			mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
 			break;
 
@@ -1453,6 +1505,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;
 
@@ -1540,6 +1598,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;
 
@@ -1629,6 +1693,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, uint32_t mission_checksum);
+	void send_group_end(uint32_t group_id, uint64_t timestamp, uint32_t mission_checksum);
+
 	void handle_mission_ack(const mavlink_message_t *msg);
 
 	void handle_mission_set_current(const mavlink_message_t *msg);

src/modules/mavlink/streams/MISSION_CHECKSUM.hpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef MISSION_CHECKSUM_HPP
+#define MISSION_CHECKSUM_HPP
+
+#include <uORB/topics/mission_checksum.h>
+
+class MavlinkStreamMissionChecksum : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamMissionChecksum(mavlink); }
+
+	static constexpr const char *get_name_static() { return "MISSION_CHECKSUM"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_MISSION_CHECKSUM; }
+
+	const char *get_name() const override { return MavlinkStreamMissionChecksum::get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return _mission_checksum_sub.advertised() ? (MAVLINK_MSG_ID_MISSION_CHECKSUM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+	}
+
+	bool request_message(float param2, float param3, float param4, float param5, float param6, float param7) override
+	{
+		return send(static_cast<uint8_t>(param2));
+	}
+
+private:
+	explicit MavlinkStreamMissionChecksum(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _mission_checksum_sub{ORB_ID(mission_checksum)};
+
+	bool send() override
+	{
+		return send(MAV_MISSION_TYPE_ALL);
+	}
+
+	bool send(uint8_t mission_type)
+	{
+		mission_checksum_s csum;
+
+		if (_mission_checksum_sub.advertised() && _mission_checksum_sub.copy(&csum)) {
+			mavlink_mission_checksum_t msg{};
+
+			if (mission_type == MAV_MISSION_TYPE_MISSION) {
+				msg.checksum = csum.mission_checksum;
+
+			} else if (mission_type == MAV_MISSION_TYPE_FENCE) {
+				msg.checksum = csum.fence_checksum;
+
+			} else if (mission_type == MAV_MISSION_TYPE_RALLY) {
+				msg.checksum = csum.rally_checksum;
+
+			} else if (mission_type == MAV_MISSION_TYPE_ALL) {
+				msg.checksum = csum.all_checksum;
+
+			} else {
+				return false;
+			}
+
+			msg.mission_type = mission_type;
+			mavlink_msg_mission_checksum_send_struct(_mavlink->get_channel(), &msg);
+		}
+
+		return true;
+	}
+};
+
+#endif // MISSION_CHECKSUM_HPP

src/modules/navigator/mission.cpp

@@ -48,6 +48,7 @@
 #include "mission.h"
 #include "navigator.h"
 
+#include <crc32.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
 #include <dataman/dataman.h>
@@ -491,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) {
@@ -554,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]);
+		}
+	}
 }
 
 
@@ -1735,6 +1776,8 @@ Mission::check_mission_valid(bool force)
 
 		// find and store landing start marker (if available)
 		find_mission_land_start();
+
+		calculate_mission_checksums();
 	}
 }
 
@@ -1925,3 +1968,214 @@ void Mission::publish_navigator_mission_item()
 
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }
+
+void Mission::calculate_mission_checksums()
+{
+	union {
+		uint8_t uint8_part;
+		uint8_t uint16_part;
+		float float_part;
+	} crc_part;
+
+	mission_s mission_state = {};
+	mission_stats_entry_s stats;
+	mission_checksum_s csum{};
+
+	csum.timestamp = hrt_absolute_time();
+	csum.mission_checksum = 0;
+	csum.fence_checksum = 0;
+	csum.rally_checksum = 0;
+	csum.all_checksum = 0;
+
+	bool fail = false;
+
+	dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+		PX4_ERR("dataman read failure");
+		fail = true;
+	}
+
+	dm_unlock(DM_KEY_MISSION_STATE);
+
+	if (fail) {
+		return;
+	}
+
+	dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);
+	dm_lock(dm_current);
+
+	for (int i = 0; i < mission_state.count && !fail; i++) {
+		struct mission_item_s mission_item = {};
+		const ssize_t len = sizeof(mission_item);
+
+		if (dm_read(dm_current, i, &mission_item, len) != len) {
+			PX4_ERR("dataman read failure");
+			fail = true;
+
+		} else {
+			crc_part.uint8_part = mission_item.frame;
+			csum.mission_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint8_part), csum.mission_checksum);
+			crc_part.uint16_part = mission_item.nav_cmd;
+			csum.mission_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint16_part), csum.mission_checksum);
+			crc_part.uint8_part = mission_item.autocontinue;
+			csum.mission_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint8_part), csum.mission_checksum);
+
+			for (int j = 0; j < 4; j++) {
+				crc_part.float_part = mission_item.params[j];
+				csum.mission_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.mission_checksum);
+			}
+
+			crc_part.float_part = mission_item.lat;
+			csum.mission_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.mission_checksum);
+			crc_part.float_part = mission_item.lon;
+			csum.mission_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.mission_checksum);
+			crc_part.float_part = mission_item.params[6];
+			csum.mission_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.mission_checksum);
+		}
+	}
+
+	dm_unlock(dm_current);
+
+	if (fail) {
+		return;
+	}
+
+	csum.all_checksum = csum.mission_checksum;
+
+	dm_lock(DM_KEY_FENCE_POINTS);
+
+	if (dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)) != sizeof(mission_stats_entry_s)) {
+		PX4_ERR("dataman read failure");
+		fail = true;
+	}
+
+	for (size_t i = 1; i <= stats.num_items && !fail; i++) {
+		mission_fence_point_s mission_fence_point;
+		const ssize_t len = sizeof(mission_fence_point);
+
+		if (dm_read(DM_KEY_FENCE_POINTS, i, &mission_fence_point, len) != len) {
+			PX4_ERR("dataman read failure");
+			fail = true;
+
+		} else {
+			crc_part.uint8_part = mission_fence_point.frame;
+			csum.fence_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint8_part), csum.fence_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint8_part), csum.all_checksum);
+
+			crc_part.uint16_part = mission_fence_point.nav_cmd;
+			csum.fence_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint16_part), csum.fence_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint16_part), csum.all_checksum);
+
+			crc_part.float_part = mission_fence_point.lat;
+			csum.fence_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.fence_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.all_checksum);
+
+			crc_part.float_part = mission_fence_point.lon;
+			csum.fence_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.fence_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.all_checksum);
+
+			crc_part.float_part = mission_fence_point.alt;
+			csum.fence_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.fence_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.all_checksum);
+
+			crc_part.float_part = mission_fence_point.circle_radius;
+			csum.fence_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.fence_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.all_checksum);
+		}
+	}
+
+	dm_unlock(DM_KEY_FENCE_POINTS);
+
+	if (fail) {
+		return;
+	}
+
+	dm_lock(DM_KEY_SAFE_POINTS);
+
+	if (dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)) != sizeof(mission_stats_entry_s)) {
+		PX4_ERR("dataman read failure");
+		fail = true;
+	}
+
+	for (size_t i = 1; i <= stats.num_items && !fail; i++) {
+		mission_safe_point_s mission_safe_point;
+		const ssize_t len = sizeof(mission_safe_point);
+
+		if (dm_read(DM_KEY_SAFE_POINTS, i, &mission_safe_point, len) != len) {
+			PX4_ERR("dataman read failure");
+			fail = true;
+
+		} else {
+			crc_part.uint8_part = mission_safe_point.frame;
+			csum.rally_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint8_part), csum.rally_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.uint8_part), csum.all_checksum);
+
+			crc_part.float_part = mission_safe_point.lat;
+			csum.rally_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.rally_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.all_checksum);
+
+			crc_part.float_part = mission_safe_point.lon;
+			csum.rally_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.rally_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.all_checksum);
+
+			crc_part.float_part = mission_safe_point.alt;
+			csum.rally_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.rally_checksum);
+			csum.all_checksum = crc32part(&crc_part.uint8_part, sizeof(crc_part.float_part), csum.all_checksum);
+		}
+	}
+
+	dm_unlock(DM_KEY_SAFE_POINTS);
+
+	if (!fail) {
+		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

@@ -56,6 +56,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
+#include <uORB/topics/mission_checksum.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/navigator_mission_item.h>
 #include <uORB/topics/position_setpoint_triplet.h>
@@ -64,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
@@ -237,6 +240,10 @@ private:
 
 	void publish_navigator_mission_item();
 
+	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,
@@ -244,6 +251,7 @@ private:
 	)
 
 	uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item};
+	uORB::Publication<mission_checksum_s> mission_checksum_pub{ORB_ID::mission_checksum};
 
 	uORB::Subscription	_mission_sub{ORB_ID(mission)};		/**< mission subscription */
 	mission_s		_mission {};

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