21916664311

Committed 11 Feb 2026 05:56PM UTC coverage: 49.003% (-0.02%) from 49.027%

Build # 21916664311

Build Type

Pull #2765

github

Committed by

web-flow

Commit Message

Merge c7c594509 into ef29d2ec8

Pull Request Pull Request #2765: Docs: Update broken links and .gitignore for new docs location

Run Details

18352 of 37451 relevant lines covered (49.0%)

670.14 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) {	1✔
310	std::this_thread::sleep_for(std::chrono::milliseconds(100));	×
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) {	4✔
324	LogInfo() << "Intercepted outgoing JSON message: " << json_message.message_name	4✔
325	<< " to system " << json_message.target_system_id	2✔
326	<< " with fields: " << json_message.fields_json;	2✔
327
328	if (json_message.message_name == "GPS_RAW_INT") {	2✔
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	1✔
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 / 21916664311

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