Skip to content

Commit 934341f

Browse files
julianoesjulianoes
authored
Merge pull request #2734 from mavlink/pr-parsing-fix
core: fix bug where invalid messages are parsed
2 parents 9f4cbbd + 4f18bb4 commit 934341f

11 files changed

+105
-87
lines changed

‎src/mavsdk/core/connection.cpp‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -97,13 +97,17 @@ void Connection::receive_libmav_message(
9797
}
9898
}
9999

100-
void Connection::receive_message(mavlink_message_t& message, Connection* connection)
100+
void Connection::receive_message(
101+
MavlinkReceiver::ParseResult result, mavlink_message_t& message, Connection* connection)
101102
{
102-
// Register system ID when receiving a message from a new system.
103-
if (_system_ids.find(message.sysid) == _system_ids.end()) {
104-
_system_ids.insert(message.sysid);
103+
// Register system ID for valid messages
104+
if (result == MavlinkReceiver::ParseResult::MessageParsed) {
105+
if (_system_ids.find(message.sysid) == _system_ids.end()) {
106+
_system_ids.insert(message.sysid);
107+
}
105108
}
106-
_receiver_callback(message, connection);
109+
// Let MavsdkImpl handle the ParseResult (queue for processing or forward-only)
110+
_receiver_callback(result, message, connection);
107111
}
108112

109113
bool Connection::should_forward_messages() const

‎src/mavsdk/core/connection.h‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,8 @@ class MavsdkImpl; // Forward declaration
1515

1616
class Connection {
1717
public:
18-
using ReceiverCallback =
19-
std::function<void(mavlink_message_t& message, Connection* connection)>;
18+
using ReceiverCallback = std::function<void(
19+
MavlinkReceiver::ParseResult result, mavlink_message_t& message, Connection* connection)>;
2020
using LibmavReceiverCallback =
2121
std::function<void(const Mavsdk::MavlinkMessage& message, Connection* connection)>;
2222

@@ -49,7 +49,8 @@ class Connection {
4949
protected:
5050
bool start_mavlink_receiver();
5151
void stop_mavlink_receiver();
52-
void receive_message(mavlink_message_t& message, Connection* connection);
52+
void receive_message(
53+
MavlinkReceiver::ParseResult result, mavlink_message_t& message, Connection* connection);
5354

5455
bool start_libmav_receiver();
5556
void stop_libmav_receiver();

‎src/mavsdk/core/mavlink_receiver.cpp‎

Lines changed: 11 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ void MavlinkReceiver::set_new_datagram(char* datagram, unsigned datagram_len)
2424
}
2525
}
2626

27-
bool MavlinkReceiver::parse_message()
27+
MavlinkReceiver::ParseResult MavlinkReceiver::parse_message()
2828
{
2929
// Note that one datagram can contain multiple mavlink messages.
3030
for (unsigned i = 0; i < _datagram_len; ++i) {
@@ -43,44 +43,24 @@ bool MavlinkReceiver::parse_message()
4343
}
4444

4545
// We have parsed one message, let's return, so it can be handled.
46-
return true;
47-
} else if (parse_result == MAVLINK_FRAMING_BAD_CRC) {
48-
// Complete message with bad CRC - store raw bytes for forwarding
49-
// Calculate the message length based on header info
50-
size_t message_length = 0;
51-
if (_last_message.magic == MAVLINK_STX_MAVLINK1) {
52-
// MAVLink v1: STX + header + payload + CRC
53-
message_length = 1 + MAVLINK_CORE_HEADER_MAVLINK1_LEN + _last_message.len +
54-
MAVLINK_NUM_CHECKSUM_BYTES;
55-
} else {
56-
// MAVLink v2: STX + header + payload + CRC + optional signature
57-
message_length =
58-
1 + MAVLINK_CORE_HEADER_LEN + _last_message.len + MAVLINK_NUM_CHECKSUM_BYTES;
59-
if (_last_message.incompat_flags & MAVLINK_IFLAG_SIGNED) {
60-
message_length += MAVLINK_SIGNATURE_BLOCK_LEN;
61-
}
62-
}
46+
return ParseResult::MessageParsed;
6347

64-
// Calculate start position (current position minus what we've consumed)
65-
if (i + 1 >= message_length) {
66-
// Note: Raw message detected but storage removed since retrieval methods were
67-
// unused
68-
69-
// Move the pointer to the datagram forward by the amount parsed.
70-
_datagram += (i + 1);
71-
// And decrease the length, so we don't overshoot in the next round.
72-
_datagram_len -= (i + 1);
48+
} else if (parse_result == MAVLINK_FRAMING_BAD_CRC) {
49+
// Message with bad CRC - _last_message is still populated with the parsed data
50+
// Move the pointer to the datagram forward by the amount parsed.
51+
_datagram += (i + 1);
52+
// And decrease the length, so we don't overshoot in the next round.
53+
_datagram_len -= (i + 1);
7354

74-
// Return true to indicate we have something to process (raw message)
75-
return true;
76-
}
55+
// Return BadCrc to indicate message should be forwarded but not processed
56+
return ParseResult::BadCrc;
7757
}
7858
}
7959

