fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 09:01:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink-mission: Compute checksum upon receipt on MAVLINK

Browse Source
This commit is contained in:
Matej Frančeškin
2023-06-13 15:01:38 +02:00
committed by Konrad
parent fc357921fd
commit 13510da701
5 changed files with 137 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/Mission.msg
+1
View File
@@ -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_mission.cpp
+116 -19
View File
@@ -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,7 +179,7 @@ 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{};
@@ -156,6 +187,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
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;
@@ -199,17 +232,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, &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 {
@@ -226,17 +261,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, &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 {
@@ -277,6 +314,51 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", 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)
{
@@ -605,6 +687,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;
}
@@ -679,7 +765,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 {
@@ -901,6 +988,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());
@@ -919,20 +1007,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:
@@ -1113,6 +1201,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;
@@ -1157,7 +1247,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 {
@@ -1226,15 +1316,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:
@@ -1248,6 +1338,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);
@@ -1281,22 +1372,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:
@@ -1400,8 +1491,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:
@@ -1435,8 +1526,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;
src/modules/mavlink/mavlink_mission.h
+8 -3
View File
@@ -91,6 +91,8 @@ public:
void check_active_mission(void);
bool send_mission_checksum(MAV_MISSION_TYPE mission_type);
private:
enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state
enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible)
@@ -110,6 +112,8 @@ private:
static bool _dataman_init; ///< Dataman initialized
static uint16_t _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE
static uint32_t _crc32[3]; ///< CRC32 checksum of (active) mission for each MAV_MISSION_TYPE
static int32_t _current_seq; ///< Current item sequence in active mission
int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached)
@@ -120,6 +124,7 @@ private:
uint16_t _transfer_seq{0}; ///< Item sequence in current transmission
int32_t _transfer_current_seq{-1}; ///< Current item ID for current transmission (-1 means not initialized)
uint32_t _transfer_current_crc32{0}; ///< Current CRC32 checksum of current transmission
uint8_t _transfer_partner_sysid{0}; ///< Partner system ID for current transmission
uint8_t _transfer_partner_compid{0}; ///< Partner component ID for current transmission
@@ -159,13 +164,13 @@ private:
void init_offboard_mission();
int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq);
int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32);
/** store the geofence count to dataman */
int update_geofence_count(unsigned count);
int update_geofence_count(unsigned count, uint32_t crc32);
/** store the safepoint count to dataman */
int update_safepoint_count(unsigned count);
int update_safepoint_count(unsigned count, uint32_t crc32);
/** load geofence stats from dataman */
int load_geofence_stats();
src/modules/mavlink/mavlink_receiver.cpp
+10 -4
View File
@@ -529,11 +529,17 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
get_message_interval((int)roundf(cmd_mavlink.param1));
} else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) {
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
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);
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 {
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);
}
} else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) {
if (_mavlink->failure_injection_enabled()) {
src/modules/navigator/navigation.h
+2 -1
View File
@@ -203,7 +203,8 @@ struct mission_item_s {
*/
struct mission_stats_entry_s {
uint16_t num_items; /**< total number of items stored (excluding this one) */
uint16_t update_counter; /**< This counter is increased when (some) items change (this can wrap) */
uint16_t update_counter; /**< This counter is increased when (some) items change (this can wrap) */
uint32_t checksum; /**< CRC32 checksum of the fence / safe points */
};
/**