core: fix MAVLink message sequence (#2762)

* core: fix MAVLink message sequence

The MAVLink headers use an inline function to get the mavlink channel,
so we end up having separate sequence numbers in different translation
units. By defining this function ourselves and linking it later, we make
sure to use the same sequence everywhere.

This should fix seq numbering that's invalid (per component).

We also add a system test to actually test this.

* system_tests: fixup thread sanitizer issues

* system_tests: use TCP for seq to avoid drops

Author: julianoes

src/mavsdk/core/CMakeLists.txt

@@ -62,6 +62,7 @@ target_sources(mavsdk
     math_utils.cpp
     mavsdk.cpp
     mavsdk_impl.cpp
+    mavlink_channel_status.cpp
     mavlink_channels.cpp
     mavlink_command_receiver.cpp
     mavlink_command_sender.cpp

src/mavsdk/core/include/mavsdk/mavlink_include.h.in

@@ -1,3 +1,8 @@
 #pragma once
 
+#include "mavlink/mavlink_types.h"
+
+#define MAVLINK_GET_CHANNEL_STATUS
+mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
+
 #include "mavlink/@MAVLINK_DIALECT@/mavlink.h"

src/mavsdk/core/mavlink_channel_status.cpp

@@ -0,0 +1,7 @@
+#include "mavlink_include.h"
+
+mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
+{
+    static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
+    return &m_mavlink_status[chan];
+}

src/mavsdk/plugins/camera_server/camera_server_impl.cpp

@@ -1769,27 +1769,29 @@ std::optional<mavlink_command_ack_t> CameraServerImpl::process_video_stream_info
 
         _video_streaming.rtsp_uri.resize(sizeof(mavlink_video_stream_information_t::uri));
 
-        mavlink_message_t msg{};
-        mavlink_msg_video_stream_information_pack(
-            _server_component_impl->get_own_system_id(),
-            _server_component_impl->get_own_component_id(),
-            &msg,
-            0, // Stream id
-            0, // Count
-            VIDEO_STREAM_TYPE_RTSP,
-            VIDEO_STREAM_STATUS_FLAGS_RUNNING,
-            0, // famerate
-            0, // resolution horizontal
-            0, // resolution vertical
-            0, // bitrate
-            0, // rotation
-            0, // horizontal field of view
-            name,
-            _video_streaming.rtsp_uri.c_str(),
-            0,
-            0);
-
-        _server_component_impl->send_message(msg);
+        _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
+            mavlink_message_t msg{};
+            mavlink_msg_video_stream_information_pack_chan(
+                mavlink_address.system_id,
+                mavlink_address.component_id,
+                channel,
+                &msg,
+                0, // Stream id
+                0, // Count
+                VIDEO_STREAM_TYPE_RTSP,
+                VIDEO_STREAM_STATUS_FLAGS_RUNNING,
+                0, // famerate
+                0, // resolution horizontal
+                0, // resolution vertical
+                0, // bitrate
+                0, // rotation
+                0, // horizontal field of view
+                name,
+                _video_streaming.rtsp_uri.c_str(),
+                0,
+                0);
+            return msg;
+        });
 
         // Ack already sent.
         return std::nullopt;
@@ -1819,21 +1821,24 @@ std::optional<mavlink_command_ack_t> CameraServerImpl::process_video_stream_stat
     _server_component_impl->send_command_ack(command_ack);
     LogDebug() << "sent video streaming ack";
 
-    mavlink_message_t msg{};
-    mavlink_msg_video_stream_status_pack(
-        _server_component_impl->get_own_system_id(),
-        _server_component_impl->get_own_component_id(),
-        &msg,
-        0, // Stream id
-        VIDEO_STREAM_STATUS_FLAGS_RUNNING,
-        0, // framerate
-        0, // resolution horizontal
-        0, // resolution vertical
-        0, // bitrate
-        0, // rotation
-        0, // horizontal field of view
-        0);
-    _server_component_impl->send_message(msg);
+    _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
+        mavlink_message_t msg{};
+        mavlink_msg_video_stream_status_pack_chan(
+            mavlink_address.system_id,
+            mavlink_address.component_id,
+            channel,
+            &msg,
+            0, // Stream id
+            VIDEO_STREAM_STATUS_FLAGS_RUNNING,
+            0, // framerate
+            0, // resolution horizontal
+            0, // resolution vertical
+            0, // bitrate
+            0, // rotation
+            0, // horizontal field of view
+            0);
+        return msg;
+    });
 
     // ack was already sent
     return std::nullopt;

src/mavsdk/plugins/events/event_handler.cpp

@@ -38,10 +38,12 @@ EventHandler::EventHandler(
     };
 
     const auto send_request_cb = [this](const mavlink_request_event_t& msg) {
-        mavlink_message_t message;
-        mavlink_msg_request_event_encode(
-            _system_impl.get_own_system_id(), _system_impl.get_own_component_id(), &message, &msg);
-        _system_impl.send_message(message);
+        _system_impl.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
+            mavlink_message_t message;
+            mavlink_msg_request_event_encode_chan(
+                mavlink_address.system_id, mavlink_address.component_id, channel, &message, &msg);
+            return message;
+        });
     };
 
     _parser.setProfile(profile);

src/system_tests/CMakeLists.txt

@@ -28,6 +28,7 @@ add_executable(system_tests_runner
     intercept.cpp
     mavlink_direct.cpp
     mavlink_direct_forwarding.cpp
+    mavlink_seq.cpp
     mission_cancellation.cpp
     mission_changed.cpp
     mission_transfer_lossy.cpp

src/system_tests/intercept.cpp

@@ -86,8 +86,10 @@ TEST(SystemTest, InterceptIncomingModifyLocal)
             pos.lat = static_cast<int32_t>(modified_lat * 1e7);
             pos.lon = static_cast<int32_t>(modified_lon * 1e7);
 
-            // Re-encode the modified message
-            mavlink_msg_global_position_int_encode(message.sysid, message.compid, &message, &pos);
+            // Re-encode the modified message using a dedicated channel to avoid
+            // racing with ServerComponentImpl::queue_message on channel 0.
+            mavlink_msg_global_position_int_encode_chan(
+                message.sysid, message.compid, MAVLINK_COMM_NUM_BUFFERS - 1, &message, &pos);
 
             LogInfo() << "Modified coordinates to San Francisco: lat=" << (pos.lat / 1e7)
                       << ", lon=" << (pos.lon / 1e7);

src/system_tests/mavlink_seq.cpp

@@ -0,0 +1,63 @@
+#include "log.h"
+#include "mavsdk.h"
+#include <chrono>
+#include <thread>
+#include <mutex>
+#include <vector>
+#include <gtest/gtest.h>
+
+using namespace mavsdk;
+
+TEST(SystemTest, MavlinkSeqSequential)
+{
+    Mavsdk mavsdk_autopilot{Mavsdk::Configuration{ComponentType::Autopilot}};
+    Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}};
+
+    ASSERT_EQ(
+        mavsdk_groundstation.add_any_connection("tcpin://0.0.0.0:17012"),
+        ConnectionResult::Success);
+    ASSERT_EQ(
+        mavsdk_autopilot.add_any_connection("tcpout://127.0.0.1:17012"), ConnectionResult::Success);
+
+    auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);
+    ASSERT_TRUE(maybe_system);
+
+    std::mutex seq_mutex;
+    std::vector<uint8_t> seq_numbers;
+
+    constexpr unsigned num_messages = 10;
+
+    // Intercept all incoming messages on the ground station and record
+    // sequence numbers from the autopilot system (sysid 1, compid 1).
+    mavsdk_groundstation.intercept_incoming_messages_async([&](mavlink_message_t& message) -> bool {
+        if (message.sysid == 1 && message.compid == MAV_COMP_ID_AUTOPILOT1) {
+            std::lock_guard<std::mutex> lock(seq_mutex);
+            seq_numbers.push_back(message.seq);
+        }
+        return true;
+    });
+
+    // Collect messages (heartbeats at 1 Hz plus any other traffic).
+    for (unsigned i = 0; i < 150; ++i) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        std::lock_guard<std::mutex> lock(seq_mutex);
+        if (seq_numbers.size() >= num_messages) {
+            break;
+        }
+    }
+
+    // Stop intercepting.
+    mavsdk_groundstation.intercept_incoming_messages_async(nullptr);
+
+    std::lock_guard<std::mutex> lock(seq_mutex);
+    ASSERT_GE(seq_numbers.size(), num_messages);
+
+    // Verify that all messages from the autopilot component have strictly
+    // sequential seq numbers (incrementing by 1 with uint8_t wrapping).
+    for (unsigned i = 1; i < num_messages; ++i) {
+        uint8_t expected = static_cast<uint8_t>(seq_numbers[i - 1] + 1);
+        EXPECT_EQ(seq_numbers[i], expected)
+            << "seq[" << i - 1 << "]=" << static_cast<int>(seq_numbers[i - 1]) << " seq[" << i
+            << "]=" << static_cast<int>(seq_numbers[i]);
+    }
+}

src/system_tests/raw_bytes.cpp

@@ -85,7 +85,7 @@ TEST(SystemTest, RawBytesSendReceive)
     ping.target_system = 0; // Broadcast
     ping.target_component = 0;
 
-    mavlink_msg_ping_encode(1, 1, &ping_msg, &ping);
+    mavlink_msg_ping_encode_chan(1, 1, MAVLINK_COMM_NUM_BUFFERS - 1, &ping_msg, &ping);
 
     // Serialize to bytes
     std::vector<uint8_t> ping_bytes(MAVLINK_MAX_PACKET_LEN);