mavlink-mission: Compute checksum upon receipt on MAVLINK

msg/CMakeLists.txt

@@ -107,7 +107,6 @@ 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.msg

@@ -3,3 +3,4 @@ uint8 dataman_id	# default 0, there are two offboard storage places in the datam
 
 uint16 count		# count of the missions stored in the dataman
 int32 current_seq	# default -1, start at the one changed latest
+uint32 checksum		# crc32 checksum of all items

src/modules/mavlink/mavlink_messages.cpp

@@ -90,7 +90,6 @@
 #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"
@@ -547,11 +546,8 @@ 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

@@ -53,24 +53,52 @@
 #include <navigator/navigation.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <crc32.h>
 
 using matrix::wrap_2pi;
 
 dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
 bool MavlinkMissionManager::_dataman_init = false;
 uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
+uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 };
 int32_t MavlinkMissionManager::_current_seq = 0;
 bool MavlinkMissionManager::_transfer_in_progress = false;
 constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
 uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
 uint16_t MavlinkMissionManager::_safepoint_update_counter = 0;
 
+static constexpr int MISSION_ITEM_CRC_SIZE = 32;
 
 #define CHECK_SYSID_COMPID_MISSION(_msg)		(_msg.target_system == mavlink_system.sysid && \
 		((_msg.target_component == mavlink_system.compid) || \
 		 (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
 		 (_msg.target_component == MAV_COMP_ID_ALL)))
 
+
+static uint32_t crc32_for_mission_item(const mission_item_s &mission_item, uint32_t prev_crc32)
+{
+	union {
+		struct __attribute__((packed)) {
+			uint8_t frame;
+			uint16_t command;
+			uint8_t autocontinue;
+			float params[7];
+		} item;
+		uint8_t raw[MISSION_ITEM_CRC_SIZE];
+	} u;
+
+	u.item.frame = mission_item.frame;
+	u.item.command = mission_item.nav_cmd;
+	u.item.autocontinue = mission_item.autocontinue;
+
+	for (int i = 0; i < 7; i++) {
+		u.item.params[i] = mission_item.params[i];
+	}
+
+	return crc32part(u.raw, sizeof(u), prev_crc32);
+}
+
+
 MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
 	_mavlink(mavlink)
 {
@@ -101,6 +129,7 @@ MavlinkMissionManager::init_offboard_mission()
 		if (ret > 0) {
 			_dataman_id = (dm_item_t)mission_state.dataman_id;
 			_count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
+			_crc32[MAV_MISSION_TYPE_MISSION] = mission_state.checksum;
 			_current_seq = mission_state.current_seq;
 
 		} else if (ret < 0) {
@@ -124,6 +153,7 @@ MavlinkMissionManager::load_geofence_stats()
 
 	if (ret == sizeof(mission_stats_entry_s)) {
 		_count[MAV_MISSION_TYPE_FENCE] = stats.num_items;
+		_crc32[MAV_MISSION_TYPE_FENCE] = stats.checksum;
 		_geofence_update_counter = stats.update_counter;
 	}
 
@@ -139,6 +169,7 @@ MavlinkMissionManager::load_safepoint_stats()
 
 	if (ret == sizeof(mission_stats_entry_s)) {
 		_count[MAV_MISSION_TYPE_RALLY] = stats.num_items;
+		_crc32[MAV_MISSION_TYPE_RALLY] = stats.checksum;
 	}
 
 	return ret;
@@ -148,14 +179,15 @@ MavlinkMissionManager::load_safepoint_stats()
  * Publish mission topic to notify navigator about changes.
  */
 int
-MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq)
+MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32)
 {
 	// We want to make sure the whole struct is initialized including padding before getting written by dataman.
 	mission_s mission{};
 	mission.timestamp = hrt_absolute_time();
 	mission.dataman_id = dataman_id;
 	mission.count = count;
 	mission.current_seq = seq;
+	mission.checksum = crc32;
 
 	/* update mission state in dataman */
 
@@ -177,6 +209,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
 		/* update active mission state */
 		_dataman_id = dataman_id;
 		_count[MAV_MISSION_TYPE_MISSION] = count;
+		_crc32[MAV_MISSION_TYPE_MISSION] = crc32;
 		_current_seq = seq;
 		_my_dataman_id = _dataman_id;
 
@@ -198,17 +231,19 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
 	}
 }
 int
-MavlinkMissionManager::update_geofence_count(unsigned count)
+MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32)
 {
 	mission_stats_entry_s stats;
 	stats.num_items = count;
 	stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data
+	stats.checksum = crc32;
 
 	/* update stats in dataman */
 	int res = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s));
 
 	if (res == sizeof(mission_stats_entry_s)) {
 		_count[MAV_MISSION_TYPE_FENCE] = count;
+		_crc32[MAV_MISSION_TYPE_FENCE] = crc32;
 
 	} else {
 
@@ -225,17 +260,19 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
 }
 
 int
-MavlinkMissionManager::update_safepoint_count(unsigned count)
+MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32)
 {
 	mission_stats_entry_s stats;
 	stats.num_items = count;
 	stats.update_counter = ++_safepoint_update_counter;
+	stats.checksum = crc32;
 
 	/* update stats in dataman */
 	int res = dm_write(DM_KEY_SAFE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s));
 
 	if (res == sizeof(mission_stats_entry_s)) {
 		_count[MAV_MISSION_TYPE_RALLY] = count;
+		_crc32[MAV_MISSION_TYPE_RALLY] = crc32;
 
 	} else {
 
@@ -288,6 +325,51 @@ MavlinkMissionManager::send_mission_current(int32_t seq)
 	}
 }
 
+
+uint32_t crc32combine(uint32_t crc_a, uint32_t crc_b, size_t len_b)
+{
+	size_t i;
+	uint8_t zero = 0;
+
+	for (i = 0;  i < len_b;  i++) {
+		crc_a = crc32part(&zero, 1, crc_a);
+	}
+
+	return (crc_a ^ crc_b);
+}
+
+
+bool MavlinkMissionManager::send_mission_checksum(MAV_MISSION_TYPE mission_type)
+{
+	if (mission_type != MAV_MISSION_TYPE_MISSION &&
+	    mission_type != MAV_MISSION_TYPE_FENCE &&
+	    mission_type != MAV_MISSION_TYPE_RALLY &&
+	    mission_type != MAV_MISSION_TYPE_ALL
+	   ) {
+		return false;
+	}
+
+	_time_last_sent = hrt_absolute_time();
+	mavlink_mission_checksum_t wpc{};
+	wpc.mission_type = mission_type;
+
+	if (mission_type == MAV_MISSION_TYPE_ALL) {
+		// combine all individual checksums
+		uint32_t checksum = _crc32[MAV_MISSION_TYPE_MISSION];
+		checksum = crc32combine(checksum, _crc32[MAV_MISSION_TYPE_FENCE],
+					_count[MAV_MISSION_TYPE_FENCE] * MISSION_ITEM_CRC_SIZE);
+		checksum = crc32combine(checksum, _crc32[MAV_MISSION_TYPE_RALLY],
+					_count[MAV_MISSION_TYPE_RALLY] * MISSION_ITEM_CRC_SIZE);
+		wpc.checksum = checksum;
+
+	} else {
+		wpc.checksum = _crc32[mission_type];
+	}
+
+	mavlink_msg_mission_checksum_send_struct(_mavlink->get_channel(), &wpc);
+	return true;
+}
+
 void
 MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type)
 {
@@ -596,6 +678,10 @@ MavlinkMissionManager::handle_message(const mavlink_message_t *msg)
 		handle_mission_clear_all(msg);
 		break;
 
+	case MAVLINK_MSG_ID_COMMAND_INT:
+		printf("GOT A COMMAND\n");
+		break;
+
 	default:
 		break;
 	}
@@ -670,7 +756,8 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
 			_time_last_recv = hrt_absolute_time();
 
 			if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) {
-				if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) {
+				if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq,
+							  _crc32[MAV_MISSION_TYPE_MISSION]) == PX4_OK) {
 					PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq);
 
 				} else {
@@ -892,6 +979,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 
 			_transfer_in_progress = true;
 			_mission_type = (MAV_MISSION_TYPE)wpc.mission_type;
+			_transfer_current_crc32 = 0;
 
 			if (wpc.count > current_max_item_count()) {
 				PX4_DEBUG("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count());
@@ -910,20 +998,20 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 					/* alternate dataman ID anyway to let navigator know about changes */
 
 					if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) {
-						update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0);
+						update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0, 0);
 
 					} else {
-						update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
+						update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
 					}
 
 					break;
 
 				case MAV_MISSION_TYPE_FENCE:
-					update_geofence_count(0);
+					update_geofence_count(0, 0);
 					break;
 
 				case MAV_MISSION_TYPE_RALLY:
-					update_safepoint_count(0);
+					update_safepoint_count(0, 0);
 					break;
 
 				default:
@@ -1104,6 +1192,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 			return;
 		}
 
+		_transfer_current_crc32 = crc32_for_mission_item(mission_item, _transfer_current_crc32);
+
 		bool write_failed = false;
 		bool check_failed = false;
 
@@ -1149,7 +1239,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 					if (mission_item.vertex_count < 3) { // feasibility check
 						PX4_ERR("Fence: too few vertices");
 						check_failed = true;
-						update_geofence_count(0);
+						update_geofence_count(0, 0);
 					}
 
 				} else {
@@ -1218,15 +1308,15 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 
 			switch (_mission_type) {
 			case MAV_MISSION_TYPE_MISSION:
-				ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq);
+				ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32);
 				break;
 
 			case MAV_MISSION_TYPE_FENCE:
-				ret = update_geofence_count(_transfer_count);
+				ret = update_geofence_count(_transfer_count, _transfer_current_crc32);
 				break;
 
 			case MAV_MISSION_TYPE_RALLY:
-				ret = update_safepoint_count(_transfer_count);
+				ret = update_safepoint_count(_transfer_count, _transfer_current_crc32);
 				break;
 
 			default:
@@ -1240,6 +1330,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 
 			if (ret == PX4_OK) {
 				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
+				send_mission_checksum(_mission_type);
 
 			} else {
 				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
@@ -1273,22 +1364,22 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 			switch (wpca.mission_type) {
 			case MAV_MISSION_TYPE_MISSION:
 				ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
-							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
+							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
 				break;
 
 			case MAV_MISSION_TYPE_FENCE:
-				ret = update_geofence_count(0);
+				ret = update_geofence_count(0, 0);
 				break;
 
 			case MAV_MISSION_TYPE_RALLY:
-				ret = update_safepoint_count(0);
+				ret = update_safepoint_count(0, 0);
 				break;
 
 			case MAV_MISSION_TYPE_ALL:
 				ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
-							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
-				ret = update_geofence_count(0) || ret;
-				ret = update_safepoint_count(0) || ret;
+							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
+				ret = update_geofence_count(0, 0) || ret;
+				ret = update_safepoint_count(0, 0) || ret;
 				break;
 
 			default: