22337580839

Committed 24 Feb 2026 05:11AM UTC coverage: 48.961% (-0.02%) from 48.982%

Build # 22337580839

Build Type

Pull #2775

github

Committed by

web-flow

Commit Message

Merge 2f0d03e12 into b46b0e529

Pull Request Pull Request #2775: Add devcontainer

Coverage Stats

18351 of 37481 relevant lines covered (48.96%)

668.89 hits per line

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

99.15

/cpp/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	}
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 / 22337580839

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