Skip to content

Commit

Permalink
generator/C: Don't parse messages outside CRC_EXTRA
Browse files Browse the repository at this point in the history
I have been wondering for a while why I sometimes see MAVLink
components "appear" with random sysid/compid when I leave a system
running for some time.

Today I finally caught the bytes of such a case, the offending bytes
were:
{253, 0, 1, 0, 0, 3, 1, 1, 42, 0, 185, 186,
 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

This is msgid 10753 from sysid 3, compid 1 with payload len 0.
It's presumably signed, hence the trailing zeros.

It turns out the msgid is way out of the CRC_EXTRA table and hence the
mavlink_get_msg_entry(msgid) function returns NULL.
In that case 0 is ignored and we carry on parsing.

My suggestion is to reset parsing when we don't know the CRC_EXTRA of
the message to avoid flagging noise as a valid message.

Included fixup:
In case a message is forwarded (even with unknown/bad CRC) we need to
copy the first CRC byte.
  • Loading branch information
julianoes authored and tridge committed Nov 13, 2024
1 parent 5c0d545 commit 6e1da0a
Showing 1 changed file with 15 additions and 9 deletions.
24 changes: 15 additions & 9 deletions generator/C/include_v2.0/mavlink_helpers.h
Original file line number Diff line number Diff line change
Expand Up @@ -742,18 +742,24 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,

case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {
const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);
uint8_t crc_extra = e?e->crc_extra:0;
mavlink_update_checksum(rxmsg, crc_extra);
if (c != (rxmsg->checksum & 0xFF)) {
if (e == NULL) {
// Message not found in CRC_EXTRA table.
status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
rxmsg->ck[0] = c;
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
}
rxmsg->ck[0] = c;
uint8_t crc_extra = e->crc_extra;
mavlink_update_checksum(rxmsg, crc_extra);
if (c != (rxmsg->checksum & 0xFF)) {
status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
} else {
status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
}
rxmsg->ck[0] = c;

// zero-fill the packet to cope with short incoming packets
if (e && status->packet_idx < e->max_msg_len) {
memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx);
// zero-fill the packet to cope with short incoming packets
if (e && status->packet_idx < e->max_msg_len) {
memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx);
}
}
break;
}
Expand Down

0 comments on commit 6e1da0a

Please sign in to comment.