From 455c88e1fc5df16e91d56b12c388df1c79763d58 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Thu, 18 Nov 2021 12:21:46 +0100 Subject: [PATCH] mavlink-mission: use mavlink_mission_item for crc calculation --- src/modules/mavlink/mavlink_mission.cpp | 30 ++++++++++++++++-------- src/modules/mavlink/mavlink_receiver.cpp | 4 ++++ 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index c9897934b3..381e944cc9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -75,7 +75,7 @@ static constexpr int MISSION_ITEM_CRC_SIZE = 32; (_msg.target_component == MAV_COMP_ID_ALL))) -static uint32_t crc32_for_mission_item(const mission_item_s &mission_item, uint32_t prev_crc32) +static uint32_t crc32_for_mission_item(const mavlink_mission_item_t &mission_item, uint32_t prev_crc32) { union { struct __attribute__((packed)) { @@ -88,12 +88,15 @@ static uint32_t crc32_for_mission_item(const mission_item_s &mission_item, uint3 } u; u.item.frame = mission_item.frame; - u.item.command = mission_item.nav_cmd; + u.item.command = mission_item.command; u.item.autocontinue = mission_item.autocontinue; - - for (int i = 0; i < 7; i++) { - u.item.params[i] = mission_item.params[i]; - } + u.item.params[0] = mission_item.param1; + u.item.params[1] = mission_item.param2; + u.item.params[2] = mission_item.param3; + u.item.params[3] = mission_item.param4; + u.item.params[4] = mission_item.x; + u.item.params[5] = mission_item.y; + u.item.params[6] = mission_item.z; return crc32part(u.raw, sizeof(u), prev_crc32); } @@ -314,7 +317,12 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq); } - +/** + * Combines two independently computed CRC32 checksums to a single checksum + * that is the same as if the checksum was computed over the concatenation + * of both blocks. + * Reference: https://stackoverflow.com/questions/23122312/crc-calculation-of-a-mostly-static-data-stream + */ uint32_t crc32combine(uint32_t crc_a, uint32_t crc_b, size_t len_b) { size_t i; @@ -327,7 +335,7 @@ uint32_t crc32combine(uint32_t crc_a, uint32_t crc_b, size_t len_b) return (crc_a ^ crc_b); } - +#if !defined(CONSTRAINED_FLASH) bool MavlinkMissionManager::send_mission_checksum(MAV_MISSION_TYPE mission_type) { if (mission_type != MAV_MISSION_TYPE_MISSION && @@ -358,6 +366,7 @@ bool MavlinkMissionManager::send_mission_checksum(MAV_MISSION_TYPE mission_type) mavlink_msg_mission_checksum_send_struct(_mavlink->get_channel(), &wpc); return true; } +#endif void MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type) @@ -688,7 +697,6 @@ MavlinkMissionManager::handle_message(const mavlink_message_t *msg) break; case MAVLINK_MSG_ID_COMMAND_INT: - printf("GOT A COMMAND\n"); break; default: @@ -1201,7 +1209,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) return; } - _transfer_current_crc32 = crc32_for_mission_item(mission_item, _transfer_current_crc32); + _transfer_current_crc32 = crc32_for_mission_item(wp, _transfer_current_crc32); bool write_failed = false; bool check_failed = false; @@ -1338,7 +1346,9 @@ 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); +#if !defined(CONSTRAINED_FLASH) send_mission_checksum(_mission_type); +#endif } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index aae33b34fa..a3704a4cf7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -530,12 +530,16 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { uint16_t message_id = (uint16_t)roundf(vehicle_command.param1); +#if !defined(CONSTRAINED_FLASH) if (message_id == MAVLINK_MSG_ID_MISSION_CHECKSUM) { result = _mission_manager.send_mission_checksum((MAV_MISSION_TYPE)roundf(vehicle_command.param2)) ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED; } else { +#else + { +#endif result = handle_request_message_command(message_id, vehicle_command.param2, vehicle_command.param3, vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);