21724008972

Committed 05 Feb 2026 06:42PM UTC coverage: 49.029% (+0.07%) from 48.959%

Build # 21724008972

Build Type

push

github

Committed by

web-flow

Commit Message

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

Run Details

51 of 56 new or added lines in 6 files covered. (91.07%)

5 existing lines in 4 files now uncovered.

18362 of 37451 relevant lines covered (49.03%)

671.17 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

99.15

/src/system_tests/intercept.cpp

#include "log.h"
#include "mavsdk.h"
#include "plugins/telemetry/telemetry.h"
#include "plugins/telemetry_server/telemetry_server.h"
#include <chrono>
#include <thread>
#include <future>
#include <gtest/gtest.h>
#include <json/json.h>

using namespace mavsdk;

TEST(SystemTest, InterceptIncomingModifyLocal)
{
    // Create 3 MAVSDK instances: Sender -> Interceptor/Forwarder -> Receiver
    Mavsdk mavsdk_sender{Mavsdk::Configuration{ComponentType::Autopilot}};
    Mavsdk mavsdk_interceptor{Mavsdk::Configuration{ComponentType::GroundStation}};
    Mavsdk mavsdk_receiver{Mavsdk::Configuration{ComponentType::GroundStation}};

    // Set up connections with forwarding enabled on interceptor
    ASSERT_EQ(
        mavsdk_sender.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);
    ASSERT_EQ(
        mavsdk_interceptor.add_any_connection(
            "udpin://0.0.0.0:17000", ForwardingOption::ForwardingOn),
        ConnectionResult::Success);
    ASSERT_EQ(
        mavsdk_interceptor.add_any_connection(
            "udpout://127.0.0.1:17001", ForwardingOption::ForwardingOn),
        ConnectionResult::Success);
    ASSERT_EQ(
        mavsdk_receiver.add_any_connection("udpin://0.0.0.0:17001"), ConnectionResult::Success);

    // Wait for connections to establish
    LogInfo() << "Waiting for connections to establish...";

    // Interceptor discovers sender
    auto sender_system_opt = mavsdk_interceptor.first_autopilot(10.0);
    ASSERT_TRUE(sender_system_opt);
    auto sender_system = sender_system_opt.value();
    ASSERT_TRUE(sender_system->has_autopilot());

    // Receiver discovers sender (via forwarding)
    auto sender_system_receiver_opt = mavsdk_receiver.first_autopilot(10.0);
    ASSERT_TRUE(sender_system_receiver_opt);
    auto sender_system_receiver = sender_system_receiver_opt.value();
    ASSERT_TRUE(sender_system_receiver->has_autopilot());

    // Wait for sender to discover interceptor connection
    LogInfo() << "Waiting for sender to connect to interceptor...";
    while (mavsdk_sender.systems().size() == 0) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    auto interceptor_system = mavsdk_sender.systems().at(0);

    // Create telemetry instances
    auto telemetry_interceptor = Telemetry{sender_system};
    auto telemetry_receiver = Telemetry{sender_system_receiver};
    auto telemetry_server = TelemetryServer{mavsdk_sender.server_component()};

    // Original position values
    const double original_lat = 47.3977421; // Zurich coordinates
    const double original_lon = 8.5455938;
    const float original_alt = 488.0f;

    // Modified position values (what intercept will change to)
    const double modified_lat = 37.7749295; // San Francisco coordinates
    const double modified_lon = -122.4194155;

    // Set up raw mavlink intercept callback on interceptor to modify incoming GLOBAL_POSITION_INT
    std::atomic<bool> intercept_called{false};

    mavsdk_interceptor.intercept_incoming_messages_async([&](mavlink_message_t& message) -> bool {
        if (message.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
            LogInfo() << "Intercepting GLOBAL_POSITION_INT message from system "
                      << (int)message.sysid;

            // Decode the message
            mavlink_global_position_int_t pos;
            mavlink_msg_global_position_int_decode(&message, &pos);

            LogInfo() << "Original coordinates: lat=" << (pos.lat / 1e7)
                      << ", lon=" << (pos.lon / 1e7);

            // Modify the coordinates to San Francisco
            pos.lat = static_cast<int32_t>(modified_lat * 1e7);
            pos.lon = static_cast<int32_t>(modified_lon * 1e7);

            // 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);

            intercept_called = true;
        }
        return true; // Keep the message
    });

    // Promise/future for interceptor telemetry
    auto interceptor_prom = std::promise<Telemetry::Position>();
    auto interceptor_fut = interceptor_prom.get_future();

    // Promise/future for receiver telemetry
    auto receiver_prom = std::promise<Telemetry::Position>();
    auto receiver_fut = receiver_prom.get_future();

    // Subscribe to position updates on interceptor
    auto interceptor_handle =
        telemetry_interceptor.subscribe_position([&interceptor_prom](Telemetry::Position position) {
            LogInfo() << "Interceptor received position: lat=" << position.latitude_deg
                      << ", lon=" << position.longitude_deg;
            interceptor_prom.set_value(position);
        });

    // Subscribe to position updates on receiver
    auto receiver_handle =
        telemetry_receiver.subscribe_position([&receiver_prom](Telemetry::Position position) {
            LogInfo() << "Receiver received position: lat=" << position.latitude_deg
                      << ", lon=" << position.longitude_deg;
            receiver_prom.set_value(position);
        });

    // Send position from sender
    LogInfo() << "Publishing original position from sender...";
    TelemetryServer::Position position{};
    position.latitude_deg = original_lat;
    position.longitude_deg = original_lon;
    position.absolute_altitude_m = original_alt;
    position.relative_altitude_m = original_alt;

    // Empty velocity and heading for this test
    TelemetryServer::VelocityNed velocity{};
    TelemetryServer::Heading heading{};

    ASSERT_EQ(
        telemetry_server.publish_position(position, velocity, heading),
        TelemetryServer::Result::Success);

    // Wait for both telemetry callbacks
    ASSERT_EQ(interceptor_fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);
    ASSERT_EQ(receiver_fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);

    // Give intercept callback time to complete
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    // Verify intercept was called
    ASSERT_TRUE(intercept_called.load());

    // Get the results
    auto interceptor_position = interceptor_fut.get();
    auto receiver_position = receiver_fut.get();

    // Verify interceptor sees modified coordinates (San Francisco)
    EXPECT_NEAR(interceptor_position.latitude_deg, modified_lat, 1e-6);
    EXPECT_NEAR(interceptor_position.longitude_deg, modified_lon, 1e-6);
    EXPECT_NEAR(interceptor_position.absolute_altitude_m, original_alt, 1.0f);

    // Receiver also gets modified coordinates because intercept happens BEFORE forwarding
    EXPECT_NEAR(
        receiver_position.latitude_deg,
        modified_lat,
        1e-6); // Should be modified San Francisco coordinates
    EXPECT_NEAR(
        receiver_position.longitude_deg,
        modified_lon,
        1e-6); // Should be modified San Francisco coordinates
    EXPECT_NEAR(receiver_position.absolute_altitude_m, original_alt, 1.0f);

    LogInfo() << "Test completed successfully:";
    LogInfo() << "  - Interceptor saw modified coordinates (San Francisco)";
    LogInfo() << "  - Receiver also saw modified coordinates (San Francisco)";
    LogInfo() << "  - This proves intercept affects BOTH local processing AND forwarding";

    // Cleanup
    telemetry_interceptor.unsubscribe_position(interceptor_handle);
    telemetry_receiver.unsubscribe_position(receiver_handle);
    mavsdk_interceptor.intercept_incoming_messages_async(nullptr);
}

