mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
fix(mavlink): forward received frames unchanged (#28891)
* fix(mavlink): forward received frames unchanged
Forwarded messages were queued as a truncated copy of mavlink_message_t
and re-serialized on the way out. The signature bytes were never
copied, so forwarded signed messages went out with a garbage signature.
Messages which are not in our dialect were not forwarded at all: the
parser can't check their CRC without CRC_EXTRA, reports them as bad CRC,
and mavlink_parse_char() drops them.
Frames are now queued as they arrived, checksum and signature included,
and written out as is. Following the MAVLink routing guide, frames that
can't be processed locally are still forwarded but not handled: unknown
MAVLink 2 messages, and messages whose signature can't be verified. The
signature isn't checked or removed, that's up to the receiver, which
may well have a different key than PX4. SETUP_SIGNING is still never
forwarded.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* fix(mavlink): only apply SETUP_SIGNING addressed to us
The target of SETUP_SIGNING was ignored, so a key meant for another
system or component, e.g. a companion computer, was applied to PX4 and
all its links instead.
SETUP_SIGNING is now only applied when it is broadcast or addressed to
us. Otherwise it is dropped, as it must never be forwarded, and a
warning tells the user that it didn't reach its target.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* refactor(mavlink): forward messages from one place
Messages which were handled locally were forwarded from the end of
Mavlink::handle_message(), while frames which can only be forwarded
took a separate path in the receive loop. Forwarding is now decided in
one place in the receive loop for both, and Mavlink::handle_message()
only deals with SETUP_SIGNING.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* refactor(mavlink): check forwarding enabled only once
Whether to forward is decided by forward_if_enabled(), so
forward_only_frame() only needs to decide whether a frame is valid but
can't be processed locally. As a result, unknown and incorrectly signed
frames no longer count as parse errors, independent of whether
forwarding is enabled.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* feat(mavlink): count unknown and incorrectly signed messages
Messages which are not in our dialect or whose signature can't be
verified are not processed and no longer count as parse errors. Count
them per link in telemetry_status instead, so they show up in the log
and in mavlink status, e.g. to find a sender using the wrong key.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* fix(mavlink): count lost messages correctly across sequence wrap-around
The sequence number wraps from 255 to 0, so the gap across the wrap is
seq + 256 - expected, not seq + 255 - expected. Every wrap-around with
loss undercounted by one message.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* fix(mavlink): report rx message loss as a fraction of all messages
rx_message_lost_rate was lost / received, which exceeds 1 with heavy
loss and is 0/0 before anything was received, and mavlink status then
printed that ratio as a percentage without scaling it. It is now
lost / (received + lost), 0 if nothing was received yet, and printed
as a percentage.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* fix(mavlink): don't forward unauthenticated frames when signing is active
With a signing key loaded, frames which fail PX4's signature check were
forwarded to the other links. PX4 keeps unauthenticated traffic from
e.g. the radio away from components which don't sign themselves, as the
signing docs describe, so drop them again, and count them as bad
signatures.
Unknown messages are reported as bad CRC by the parser, which doesn't
report their signature result, so apply the same rules to them: with
signing active they are only forwarded if signed with PX4's key or
allowed unsigned.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* fix(mavlink): don't count forwarded unknown messages as lost
Unknown messages didn't advance the sender's sequence, so the next
message from it counted them as lost. Track their sequence as well, but
only for components we have already seen: the header of an unknown
message isn't CRC checked, and garbage IDs would fill the component
table, which is also used for routing.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* fix(mavlink): forward signed frames PX4 can't verify
With signing active, a signed frame which fails PX4's check might be
signed with the receiver's key, so forward it for the receiver to
check, as the MAVLink routing guide says, but don't process it.
Unsigned frames are still dropped, apart from the unsigned allowlist,
so an attacker still can't send unsigned messages through PX4.
Suggested by @dakejahl in review.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* fix(mavlink): drop signed frames PX4 can't verify again
This reverts commit 80639d05d8. Setting the signed flag and appending a
random signature is trivial, so forwarding signed frames that fail
verification would let an attacker reach components behind PX4 that
don't check signatures themselves, just like unsigned frames. In
practice all systems share one key anyway, so with signing active only
forward what verifies with PX4's key, or is on the unsigned allowlist.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
* docs(mavlink): signing assumes one shared key
Make explicit that all systems and components share one symmetric key,
that components communicating through PX4 need to use PX4's key, and
that incorrectly signed messages are not forwarded, so they don't reach
components behind PX4 which don't check signatures themselves.
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
---------
Signed-off-by: Julian Oes <julian@oes.ch>
This commit is contained in:
1 parent
0424edb0da
commit
107d8bb038
6 files changed
+218
-35
No files matched your search
@@ -10,7 +10,12 @@ This mechanism does not _encrypt_ the message payload.
|
||||
|
||||
When signing is enabled, PX4 appends a 13-byte [signature](https://mavlink.io/en/guide/message_signing.html#signature) to every outgoing MAVLink 2 message.
|
||||
|
||||
Incoming messages are checked against the shared secret key, and unsigned or incorrectly signed messages are rejected (with [exceptions for safety-critical messages](#unsigned-message-allowlist)).
|
||||
Signing uses a single secret key, shared by all MAVLink systems and components that communicate with each other: the ground station, PX4, and any companion computer, camera, or gimbal connected to it.
|
||||
|
||||
Incoming messages are checked against this key, and unsigned or incorrectly signed messages are rejected (with [exceptions for safety-critical messages](#unsigned-message-allowlist)).
|
||||
Rejected messages are not forwarded to other links either.
|
||||
This includes messages that PX4 does not know (for example custom messages), which are only forwarded if they are signed with the shared key.
|
||||
Components that communicate through PX4 therefore need to use the same key as PX4.
|
||||
|
||||
The signing implementation is built into the MAVLink module and is always available, with no special build flags required.
|
||||
The key is stored in an SD card:
|
||||
@@ -58,6 +63,7 @@ When signing is **not active** (no key provisioned), the first `SETUP_SIGNING` w
|
||||
When signing is **already active**, key changes (including disabling) require that the `SETUP_SIGNING` message is signed with the current key.
|
||||
|
||||
Note that `SETUP_SIGNING` is rejected while the vehicle is armed (disarm before provisioning or changing keys).
|
||||
`SETUP_SIGNING` is only applied if it is broadcast or addressed to PX4 (its target system and component).
|
||||
As per the MAVLink specification, `SETUP_SIGNING` is never forwarded to other links.
|
||||
|
||||
## Key Storage
|
||||
@@ -134,9 +140,16 @@ When signing is active, **all links require signed messages**.
|
||||
This means:
|
||||
|
||||
- An attacker cannot send unsigned commands on any link.
|
||||
- Unsigned or incorrectly signed messages are not forwarded, so they don't reach components connected through PX4 (for example a companion computer), even if those don't check signatures themselves.
|
||||
- Changing or disabling the key requires sending a `SETUP_SIGNING` message **signed with the current key**.
|
||||
- Signing can be disabled via MAVLink by sending a signed `SETUP_SIGNING` with an all-zero key.
|
||||
|
||||
### One shared key
|
||||
|
||||
The key is symmetric: every device that holds it can both verify and sign messages, as any system and component ID.
|
||||
Signing therefore authenticates that a message comes from a device holding the key, not which device sent it.
|
||||
Only provision the key on devices you trust.
|
||||
|
||||
### Armed guard
|
||||
|
||||
`SETUP_SIGNING` is rejected while the vehicle is armed.
|
||||
|
||||
@@ -32,7 +32,9 @@ uint32 rx_message_lost_count
|
||||
uint32 rx_buffer_overruns # number of RX buffer overruns
|
||||
uint32 rx_parse_errors # number of parse errors
|
||||
uint32 rx_packet_drop_count # number of packet drops
|
||||
float32 rx_message_lost_rate
|
||||
uint32 rx_unknown_message_count # number of messages not in the dialect, not processed
|
||||
uint32 rx_bad_signature_count # number of messages with missing or invalid signature, not processed
|
||||
float32 rx_message_lost_rate # fraction of messages lost (0 to 1)
|
||||
|
||||
|
||||
uint64 HEARTBEAT_TIMEOUT_US = 2500000 # Heartbeat timeout (tolerate missing 1 + jitter)
|
||||
|
||||
@@ -588,6 +588,12 @@ Mavlink::component_was_seen(int system_id, int component_id, Mavlink &self)
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool target_is_us_or_broadcast(int target_system_id, int target_component_id)
|
||||
{
|
||||
return (target_system_id == 0 || target_system_id == mavlink_system.sysid)
|
||||
&& (target_component_id == 0 || target_component_id == mavlink_system.compid);
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
|
||||
{
|
||||
@@ -608,6 +614,16 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
|
||||
}
|
||||
}
|
||||
|
||||
// SETUP_SIGNING must never be forwarded (MAVLink spec requirement).
|
||||
if (msg->msgid == MAVLINK_MSG_ID_SETUP_SIGNING) {
|
||||
if (!target_is_us_or_broadcast(target_system_id, target_component_id)) {
|
||||
mavlink_log_warning(&self->_mavlink_log_pub, "MAVLink signing: SETUP_SIGNING for %d/%d not forwarded",
|
||||
target_system_id, target_component_id);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// Avoid locking/iteration when there is no instance to forward to.
|
||||
if (mavlink_instance_count.load() <= 1) {
|
||||
return;
|
||||
@@ -1181,8 +1197,16 @@ Mavlink::handle_message(const mavlink_message_t *msg)
|
||||
* NOTE: this is called from the receiver thread
|
||||
*/
|
||||
|
||||
// SETUP_SIGNING must never be forwarded to other links (MAVLink spec requirement).
|
||||
if (msg->msgid == MAVLINK_MSG_ID_SETUP_SIGNING) {
|
||||
if (msg->msgid != MAVLINK_MSG_ID_SETUP_SIGNING) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Only apply SETUP_SIGNING if it is meant for us. It is never forwarded,
|
||||
// forward_message() drops it and warns if it was meant for someone else.
|
||||
mavlink_setup_signing_t setup_signing;
|
||||
mavlink_msg_setup_signing_decode(msg, &setup_signing);
|
||||
|
||||
if (target_is_us_or_broadcast(setup_signing.target_system, setup_signing.target_component)) {
|
||||
// Reject signing changes while armed
|
||||
vehicle_status_s vehicle_status{};
|
||||
|
||||
@@ -1223,9 +1247,15 @@ Mavlink::handle_message(const mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Mavlink::forward_if_enabled(const mavlink_message_t *msg)
|
||||
{
|
||||
/*
|
||||
* NOTE: this is called from the receiver thread
|
||||
*/
|
||||
|
||||
if (get_forwarding_on()) {
|
||||
/* forward any messages to other mavlink instances */
|
||||
@@ -1456,11 +1486,46 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
|
||||
void
|
||||
Mavlink::pass_message(const mavlink_message_t *msg)
|
||||
{
|
||||
/* size is 12 bytes plus variable payload */
|
||||
int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
|
||||
// Queue the frame as it came in, so checksum and signature stay valid.
|
||||
// mavlink_msg_to_send_buffer() can't be used because it trims the payload
|
||||
// again, which would break the checksum of an untrimmed frame.
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||
size_t size = 0;
|
||||
|
||||
buf[size++] = msg->magic;
|
||||
buf[size++] = msg->len;
|
||||
|
||||
if (msg->magic == MAVLINK_STX_MAVLINK1) {
|
||||
buf[size++] = msg->seq;
|
||||
buf[size++] = msg->sysid;
|
||||
buf[size++] = msg->compid;
|
||||
buf[size++] = msg->msgid & 0xFF;
|
||||
|
||||
} else {
|
||||
buf[size++] = msg->incompat_flags;
|
||||
buf[size++] = msg->compat_flags;
|
||||
buf[size++] = msg->seq;
|
||||
buf[size++] = msg->sysid;
|
||||
buf[size++] = msg->compid;
|
||||
buf[size++] = msg->msgid & 0xFF;
|
||||
buf[size++] = (msg->msgid >> 8) & 0xFF;
|
||||
buf[size++] = (msg->msgid >> 16) & 0xFF;
|
||||
}
|
||||
|
||||
memcpy(&buf[size], _MAV_PAYLOAD(msg), msg->len);
|
||||
size += msg->len;
|
||||
|
||||
buf[size++] = msg->ck[0];
|
||||
buf[size++] = msg->ck[1];
|
||||
|
||||
if (msg->magic != MAVLINK_STX_MAVLINK1 && (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)) {
|
||||
memcpy(&buf[size], msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN);
|
||||
size += MAVLINK_SIGNATURE_BLOCK_LEN;
|
||||
}
|
||||
|
||||
LockGuard lg{_message_buffer_mutex};
|
||||
|
||||
if (!_message_buffer.push_back(reinterpret_cast<const uint8_t *>(msg), size)) {
|
||||
if (!_message_buffer.push_back(buf, size)) {
|
||||
perf_count(_forwarding_error_perf);
|
||||
}
|
||||
}
|
||||
@@ -2824,19 +2889,22 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
/* pass messages from other instances */
|
||||
if (get_forwarding_on()) {
|
||||
|
||||
mavlink_message_t msg;
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||||
size_t available_bytes;
|
||||
{
|
||||
// We only send one message at a time, not to put too much strain on a
|
||||
// link from forwarded messages.
|
||||
LockGuard lg{_message_buffer_mutex};
|
||||
available_bytes = _message_buffer.pop_front(reinterpret_cast<uint8_t *>(&msg), sizeof(msg));
|
||||
available_bytes = _message_buffer.pop_front(buf, sizeof(buf));
|
||||
// We need to make sure to release the lock here before sending the
|
||||
// bytes out via IP or UART which could potentially take longer.
|
||||
}
|
||||
|
||||
if (available_bytes > 0) {
|
||||
resend_message(&msg);
|
||||
// Send the frame unchanged, sequence number, checksum and signature included.
|
||||
send_start(available_bytes);
|
||||
send_bytes(buf, available_bytes);
|
||||
send_finish();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3456,7 +3524,9 @@ Mavlink::display_status()
|
||||
printf("\t tx rate mult: %.3f\n", (double)_rate_mult);
|
||||
printf("\t tx rate max: %i B/s\n", _datarate);
|
||||
printf("\t rx: %.1f B/s\n", (double)_tstatus.rx_rate_avg);
|
||||
printf("\t rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate);
|
||||
printf("\t rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate * 100.0);
|
||||
printf("\t rx unknown messages: %" PRIu32 "\n", _tstatus.rx_unknown_message_count);
|
||||
printf("\t rx bad signatures: %" PRIu32 "\n", _tstatus.rx_bad_signature_count);
|
||||
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
_receiver.print_detailed_rx_stats();
|
||||
|
||||
@@ -318,13 +318,13 @@ public:
|
||||
*/
|
||||
void send_finish();
|
||||
|
||||
/**
|
||||
* Resend message as is, don't change sequence number and CRC.
|
||||
*/
|
||||
void resend_message(mavlink_message_t *msg) { _mavlink_resend_uart(_channel, msg); }
|
||||
|
||||
void handle_message(const mavlink_message_t *msg);
|
||||
|
||||
/**
|
||||
* Forward a received message to the other instances if forwarding is enabled.
|
||||
*/
|
||||
void forward_if_enabled(const mavlink_message_t *msg);
|
||||
|
||||
int get_instance_id() const { return _instance_id; }
|
||||
|
||||
/**
|
||||
|
||||
@@ -525,7 +525,7 @@ void MavlinkReceiver::handle_messages_in_gimbal_mode(mavlink_message_t &msg)
|
||||
break;
|
||||
}
|
||||
|
||||
// Message forwarding
|
||||
/* handle packet with parent object */
|
||||
_mavlink.handle_message(&msg);
|
||||
}
|
||||
|
||||
@@ -3974,10 +3974,16 @@ MavlinkReceiver::run()
|
||||
// waiting for lock_send(), producing a circular wait. Individual handlers
|
||||
// that actually send take lock_send() locally.
|
||||
_mavlink.lock_send();
|
||||
const uint8_t parsed = mavlink_parse_char(_mavlink.get_channel(), buf[i], &msg, &_status);
|
||||
const uint8_t framing = mavlink_frame_char(_mavlink.get_channel(), buf[i], &msg, &_status);
|
||||
const FrameCheck frame_check = check_frame(framing, msg);
|
||||
|
||||
if (frame_check == FrameCheck::Invalid) {
|
||||
reset_parser_after_rejected_frame(buf[i]);
|
||||
}
|
||||
|
||||
_mavlink.unlock_send();
|
||||
|
||||
if (parsed) {
|
||||
if (frame_check == FrameCheck::Ok) {
|
||||
|
||||
// If we receive a complete MAVLink 2 packet, also switch the outgoing protocol version.
|
||||
// Read flags from the receiver-local _status (mavlink_parse_char copies flags from the
|
||||
@@ -4001,11 +4007,25 @@ MavlinkReceiver::run()
|
||||
}
|
||||
|
||||
_mavlink.set_has_received_messages(true); // Received first message, unlock wait to transmit '-w' command-line flag
|
||||
update_rx_stats(msg);
|
||||
update_rx_stats(msg, true);
|
||||
|
||||
if (_message_statistics_enabled) {
|
||||
update_message_statistics(msg);
|
||||
}
|
||||
|
||||
} else if (frame_check == FrameCheck::ForwardOnly) {
|
||||
_unknown_message_counter++;
|
||||
|
||||
// The header of an unknown message isn't CRC checked, so only
|
||||
// track the sequence of components we have already seen.
|
||||
update_rx_stats(msg, false);
|
||||
|
||||
} else if (frame_check == FrameCheck::BadSignature) {
|
||||
_bad_signature_counter++;
|
||||
}
|
||||
|
||||
if (frame_check == FrameCheck::Ok || frame_check == FrameCheck::ForwardOnly) {
|
||||
_mavlink.forward_if_enabled(&msg);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4019,7 +4039,11 @@ MavlinkReceiver::run()
|
||||
telemetry_status_s &tstatus = _mavlink.telemetry_status();
|
||||
tstatus.rx_message_count = _total_received_counter;
|
||||
tstatus.rx_message_lost_count = _total_lost_counter;
|
||||
tstatus.rx_message_lost_rate = static_cast<float>(_total_lost_counter) / static_cast<float>(_total_received_counter);
|
||||
tstatus.rx_unknown_message_count = _unknown_message_counter;
|
||||
tstatus.rx_bad_signature_count = _bad_signature_counter;
|
||||
const uint64_t total_messages = _total_received_counter + _total_lost_counter;
|
||||
tstatus.rx_message_lost_rate = (total_messages > 0)
|
||||
? static_cast<float>(_total_lost_counter) / static_cast<float>(total_messages) : 0.f;
|
||||
|
||||
if (_mavlink_status_last_buffer_overrun != _status.buffer_overrun) {
|
||||
tstatus.rx_buffer_overruns++;
|
||||
@@ -4077,6 +4101,61 @@ MavlinkReceiver::run()
|
||||
}
|
||||
}
|
||||
|
||||
MavlinkReceiver::FrameCheck MavlinkReceiver::check_frame(uint8_t framing, const mavlink_message_t &message)
|
||||
{
|
||||
switch (framing) {
|
||||
case MAVLINK_FRAMING_OK:
|
||||
return FrameCheck::Ok;
|
||||
|
||||
case MAVLINK_FRAMING_BAD_SIGNATURE:
|
||||
// With signing enabled, PX4 keeps unauthenticated traffic away from the
|
||||
// other links, so we don't forward it either.
|
||||
return FrameCheck::BadSignature;
|
||||
|
||||
case MAVLINK_FRAMING_BAD_CRC:
|
||||
break;
|
||||
|
||||
default:
|
||||
return FrameCheck::Incomplete;
|
||||
}
|
||||
|
||||
// Messages which are not in our dialect can't have their CRC checked and
|
||||
// are reported as bad CRC by the parser. They can still be forwarded.
|
||||
if ((mavlink_get_msg_entry(message.msgid) != nullptr) || (_status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) {
|
||||
return FrameCheck::Invalid;
|
||||
}
|
||||
|
||||
// The parser doesn't report the signature result of a bad CRC frame, so
|
||||
// apply the same rules as for known messages ourselves.
|
||||
const mavlink_signing_t *signing = _mavlink.get_status()->signing;
|
||||
|
||||
if (signing == nullptr) {
|
||||
return FrameCheck::ForwardOnly;
|
||||
}
|
||||
|
||||
if (message.incompat_flags & MAVLINK_IFLAG_SIGNED) {
|
||||
// The parser has checked the signature of this frame already.
|
||||
return (signing->last_status == MAVLINK_SIGNING_STATUS_OK) ? FrameCheck::ForwardOnly : FrameCheck::BadSignature;
|
||||
}
|
||||
|
||||
return _mavlink.accept_unsigned(message.msgid) ? FrameCheck::ForwardOnly : FrameCheck::BadSignature;
|
||||
}
|
||||
|
||||
void MavlinkReceiver::reset_parser_after_rejected_frame(uint8_t c)
|
||||
{
|
||||
mavlink_status_t *status = _mavlink.get_status();
|
||||
_mav_parse_error(status);
|
||||
status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
|
||||
if (c == MAVLINK_STX) {
|
||||
mavlink_message_t *rxmsg = _mavlink.get_buffer();
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
||||
rxmsg->len = 0;
|
||||
mavlink_start_checksum(rxmsg);
|
||||
}
|
||||
}
|
||||
|
||||
bool MavlinkReceiver::component_was_seen(int system_id, int component_id)
|
||||
{
|
||||
// For system broadcast messages return true if at least one component was seen before
|
||||
@@ -4094,22 +4173,17 @@ bool MavlinkReceiver::component_was_seen(int system_id, int component_id)
|
||||
return false;
|
||||
}
|
||||
|
||||
void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message)
|
||||
void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message, bool add_component)
|
||||
{
|
||||
const bool component_states_has_still_space = [this, &message]() {
|
||||
const bool component_states_has_still_space = [this, &message, add_component]() {
|
||||
for (unsigned i = 0; i < MAX_REMOTE_COMPONENTS; ++i) {
|
||||
if (_component_states[i].system_id == message.sysid && _component_states[i].component_id == message.compid) {
|
||||
|
||||
int lost_messages = 0;
|
||||
const uint8_t expected_seq = _component_states[i].last_sequence + 1;
|
||||
|
||||
// Account for overflow during packet loss
|
||||
if (message.seq < expected_seq) {
|
||||
lost_messages = (message.seq + 255) - expected_seq;
|
||||
|
||||
} else {
|
||||
lost_messages = message.seq - expected_seq;
|
||||
}
|
||||
// The sequence number is 8 bit, so the difference modulo 256 is the
|
||||
// number of lost messages, also across the wrap-around.
|
||||
const int lost_messages = static_cast<uint8_t>(message.seq - expected_seq);
|
||||
|
||||
_component_states[i].missed_messages += lost_messages;
|
||||
|
||||
@@ -4123,6 +4197,10 @@ void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message)
|
||||
return true;
|
||||
|
||||
} else if (_component_states[i].system_id == 0 && _component_states[i].component_id == 0) {
|
||||
if (!add_component) {
|
||||
return true;
|
||||
}
|
||||
|
||||
_component_states[i].system_id = message.sysid;
|
||||
_component_states[i].component_id = message.compid;
|
||||
|
||||
@@ -4141,7 +4219,7 @@ void MavlinkReceiver::update_rx_stats(const mavlink_message_t &message)
|
||||
return false;
|
||||
}();
|
||||
|
||||
if (!component_states_has_still_space && !_warned_component_states_full_once) {
|
||||
if (add_component && !component_states_has_still_space && !_warned_component_states_full_once) {
|
||||
PX4_WARN("Max remote components of %u used up", MAX_REMOTE_COMPONENTS);
|
||||
_warned_component_states_full_once = true;
|
||||
}
|
||||
|
||||
@@ -282,7 +282,25 @@ private:
|
||||
void schedule_tune(const char *tune);
|
||||
|
||||
void update_message_statistics(const mavlink_message_t &message);
|
||||
void update_rx_stats(const mavlink_message_t &message);
|
||||
/**
|
||||
* @param add_component false to only update components which were already seen
|
||||
*/
|
||||
void update_rx_stats(const mavlink_message_t &message, bool add_component);
|
||||
|
||||
enum class FrameCheck {
|
||||
Incomplete, ///< no complete frame yet
|
||||
Ok, ///< valid, handle and forward
|
||||
ForwardOnly, ///< not in our dialect, can't be handled but can be forwarded
|
||||
BadSignature, ///< signature missing or not valid with our key, drop
|
||||
Invalid, ///< bad CRC, drop
|
||||
};
|
||||
|
||||
FrameCheck check_frame(uint8_t framing, const mavlink_message_t &message);
|
||||
|
||||
/**
|
||||
* Reset the parser after a rejected frame, same as mavlink_parse_char() does.
|
||||
*/
|
||||
void reset_parser_after_rejected_frame(uint8_t c);
|
||||
|
||||
void publish_hil_battery();
|
||||
void publish_rtcm_corrections(const uint8_t *data, size_t len);
|
||||
@@ -334,6 +352,8 @@ private:
|
||||
|
||||
uint64_t _total_received_counter{0}; ///< The total number of successfully received messages
|
||||
uint64_t _total_lost_counter{0}; ///< Total messages lost during transmission.
|
||||
uint32_t _unknown_message_counter{0}; ///< Messages not in our dialect
|
||||
uint32_t _bad_signature_counter{0}; ///< Messages with missing or invalid signature
|
||||
|
||||
uint8_t _mavlink_status_last_buffer_overrun{0};
|
||||
uint8_t _mavlink_status_last_parse_error{0};
|
||||
|
||||
Reference in new issue
Block a user