diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 4ac540d62c..d8b60c34aa 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/mission_checksum.msg b/msg/mission_checksum.msg new file mode 100644 index 0000000000..f52b0cf258 --- /dev/null +++ b/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 diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 17451f25d8..2b0b1b8ea5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/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(), #endif // EFI_STATUS_HPP #if defined(GPS_RTCM_DATA_HPP) - create_stream_list_item() + create_stream_list_item(), #endif // GPS_RTCM_DATA_HPP +#if defined(MISSION_CHECKSUM_HPP) + create_stream_list_item() +#endif // MISSION_CHECKSUM_HPP }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/streams/MISSION_CHECKSUM.hpp b/src/modules/mavlink/streams/MISSION_CHECKSUM.hpp new file mode 100644 index 0000000000..2a8369bc87 --- /dev/null +++ b/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 + +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(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 diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 5c73004497..4f1a6e2bb2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -48,6 +48,7 @@ #include "mission.h" #include "navigator.h" +#include #include #include #include @@ -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(&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(&mission_item.params[j]), sizeof(mission_item.params[j]), + csum.mission_checksum); + } + + float lat = static_cast(mission_item.lat); + csum.mission_checksum = crc32part(reinterpret_cast(&lat), sizeof(lat), csum.mission_checksum); + float lon = static_cast(mission_item.lon); + csum.mission_checksum = crc32part(reinterpret_cast(&lon), sizeof(lon), csum.mission_checksum); + float alt = mission_item.params[6]; + csum.mission_checksum = crc32part(reinterpret_cast(&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(&mission_fence_point.nav_cmd), + sizeof(mission_fence_point.nav_cmd), csum.fence_checksum); + csum.all_checksum = crc32part(reinterpret_cast(&mission_fence_point.nav_cmd), + sizeof(mission_fence_point.nav_cmd), csum.all_checksum); + float lat = static_cast(mission_fence_point.lat); + csum.fence_checksum = crc32part(reinterpret_cast(&lat), sizeof(lat), csum.fence_checksum); + csum.all_checksum = crc32part(reinterpret_cast(&lat), sizeof(lat), csum.all_checksum); + float lon = static_cast(mission_fence_point.lon); + csum.fence_checksum = crc32part(reinterpret_cast(&lon), sizeof(lon), csum.fence_checksum); + csum.all_checksum = crc32part(reinterpret_cast(&lon), sizeof(lon), csum.all_checksum); + csum.fence_checksum = crc32part(reinterpret_cast(&mission_fence_point.alt), sizeof(mission_fence_point.alt), + csum.fence_checksum); + csum.all_checksum = crc32part(reinterpret_cast(&mission_fence_point.alt), sizeof(mission_fence_point.alt), + csum.all_checksum); + csum.fence_checksum = crc32part(reinterpret_cast(&mission_fence_point.circle_radius), + sizeof(mission_fence_point.circle_radius), csum.fence_checksum); + csum.all_checksum = crc32part(reinterpret_cast(&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(mission_safe_point.lat); + csum.rally_checksum = crc32part(reinterpret_cast(&lat), sizeof(lat), csum.rally_checksum); + csum.all_checksum = crc32part(reinterpret_cast(&lat), sizeof(lat), csum.all_checksum); + float lon = static_cast(mission_safe_point.lon); + csum.rally_checksum = crc32part(reinterpret_cast(&lon), sizeof(lon), csum.rally_checksum); + csum.all_checksum = crc32part(reinterpret_cast(&lon), sizeof(lon), csum.all_checksum); + csum.rally_checksum = crc32part(reinterpret_cast(&mission_safe_point.alt), sizeof(mission_safe_point.alt), + csum.rally_checksum); + csum.all_checksum = crc32part(reinterpret_cast(&mission_safe_point.alt), sizeof(mission_safe_point.alt), + csum.all_checksum); + } + + mission_checksum_pub.publish(csum); +} \ No newline at end of file diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 308b238728..6b4fd9f4cf 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -237,6 +238,8 @@ private: void publish_navigator_mission_item(); + void calculate_mission_checksums(); + DEFINE_PARAMETERS( (ParamFloat) _param_mis_dist_1wp, (ParamFloat) _param_mis_dist_wps, @@ -244,6 +247,7 @@ private: ) uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; + uORB::Publication mission_checksum_pub{ORB_ID::mission_checksum}; uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ mission_s _mission {};