TEST(SystemTest, InterceptJsonIncoming)
{
    Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}};
    Mavsdk mavsdk_autopilot{Mavsdk::Configuration{ComponentType::Autopilot}};

    ASSERT_EQ(
        mavsdk_groundstation.add_any_connection("udpin://0.0.0.0:17000"),
        ConnectionResult::Success);
    ASSERT_EQ(
        mavsdk_autopilot.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);

    // Ground station discovers the autopilot system
    auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);
    ASSERT_TRUE(maybe_system);
    auto system = maybe_system.value();
    ASSERT_TRUE(system->has_autopilot());

    // Wait for autopilot instance to discover the connection to the ground station
    LogInfo() << "Waiting for autopilot system to connect...";
    while (mavsdk_autopilot.systems().size() == 0) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    // Create telemetry instances
    auto telemetry = Telemetry{system};
    auto telemetry_server = TelemetryServer{mavsdk_autopilot.server_component()};

    // Set up incoming JSON message interception on ground station
    auto json_prom = std::promise<Mavsdk::MavlinkMessage>();
    auto json_fut = json_prom.get_future();
    std::atomic<bool> intercept_called{false};

    auto json_handle = mavsdk_groundstation.subscribe_incoming_messages_json(
        [&json_prom, &intercept_called](Mavsdk::MavlinkMessage json_message) {
            LogInfo() << "Intercepted incoming JSON message: " << json_message.message_name
                      << " from system " << json_message.system_id
                      << " with fields: " << json_message.fields_json;

            if (json_message.message_name == "GLOBAL_POSITION_INT") {
                intercept_called = true;
                json_prom.set_value(json_message);
                return true; // Don't drop the message
            }
            return true; // Let other messages through
        });

    // Publish position from autopilot
    LogInfo() << "Publishing position from autopilot...";
    TelemetryServer::Position position{};
    position.latitude_deg = 47.3977421; // Zurich coordinates
    position.longitude_deg = 8.5455938;
    position.absolute_altitude_m = 488.0f;
    position.relative_altitude_m = 488.0f;

    TelemetryServer::VelocityNed velocity{};
    velocity.north_m_s = 1.5f;
    velocity.east_m_s = 2.0f;
    velocity.down_m_s = -0.5f;

    TelemetryServer::Heading heading{};
    heading.heading_deg = 180.0f;

    ASSERT_EQ(
        telemetry_server.publish_position(position, velocity, heading),
        TelemetryServer::Result::Success);

    // Wait for JSON interception to occur
    ASSERT_EQ(json_fut.wait_for(std::chrono::seconds(1)), std::future_status::ready);

    // Verify intercept was called
    ASSERT_TRUE(intercept_called.load());

    auto intercepted_message = json_fut.get();

    // Verify the intercepted JSON message structure
    EXPECT_EQ(intercepted_message.message_name, "GLOBAL_POSITION_INT");
    EXPECT_EQ(intercepted_message.system_id, 1u); // Autopilot system ID
    EXPECT_GT(intercepted_message.fields_json.length(), 0u);

    // Parse JSON to verify field values
    Json::Value json;
    Json::Reader reader;
    ASSERT_TRUE(reader.parse(intercepted_message.fields_json, json));

    // Verify position fields from the intercepted message
    EXPECT_NEAR(json["lat"].asInt() / 1e7, position.latitude_deg, 1e-6);
    EXPECT_NEAR(json["lon"].asInt() / 1e7, position.longitude_deg, 1e-6);
    EXPECT_NEAR(json["alt"].asInt() / 1e3, position.absolute_altitude_m, 1.0);
    EXPECT_NEAR(json["relative_alt"].asInt() / 1e3, position.relative_altitude_m, 1.0);

    // Verify velocity fields
    EXPECT_NEAR(json["vx"].asInt() / 1e2, velocity.north_m_s, 0.1);
    EXPECT_NEAR(json["vy"].asInt() / 1e2, velocity.east_m_s, 0.1);
    EXPECT_NEAR(json["vz"].asInt() / 1e2, velocity.down_m_s, 0.1);

    // Verify heading field
    EXPECT_NEAR(json["hdg"].asInt() / 1e2, heading.heading_deg, 1.0);

    LogInfo() << "Successfully tested incoming JSON message interception";
    LogInfo() << "  - Message name: " << intercepted_message.message_name;
    LogInfo() << "  - System ID: " << intercepted_message.system_id;
    LogInfo() << "  - Fields JSON length: " << intercepted_message.fields_json.length();

    // Cleanup
    mavsdk_groundstation.unsubscribe_incoming_messages_json(json_handle);
}