8060
// No (more) messages, let's give up.
8161
_datagram = nullptr;
8262
_datagram_len = 0;
83-
return false;
63+
return ParseResult::NoneAvailable;
8464
}
8565

8666
void MavlinkReceiver::debug_drop_rate()

‎src/mavsdk/core/mavlink_receiver.h‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,12 @@ namespace mavsdk {
88

99
class MavlinkReceiver {
1010
public:
11+
enum class ParseResult {
12+
MessageParsed, // Valid message ready for processing and forwarding
13+
BadCrc, // Corrupted message - forward raw bytes only, don't process
14+
NoneAvailable // No more messages in datagram
15+
};
16+
1117
MavlinkReceiver();
1218

1319
mavlink_message_t& get_last_message() { return _last_message; }
@@ -16,7 +22,7 @@ class MavlinkReceiver {
1622

1723
void set_new_datagram(char* datagram, unsigned datagram_len);
1824

19-
bool parse_message();
25+
ParseResult parse_message();
2026

2127
void debug_drop_rate();
2228
void print_line(

‎src/mavsdk/core/mavsdk_impl.cpp‎

Lines changed: 36 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -345,18 +345,28 @@ void MavsdkImpl::forward_message(mavlink_message_t& message, Connection* connect
345345
}
346346
}
347347
if (successful_emissions == 0) {
348-
LogErr() << "Message forwarding failed";
348+
if (_system_debugging) {
349+
LogErr() << "Message forwarding failed";
350+
}
349351
}
350352
}
351353
}
352354

353-
void MavsdkImpl::receive_message(mavlink_message_t& message, Connection* connection)
355+
void MavsdkImpl::receive_message(
356+
MavlinkReceiver::ParseResult result, mavlink_message_t& message, Connection* connection)
354357
{
355-
{
356-
std::lock_guard lock(_received_messages_mutex);
357-
_received_messages.emplace(ReceivedMessage{std::move(message), connection});
358+
if (result == MavlinkReceiver::ParseResult::MessageParsed) {
359+
// Valid message: queue for full processing (which includes forwarding)
360+
{
361+
std::lock_guard lock(_received_messages_mutex);
362+
_received_messages.emplace(ReceivedMessage{std::move(message), connection});
363+
}
364+
_received_messages_cv.notify_one();
365+
366+
} else if (result == MavlinkReceiver::ParseResult::BadCrc) {
367+
// Unknown message: forward only, don't process locally
368+
forward_message(message, connection);
358369
}
359-
_received_messages_cv.notify_one();
360370
}
361371

362372
void MavsdkImpl::receive_libmav_message(
@@ -802,9 +812,10 @@ std::pair<ConnectionResult, Mavsdk::ConnectionHandle>
802812
MavsdkImpl::add_udp_connection(const CliArg::Udp& udp, ForwardingOption forwarding_option)
803813
{
804814
auto new_conn = std::make_unique<UdpConnection>(
805-
[this](mavlink_message_t& message, Connection* connection) {
806-
receive_message(message, connection);
807-
},
815+
[this](
816+
MavlinkReceiver::ParseResult result,
817+
mavlink_message_t& message,
818+
Connection* connection) { receive_message(result, message, connection); },
808819
[this](const Mavsdk::MavlinkMessage& message, Connection* connection) {
809820
receive_libmav_message(message, connection);
810821
},
@@ -851,9 +862,10 @@ MavsdkImpl::add_tcp_connection(const CliArg::Tcp& tcp, ForwardingOption forwardi
851862
{
852863
if (tcp.mode == CliArg::Tcp::Mode::Out) {
853864
auto new_conn = std::make_unique<TcpClientConnection>(
854-
[this](mavlink_message_t& message, Connection* connection) {
855-
receive_message(message, connection);
856-
},
865+
[this](
866+
MavlinkReceiver::ParseResult result,
867+
mavlink_message_t& message,
868+
Connection* connection) { receive_message(result, message, connection); },
857869
[this](const Mavsdk::MavlinkMessage& message, Connection* connection) {
858870
receive_libmav_message(message, connection);
859871
},
@@ -872,9 +884,10 @@ MavsdkImpl::add_tcp_connection(const CliArg::Tcp& tcp, ForwardingOption forwardi
872884
}
873885
} else {
874886
auto new_conn = std::make_unique<TcpServerConnection>(
875-
[this](mavlink_message_t& message, Connection* connection) {
876-
receive_message(message, connection);
877-
},
887+
[this](
888+
MavlinkReceiver::ParseResult result,
889+
mavlink_message_t& message,
890+
Connection* connection) { receive_message(result, message, connection); },
878891
[this](const Mavsdk::MavlinkMessage& message, Connection* connection) {
879892
receive_libmav_message(message, connection);
880893
},
@@ -901,9 +914,10 @@ std::pair<ConnectionResult, Mavsdk::ConnectionHandle> MavsdkImpl::add_serial_con
901914
ForwardingOption forwarding_option)
902915
{
903916
auto new_conn = std::make_unique<SerialConnection>(
904-
[this](mavlink_message_t& message, Connection* connection) {
905-
receive_message(message, connection);
906-
},
917+
[this](
918+
MavlinkReceiver::ParseResult result,
919+
mavlink_message_t& message,
920+
Connection* connection) { receive_message(result, message, connection); },
907921
[this](const Mavsdk::MavlinkMessage& message, Connection* connection) {
908922
receive_libmav_message(message, connection);
909923
},
@@ -944,9 +958,10 @@ MavsdkImpl::add_raw_connection(ForwardingOption forwarding_option)
944958
}
945959

946960
auto new_conn = std::make_unique<RawConnection>(
947-
[this](mavlink_message_t& message, Connection* connection) {
948-
receive_message(message, connection);
949-
},
961+
[this](
962+
MavlinkReceiver::ParseResult result,
963+
mavlink_message_t& message,
964+
Connection* connection) { receive_message(result, message, connection); },
950965
[this](const Mavsdk::MavlinkMessage& message, Connection* connection) {
951966
receive_libmav_message(message, connection);
952967
},

‎src/mavsdk/core/mavsdk_impl.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,8 @@ class MavsdkImpl {
5454
static std::string version();
5555

5656
void forward_message(mavlink_message_t& message, Connection* connection);
57-
void receive_message(mavlink_message_t& message, Connection* connection);
57+
void receive_message(
58+
MavlinkReceiver::ParseResult result, mavlink_message_t& message, Connection* connection);
5859
void receive_libmav_message(const Mavsdk::MavlinkMessage& message, Connection* connection);
5960

6061
std::pair<ConnectionResult, Mavsdk::ConnectionHandle>

‎src/mavsdk/core/raw_connection.cpp‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -57,10 +57,11 @@ void RawConnection::receive(const char* bytes, size_t length)
5757
// This is safe because the receivers only read from the buffer
5858
_mavlink_receiver->set_new_datagram(const_cast<char*>(bytes), static_cast<int>(length));
5959

60-
// Parse all mavlink messages in one datagram. Once exhausted, we'll exit while.
61-
while (_mavlink_receiver->parse_message()) {
62-
// Handle parsed message
63-
receive_message(_mavlink_receiver->get_last_message(), this);
60+
// Parse all mavlink messages in one datagram. Once exhausted, we'll exit loop.
61+
auto parse_result = _mavlink_receiver->parse_message();
62+
while (parse_result != MavlinkReceiver::ParseResult::NoneAvailable) {
63+
receive_message(parse_result, _mavlink_receiver->get_last_message(), this);
64+
parse_result = _mavlink_receiver->parse_message();
6465
}
6566

6667
// Also parse with libmav if available

‎src/mavsdk/core/serial_connection.cpp‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -315,9 +315,11 @@ void SerialConnection::receive()
315315
continue;
316316
}
317317
_mavlink_receiver->set_new_datagram(buffer, recv_len);
318-
// Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.
319-
while (_mavlink_receiver->parse_message()) {
320-
receive_message(_mavlink_receiver->get_last_message(), this);
318+
// Parse all mavlink messages in one data packet. Once exhausted, we'll exit loop.
319+
auto parse_result = _mavlink_receiver->parse_message();
320+
while (parse_result != MavlinkReceiver::ParseResult::NoneAvailable) {
321+
receive_message(parse_result, _mavlink_receiver->get_last_message(), this);
322+
parse_result = _mavlink_receiver->parse_message();
321323
}
322324

323325
// Also parse with libmav if available

‎src/mavsdk/core/tcp_client_connection.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -260,8 +260,10 @@ void TcpClientConnection::receive()
260260

261261
_mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));
262262

263-
while (_mavlink_receiver->parse_message()) {
264-
receive_message(_mavlink_receiver->get_last_message(), this);
263+
auto parse_result = _mavlink_receiver->parse_message();
264+
while (parse_result != MavlinkReceiver::ParseResult::NoneAvailable) {
265+
receive_message(parse_result, _mavlink_receiver->get_last_message(), this);
266+
parse_result = _mavlink_receiver->parse_message();
265267
}
266268

267269
// Also parse with libmav if available

‎src/mavsdk/core/tcp_server_connection.cpp‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -280,9 +280,11 @@ void TcpServerConnection::receive()
280280

281281
_mavlink_receiver->set_new_datagram(buffer.data(), static_cast<int>(recv_len));
282282

283-
// Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.
284-
while (_mavlink_receiver->parse_message()) {
285-
receive_message(_mavlink_receiver->get_last_message(), this);
283+
// Parse all mavlink messages in one data packet. Once exhausted, we'll exit loop.
284+
auto parse_result = _mavlink_receiver->parse_message();
285+
while (parse_result != MavlinkReceiver::ParseResult::NoneAvailable) {
286+
receive_message(parse_result, _mavlink_receiver->get_last_message(), this);
287+
parse_result = _mavlink_receiver->parse_message();
286288
}
287289

288290
// Also parse with libmav if available

0 commit comments

Comments
 (0)