Mission checksum

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

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/streams/MISSION_CHECKSUM.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   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 = 0.0, float param3 = 0.0, float param4 = 0.0,
+			     float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) 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>
@@ -1735,6 +1736,8 @@ Mission::check_mission_valid(bool force)
 
 		// find and store landing start marker (if available)
 		find_mission_land_start();
+
+		calculate_mission_checksums();
 	}
 }
 
@@ -1925,3 +1928,127 @@ void Mission::publish_navigator_mission_item()
 
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }
+
+
+void Mission::calculate_mission_checksums()
+{
+	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;
+
+	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");
+		return;
+	}
+
+	dm_unlock(DM_KEY_MISSION_STATE);
+
+	dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);
+
+	for (int i = 0; i < mission_state.count; 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");
+			return;
+		}
+
+		uint8_t frame = mission_item.frame;
+		csum.mission_checksum = crc32part(&frame, sizeof(frame), csum.mission_checksum);
+		csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_item.nav_cmd), sizeof(mission_item.nav_cmd),
+						  csum.mission_checksum);
+		uint8_t autocontinue = mission_item.autocontinue;
+		csum.mission_checksum = crc32part(&autocontinue, sizeof(autocontinue), csum.mission_checksum);
+
+		for (int j = 0; j < 4; j++) {
+			csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_item.params[j]), sizeof(mission_item.params[j]),
+							  csum.mission_checksum);
+		}
+
+		float lat = static_cast<float>(mission_item.lat);
+		csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.mission_checksum);
+		float lon = static_cast<float>(mission_item.lon);
+		csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.mission_checksum);
+		float alt = mission_item.params[6];
+		csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&alt), sizeof(alt), csum.mission_checksum);
+	}
+
+	csum.all_checksum = csum.mission_checksum;
+
+	if (dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)) != sizeof(mission_stats_entry_s)) {
+		PX4_ERR("dataman read failure");
+		return;
+	}
+
+	for (size_t i = 1; i <= stats.num_items; 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");
+			return;
+		}
+
+		uint8_t frame = mission_fence_point.frame;
+		csum.fence_checksum = crc32part(&frame, sizeof(frame), csum.fence_checksum);
+		csum.all_checksum = crc32part(&frame, sizeof(frame), csum.all_checksum);
+		csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.nav_cmd),
+						sizeof(mission_fence_point.nav_cmd), csum.fence_checksum);
+		csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.nav_cmd),
+					      sizeof(mission_fence_point.nav_cmd), csum.all_checksum);
+		float lat = static_cast<float>(mission_fence_point.lat);
+		csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.fence_checksum);
+		csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.all_checksum);
+		float lon = static_cast<float>(mission_fence_point.lon);
+		csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.fence_checksum);
+		csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.all_checksum);
+		csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.alt), sizeof(mission_fence_point.alt),
+						csum.fence_checksum);
+		csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.alt), sizeof(mission_fence_point.alt),
+					      csum.all_checksum);
+		csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.circle_radius),
+						sizeof(mission_fence_point.circle_radius), csum.fence_checksum);
+		csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.circle_radius),
+					      sizeof(mission_fence_point.circle_radius), csum.all_checksum);
+	}
+
+	if (dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)) != sizeof(mission_stats_entry_s)) {
+		PX4_ERR("dataman read failure");
+		return;
+	}
+
+	for (size_t i = 1; i <= stats.num_items; 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");
+			return;
+		}
+
+		uint8_t frame = mission_safe_point.frame;
+		csum.rally_checksum = crc32part(&frame, sizeof(frame), csum.rally_checksum);
+		csum.all_checksum = crc32part(&frame, sizeof(frame), csum.all_checksum);
+		float lat = static_cast<float>(mission_safe_point.lat);
+		csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.rally_checksum);
+		csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.all_checksum);
+		float lon = static_cast<float>(mission_safe_point.lon);
+		csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.rally_checksum);
+		csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.all_checksum);
+		csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_safe_point.alt), sizeof(mission_safe_point.alt),
+						csum.rally_checksum);
+		csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_safe_point.alt), sizeof(mission_safe_point.alt),
+					      csum.all_checksum);
+	}
+
+	mission_checksum_pub.publish(csum);
+}

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>
@@ -237,6 +238,8 @@ private:
 
 	void publish_navigator_mission_item();
 
+	void calculate_mission_checksums();
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
 		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
@@ -244,6 +247,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 {};