TEST(SystemTest, InterceptJsonOutgoing)
{
    Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}};
    Mavsdk mavsdk_autopilot{Mavsdk::Configuration{ComponentType::Autopilot}};

    ASSERT_EQ(
        mavsdk_groundstation.add_any_connection("udpin://0.0.0.0:17000"),
        ConnectionResult::Success);
    ASSERT_EQ(
        mavsdk_autopilot.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);

    // Ground station discovers the autopilot system
    auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);
    ASSERT_TRUE(maybe_system);
    auto system = maybe_system.value();
    ASSERT_TRUE(system->has_autopilot());

    // Wait for autopilot instance to discover the connection to the ground station
    LogInfo() << "Waiting for autopilot system to connect...";
    while (mavsdk_autopilot.systems().size() == 0) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    // Create telemetry instances
    auto telemetry = Telemetry{system};
    auto telemetry_server = TelemetryServer{mavsdk_autopilot.server_component()};

    // Set up outgoing JSON message interception on autopilot
    auto json_prom = std::promise<Mavsdk::MavlinkMessage>();
    auto json_fut = json_prom.get_future();
    std::atomic<bool> intercept_called{false};

    auto json_handle = mavsdk_autopilot.subscribe_outgoing_messages_json(
        [&json_prom, &intercept_called](Mavsdk::MavlinkMessage json_message) {
            LogInfo() << "Intercepted outgoing JSON message: " << json_message.message_name
                      << " to system " << json_message.target_system_id
                      << " with fields: " << json_message.fields_json;

            if (json_message.message_name == "GPS_RAW_INT") {
                intercept_called = true;
                json_prom.set_value(json_message);
                return true; // Don't drop the message
            }
            return true; // Let other messages through
        });

    // Publish GPS data from autopilot
    LogInfo() << "Publishing GPS data from autopilot...";
    TelemetryServer::RawGps raw_gps{};
    raw_gps.timestamp_us = 1234567890123456789;
    raw_gps.latitude_deg = 47.3977421; // Zurich coordinates
    raw_gps.longitude_deg = 8.5455938;
    raw_gps.absolute_altitude_m = 488.0f;
    raw_gps.hdop = 1.0f;
    raw_gps.vdop = 1.5f;
    raw_gps.velocity_m_s = 15.0f;
    raw_gps.cog_deg = 90.0f;
    raw_gps.altitude_ellipsoid_m = 490.0f;
    raw_gps.horizontal_uncertainty_m = 2.0f;
    raw_gps.vertical_uncertainty_m = 3.0f;
    raw_gps.velocity_uncertainty_m_s = 0.5f;
    raw_gps.heading_uncertainty_deg = 5.0f;
    raw_gps.yaw_deg = 90.0f;

    TelemetryServer::GpsInfo gps_info{};
    gps_info.num_satellites = 12;
    gps_info.fix_type = TelemetryServer::FixType::Fix3D;

    ASSERT_EQ(
        telemetry_server.publish_raw_gps(raw_gps, gps_info), TelemetryServer::Result::Success);

    // Wait for JSON interception to occur
    ASSERT_EQ(json_fut.wait_for(std::chrono::seconds(1)), std::future_status::ready);

    // Verify intercept was called
    ASSERT_TRUE(intercept_called.load());

    auto intercepted_message = json_fut.get();

    // Verify the intercepted JSON message structure
    EXPECT_EQ(intercepted_message.message_name, "GPS_RAW_INT");
    EXPECT_GT(intercepted_message.fields_json.length(), 0u);

    // Parse JSON to verify field values
    Json::Value json;
    Json::Reader reader;
    ASSERT_TRUE(reader.parse(intercepted_message.fields_json, json));

    // Verify GPS fields from the intercepted message
    EXPECT_EQ(json["time_usec"].asUInt64(), raw_gps.timestamp_us);
    EXPECT_EQ(json["fix_type"].asUInt(), static_cast<uint8_t>(gps_info.fix_type));
    EXPECT_NEAR(json["lat"].asInt() / 1e7, raw_gps.latitude_deg, 1e-6);
    EXPECT_NEAR(json["lon"].asInt() / 1e7, raw_gps.longitude_deg, 1e-6);
    EXPECT_NEAR(json["alt"].asInt() / 1e3, raw_gps.absolute_altitude_m, 1.0);
    EXPECT_NEAR(json["eph"].asInt() / 1e2, raw_gps.hdop, 0.1);
    EXPECT_NEAR(json["epv"].asInt() / 1e2, raw_gps.vdop, 0.1);
    EXPECT_NEAR(json["vel"].asInt() / 1e2, raw_gps.velocity_m_s, 0.1);
    EXPECT_NEAR(json["cog"].asInt() / 1e2, raw_gps.cog_deg, 1.0);
    EXPECT_EQ(json["satellites_visible"].asUInt(), gps_info.num_satellites);

    LogInfo() << "Successfully tested outgoing JSON message interception";
    LogInfo() << "  - Message name: " << intercepted_message.message_name;
    LogInfo() << "  - Target system: " << intercepted_message.target_system_id;
    LogInfo() << "  - Fields JSON length: " << intercepted_message.fields_json.length();

    // Cleanup
    mavsdk_autopilot.unsubscribe_outgoing_messages_json(json_handle);
}

1	#include "log.h"
2	#include "mavsdk.h"
3	#include "plugins/telemetry/telemetry.h"
4	#include "plugins/telemetry_server/telemetry_server.h"
5	#include <chrono>
6	#include <thread>
7	#include <future>
8	#include <gtest/gtest.h>
9	#include <json/json.h>
10
11	using namespace mavsdk;
12
13	TEST(SystemTest, InterceptIncomingModifyLocal)	4✔
14	{
15	// Create 3 MAVSDK instances: Sender -> Interceptor/Forwarder -> Receiver
16	Mavsdk mavsdk_sender{Mavsdk::Configuration{ComponentType::Autopilot}};	1✔
17	Mavsdk mavsdk_interceptor{Mavsdk::Configuration{ComponentType::GroundStation}};	1✔
18	Mavsdk mavsdk_receiver{Mavsdk::Configuration{ComponentType::GroundStation}};	1✔
19
20	// Set up connections with forwarding enabled on interceptor
21	ASSERT_EQ(	1✔
22	mavsdk_sender.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);	1✔
23	ASSERT_EQ(	1✔
24	mavsdk_interceptor.add_any_connection(
25	"udpin://0.0.0.0:17000", ForwardingOption::ForwardingOn),
26	ConnectionResult::Success);	1✔
27	ASSERT_EQ(	1✔
28	mavsdk_interceptor.add_any_connection(
29	"udpout://127.0.0.1:17001", ForwardingOption::ForwardingOn),
30	ConnectionResult::Success);	1✔
31	ASSERT_EQ(	1✔
32	mavsdk_receiver.add_any_connection("udpin://0.0.0.0:17001"), ConnectionResult::Success);	1✔
33
34	// Wait for connections to establish
35	LogInfo() << "Waiting for connections to establish...";	1✔
36
37	// Interceptor discovers sender
38	auto sender_system_opt = mavsdk_interceptor.first_autopilot(10.0);	1✔
39	ASSERT_TRUE(sender_system_opt);	1✔
40	auto sender_system = sender_system_opt.value();	1✔
41	ASSERT_TRUE(sender_system->has_autopilot());	1✔
42
43	// Receiver discovers sender (via forwarding)
44	auto sender_system_receiver_opt = mavsdk_receiver.first_autopilot(10.0);	1✔
45	ASSERT_TRUE(sender_system_receiver_opt);	1✔
46	auto sender_system_receiver = sender_system_receiver_opt.value();	1✔
47	ASSERT_TRUE(sender_system_receiver->has_autopilot());	1✔
48
49	// Wait for sender to discover interceptor connection
50	LogInfo() << "Waiting for sender to connect to interceptor...";	1✔
51	while (mavsdk_sender.systems().size() == 0) {	2✔
52	std::this_thread::sleep_for(std::chrono::milliseconds(100));	1✔
53	}
54	auto interceptor_system = mavsdk_sender.systems().at(0);	1✔
55
56	// Create telemetry instances
57	auto telemetry_interceptor = Telemetry{sender_system};	1✔
58	auto telemetry_receiver = Telemetry{sender_system_receiver};	1✔
59	auto telemetry_server = TelemetryServer{mavsdk_sender.server_component()};	1✔
60
61	// Original position values
62	const double original_lat = 47.3977421; // Zurich coordinates	1✔
63	const double original_lon = 8.5455938;	1✔
64	const float original_alt = 488.0f;	1✔
65
66	// Modified position values (what intercept will change to)
67	const double modified_lat = 37.7749295; // San Francisco coordinates	1✔
68	const double modified_lon = -122.4194155;	1✔
69
70	// Set up raw mavlink intercept callback on interceptor to modify incoming GLOBAL_POSITION_INT
71	std::atomic<bool> intercept_called{false};	1✔
72
73	mavsdk_interceptor.intercept_incoming_messages_async([&](mavlink_message_t& message) -> bool {	7✔
74	if (message.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {	6✔
75	LogInfo() << "Intercepting GLOBAL_POSITION_INT message from system "	1✔
76	<< (int)message.sysid;	1✔
77
78	// Decode the message
79	mavlink_global_position_int_t pos;
80	mavlink_msg_global_position_int_decode(&message, &pos);	1✔
81
82	LogInfo() << "Original coordinates: lat=" << (pos.lat / 1e7)	1✔
83	<< ", lon=" << (pos.lon / 1e7);	1✔
84
85	// Modify the coordinates to San Francisco
86	pos.lat = static_cast<int32_t>(modified_lat * 1e7);	1✔
87	pos.lon = static_cast<int32_t>(modified_lon * 1e7);	1✔
88
89	// Re-encode the modified message using a dedicated channel to avoid
90	// racing with ServerComponentImpl::queue_message on channel 0.
91	mavlink_msg_global_position_int_encode_chan(	1✔
92	message.sysid, message.compid, MAVLINK_COMM_NUM_BUFFERS - 1, &message, &pos);	1✔
93
94	LogInfo() << "Modified coordinates to San Francisco: lat=" << (pos.lat / 1e7)	1✔
95	<< ", lon=" << (pos.lon / 1e7);	1✔
96
97	intercept_called = true;	1✔
98	}
99	return true; // Keep the message	6✔
100	});
101
102	// Promise/future for interceptor telemetry
103	auto interceptor_prom = std::promise<Telemetry::Position>();	1✔
104	auto interceptor_fut = interceptor_prom.get_future();	1✔
105
106	// Promise/future for receiver telemetry
107	auto receiver_prom = std::promise<Telemetry::Position>();	1✔
108	auto receiver_fut = receiver_prom.get_future();	1✔
109
110	// Subscribe to position updates on interceptor
111	auto interceptor_handle =
112	telemetry_interceptor.subscribe_position([&interceptor_prom](Telemetry::Position position) {	3✔
113	LogInfo() << "Interceptor received position: lat=" << position.latitude_deg	2✔
114	<< ", lon=" << position.longitude_deg;	1✔
115	interceptor_prom.set_value(position);	1✔
116	});	1✔
117
118	// Subscribe to position updates on receiver
119	auto receiver_handle =
120	telemetry_receiver.subscribe_position([&receiver_prom](Telemetry::Position position) {	3✔
121	LogInfo() << "Receiver received position: lat=" << position.latitude_deg	2✔
122	<< ", lon=" << position.longitude_deg;	1✔
123	receiver_prom.set_value(position);	1✔
124	});	1✔
125
126	// Send position from sender
127	LogInfo() << "Publishing original position from sender...";	1✔
128	TelemetryServer::Position position{};	1✔
129	position.latitude_deg = original_lat;	1✔
130	position.longitude_deg = original_lon;	1✔
131	position.absolute_altitude_m = original_alt;	1✔
132	position.relative_altitude_m = original_alt;	1✔
133
134	// Empty velocity and heading for this test
135	TelemetryServer::VelocityNed velocity{};	1✔
136	TelemetryServer::Heading heading{};	1✔
137
138	ASSERT_EQ(	1✔
139	telemetry_server.publish_position(position, velocity, heading),
140	TelemetryServer::Result::Success);	1✔
141
142	// Wait for both telemetry callbacks
143	ASSERT_EQ(interceptor_fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);	1✔
144	ASSERT_EQ(receiver_fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);	1✔
145
146	// Give intercept callback time to complete
147	std::this_thread::sleep_for(std::chrono::milliseconds(500));	1✔
148
149	// Verify intercept was called
150	ASSERT_TRUE(intercept_called.load());	1✔
151
152	// Get the results
153	auto interceptor_position = interceptor_fut.get();	1✔
154	auto receiver_position = receiver_fut.get();	1✔
155
156	// Verify interceptor sees modified coordinates (San Francisco)
157	EXPECT_NEAR(interceptor_position.latitude_deg, modified_lat, 1e-6);	1✔
158	EXPECT_NEAR(interceptor_position.longitude_deg, modified_lon, 1e-6);	1✔
159	EXPECT_NEAR(interceptor_position.absolute_altitude_m, original_alt, 1.0f);	1✔
160
161	// Receiver also gets modified coordinates because intercept happens BEFORE forwarding
162	EXPECT_NEAR(	1✔
163	receiver_position.latitude_deg,
164	modified_lat,
165	1e-6); // Should be modified San Francisco coordinates	1✔
166	EXPECT_NEAR(	1✔
167	receiver_position.longitude_deg,
168	modified_lon,
169	1e-6); // Should be modified San Francisco coordinates	1✔
170	EXPECT_NEAR(receiver_position.absolute_altitude_m, original_alt, 1.0f);	1✔
171
172	LogInfo() << "Test completed successfully:";	1✔
173	LogInfo() << " - Interceptor saw modified coordinates (San Francisco)";	1✔
174	LogInfo() << " - Receiver also saw modified coordinates (San Francisco)";	1✔
175	LogInfo() << " - This proves intercept affects BOTH local processing AND forwarding";	1✔
176
177	// Cleanup
178	telemetry_interceptor.unsubscribe_position(interceptor_handle);	1✔
179	telemetry_receiver.unsubscribe_position(receiver_handle);	1✔
180	mavsdk_interceptor.intercept_incoming_messages_async(nullptr);	1✔
181	}	1✔
182
183	TEST(SystemTest, InterceptJsonIncoming)	4✔
184	{
185	Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}};	1✔
186	Mavsdk mavsdk_autopilot{Mavsdk::Configuration{ComponentType::Autopilot}};	1✔
187
188	ASSERT_EQ(	1✔
189	mavsdk_groundstation.add_any_connection("udpin://0.0.0.0:17000"),
190	ConnectionResult::Success);	1✔
191	ASSERT_EQ(	1✔
192	mavsdk_autopilot.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);	1✔
193
194	// Ground station discovers the autopilot system
195	auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);	1✔
196	ASSERT_TRUE(maybe_system);	1✔
197	auto system = maybe_system.value();	1✔
198	ASSERT_TRUE(system->has_autopilot());	1✔
199
200	// Wait for autopilot instance to discover the connection to the ground station
201	LogInfo() << "Waiting for autopilot system to connect...";	1✔
202	while (mavsdk_autopilot.systems().size() == 0) {	2✔
203	std::this_thread::sleep_for(std::chrono::milliseconds(100));	1✔
204	}
205
206	// Create telemetry instances
207	auto telemetry = Telemetry{system};	1✔
208	auto telemetry_server = TelemetryServer{mavsdk_autopilot.server_component()};	1✔
209
210	// Set up incoming JSON message interception on ground station
211	auto json_prom = std::promise<Mavsdk::MavlinkMessage>();	1✔
212	auto json_fut = json_prom.get_future();	1✔
213	std::atomic<bool> intercept_called{false};	1✔
214
215	auto json_handle = mavsdk_groundstation.subscribe_incoming_messages_json(	1✔
216	[&json_prom, &intercept_called](Mavsdk::MavlinkMessage json_message) {	3✔
217	LogInfo() << "Intercepted incoming JSON message: " << json_message.message_name	2✔
218	<< " from system " << json_message.system_id	1✔
219	<< " with fields: " << json_message.fields_json;	1✔
220
221	if (json_message.message_name == "GLOBAL_POSITION_INT") {	1✔
222	intercept_called = true;	1✔
223	json_prom.set_value(json_message);	1✔
224	return true; // Don't drop the message	1✔
225	}
UNCOV 226	return true; // Let other messages through	×
227	});
228
229	// Publish position from autopilot
230	LogInfo() << "Publishing position from autopilot...";	1✔
231	TelemetryServer::Position position{};	1✔
232	position.latitude_deg = 47.3977421; // Zurich coordinates	1✔
233	position.longitude_deg = 8.5455938;	1✔
234	position.absolute_altitude_m = 488.0f;	1✔
235	position.relative_altitude_m = 488.0f;	1✔
236
237	TelemetryServer::VelocityNed velocity{};	1✔
238	velocity.north_m_s = 1.5f;	1✔
239	velocity.east_m_s = 2.0f;	1✔
240	velocity.down_m_s = -0.5f;	1✔
241
242	TelemetryServer::Heading heading{};	1✔
243	heading.heading_deg = 180.0f;	1✔
244
245	ASSERT_EQ(	1✔
246	telemetry_server.publish_position(position, velocity, heading),
247	TelemetryServer::Result::Success);	1✔
248
249	// Wait for JSON interception to occur
250	ASSERT_EQ(json_fut.wait_for(std::chrono::seconds(1)), std::future_status::ready);	1✔
251
252	// Verify intercept was called
253	ASSERT_TRUE(intercept_called.load());	1✔
254
255	auto intercepted_message = json_fut.get();	1✔
256
257	// Verify the intercepted JSON message structure
258	EXPECT_EQ(intercepted_message.message_name, "GLOBAL_POSITION_INT");	1✔
259	EXPECT_EQ(intercepted_message.system_id, 1u); // Autopilot system ID	1✔
260	EXPECT_GT(intercepted_message.fields_json.length(), 0u);	1✔
261
262	// Parse JSON to verify field values
263	Json::Value json;	1✔
264	Json::Reader reader;	1✔
265	ASSERT_TRUE(reader.parse(intercepted_message.fields_json, json));	1✔
266
267	// Verify position fields from the intercepted message
268	EXPECT_NEAR(json["lat"].asInt() / 1e7, position.latitude_deg, 1e-6);	1✔
269	EXPECT_NEAR(json["lon"].asInt() / 1e7, position.longitude_deg, 1e-6);	1✔
270	EXPECT_NEAR(json["alt"].asInt() / 1e3, position.absolute_altitude_m, 1.0);	1✔
271	EXPECT_NEAR(json["relative_alt"].asInt() / 1e3, position.relative_altitude_m, 1.0);	1✔
272
273	// Verify velocity fields
274	EXPECT_NEAR(json["vx"].asInt() / 1e2, velocity.north_m_s, 0.1);	1✔
275	EXPECT_NEAR(json["vy"].asInt() / 1e2, velocity.east_m_s, 0.1);	1✔
276	EXPECT_NEAR(json["vz"].asInt() / 1e2, velocity.down_m_s, 0.1);	1✔
277
278	// Verify heading field
279	EXPECT_NEAR(json["hdg"].asInt() / 1e2, heading.heading_deg, 1.0);	1✔
280
281	LogInfo() << "Successfully tested incoming JSON message interception";	1✔
282	LogInfo() << " - Message name: " << intercepted_message.message_name;	1✔
283	LogInfo() << " - System ID: " << intercepted_message.system_id;	1✔
284	LogInfo() << " - Fields JSON length: " << intercepted_message.fields_json.length();	1✔
285
286	// Cleanup
287	mavsdk_groundstation.unsubscribe_incoming_messages_json(json_handle);	1✔
288	}	1✔
289
290	TEST(SystemTest, InterceptJsonOutgoing)	4✔
291	{
292	Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}};	1✔
293	Mavsdk mavsdk_autopilot{Mavsdk::Configuration{ComponentType::Autopilot}};	1✔
294
295	ASSERT_EQ(	1✔
296	mavsdk_groundstation.add_any_connection("udpin://0.0.0.0:17000"),
297	ConnectionResult::Success);	1✔
298	ASSERT_EQ(	1✔
299	mavsdk_autopilot.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);	1✔
300
301	// Ground station discovers the autopilot system
302	auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);	1✔
303	ASSERT_TRUE(maybe_system);	1✔
304	auto system = maybe_system.value();	1✔
305	ASSERT_TRUE(system->has_autopilot());	1✔
306
307	// Wait for autopilot instance to discover the connection to the ground station
308	LogInfo() << "Waiting for autopilot system to connect...";	1✔
309	while (mavsdk_autopilot.systems().size() == 0) {	2✔
310	std::this_thread::sleep_for(std::chrono::milliseconds(100));	1✔
311	}
312
313	// Create telemetry instances
314	auto telemetry = Telemetry{system};	1✔
315	auto telemetry_server = TelemetryServer{mavsdk_autopilot.server_component()};	1✔
316
317	// Set up outgoing JSON message interception on autopilot
318	auto json_prom = std::promise<Mavsdk::MavlinkMessage>();	1✔
319	auto json_fut = json_prom.get_future();	1✔
320	std::atomic<bool> intercept_called{false};	1✔
321
322	auto json_handle = mavsdk_autopilot.subscribe_outgoing_messages_json(	1✔
323	[&json_prom, &intercept_called](Mavsdk::MavlinkMessage json_message) {	3✔
324	LogInfo() << "Intercepted outgoing JSON message: " << json_message.message_name	2✔
325	<< " to system " << json_message.target_system_id	1✔
326	<< " with fields: " << json_message.fields_json;	1✔
327
328	if (json_message.message_name == "GPS_RAW_INT") {	1✔
329	intercept_called = true;	1✔
330	json_prom.set_value(json_message);	1✔
331	return true; // Don't drop the message	1✔
332	}
333	return true; // Let other messages through	×
334	});
335
336	// Publish GPS data from autopilot
337	LogInfo() << "Publishing GPS data from autopilot...";	1✔
338	TelemetryServer::RawGps raw_gps{};	1✔
339	raw_gps.timestamp_us = 1234567890123456789;	1✔
340	raw_gps.latitude_deg = 47.3977421; // Zurich coordinates	1✔
341	raw_gps.longitude_deg = 8.5455938;	1✔
342	raw_gps.absolute_altitude_m = 488.0f;	1✔
343	raw_gps.hdop = 1.0f;	1✔
344	raw_gps.vdop = 1.5f;	1✔
345	raw_gps.velocity_m_s = 15.0f;	1✔
346	raw_gps.cog_deg = 90.0f;	1✔
347	raw_gps.altitude_ellipsoid_m = 490.0f;	1✔
348	raw_gps.horizontal_uncertainty_m = 2.0f;	1✔
349	raw_gps.vertical_uncertainty_m = 3.0f;	1✔
350	raw_gps.velocity_uncertainty_m_s = 0.5f;	1✔
351	raw_gps.heading_uncertainty_deg = 5.0f;	1✔
352	raw_gps.yaw_deg = 90.0f;	1✔
353
354	TelemetryServer::GpsInfo gps_info{};	1✔
355	gps_info.num_satellites = 12;	1✔
356	gps_info.fix_type = TelemetryServer::FixType::Fix3D;	1✔
357
358	ASSERT_EQ(	1✔
359	telemetry_server.publish_raw_gps(raw_gps, gps_info), TelemetryServer::Result::Success);	1✔
360
361	// Wait for JSON interception to occur
362	ASSERT_EQ(json_fut.wait_for(std::chrono::seconds(1)), std::future_status::ready);	1✔
363
364	// Verify intercept was called
365	ASSERT_TRUE(intercept_called.load());	1✔
366
367	auto intercepted_message = json_fut.get();	1✔
368
369	// Verify the intercepted JSON message structure
370	EXPECT_EQ(intercepted_message.message_name, "GPS_RAW_INT");	1✔
371	EXPECT_GT(intercepted_message.fields_json.length(), 0u);	1✔
372
373	// Parse JSON to verify field values
374	Json::Value json;	1✔
375	Json::Reader reader;	1✔
376	ASSERT_TRUE(reader.parse(intercepted_message.fields_json, json));	1✔
377
378	// Verify GPS fields from the intercepted message
379	EXPECT_EQ(json["time_usec"].asUInt64(), raw_gps.timestamp_us);	1✔
380	EXPECT_EQ(json["fix_type"].asUInt(), static_cast<uint8_t>(gps_info.fix_type));	1✔
381	EXPECT_NEAR(json["lat"].asInt() / 1e7, raw_gps.latitude_deg, 1e-6);	1✔
382	EXPECT_NEAR(json["lon"].asInt() / 1e7, raw_gps.longitude_deg, 1e-6);	1✔
383	EXPECT_NEAR(json["alt"].asInt() / 1e3, raw_gps.absolute_altitude_m, 1.0);	1✔
384	EXPECT_NEAR(json["eph"].asInt() / 1e2, raw_gps.hdop, 0.1);	1✔
385	EXPECT_NEAR(json["epv"].asInt() / 1e2, raw_gps.vdop, 0.1);	1✔
386	EXPECT_NEAR(json["vel"].asInt() / 1e2, raw_gps.velocity_m_s, 0.1);	1✔
387	EXPECT_NEAR(json["cog"].asInt() / 1e2, raw_gps.cog_deg, 1.0);	1✔
388	EXPECT_EQ(json["satellites_visible"].asUInt(), gps_info.num_satellites);	1✔
389
390	LogInfo() << "Successfully tested outgoing JSON message interception";	1✔
391	LogInfo() << " - Message name: " << intercepted_message.message_name;	1✔
392	LogInfo() << " - Target system: " << intercepted_message.target_system_id;	1✔
393	LogInfo() << " - Fields JSON length: " << intercepted_message.fields_json.length();	1✔
394
395	// Cleanup
396	mavsdk_autopilot.unsubscribe_outgoing_messages_json(json_handle);	1✔
397	}	1✔

mavlink / MAVSDK / 21724008972

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous