17083234422

Committed 19 Aug 2025 10:23PM UTC coverage: 47.262% (+0.004%) from 47.258%

Build # 17083234422

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2645 from mavlink/pr-proto-comment

proto: update to get comment regardig GPS fix

Run Details

16702 of 35339 relevant lines covered (47.26%)

443.76 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
            mavlink_msg_global_position_int_encode(message.sysid, message.compid, &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);

    // FIXED BEHAVIOR: Receiver gets original coordinates (forwarded message not affected by
    // intercept)
    EXPECT_NEAR(
        receiver_position.latitude_deg,
        original_lat,
        1e-6); // Should be original Zurich coordinates
    EXPECT_NEAR(
        receiver_position.longitude_deg,
        original_lon,
        1e-6); // Should be original Zurich coordinates
    EXPECT_NEAR(receiver_position.absolute_altitude_m, original_alt, 1.0f);

    LogInfo() << "Test completed successfully - intercept bug FIXED:";
    LogInfo() << "  - Interceptor saw modified coordinates (San Francisco)";
    LogInfo() << "  - Receiver saw original coordinates (Zurich)";
    LogInfo() << "  - This proves intercept affects local processing but NOT 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 = 1234567890;
    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) {	1✔
52	std::this_thread::sleep_for(std::chrono::milliseconds(100));	×
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
90	mavlink_msg_global_position_int_encode(message.sysid, message.compid, &message, &pos);	1✔
91
92	LogInfo() << "Modified coordinates to San Francisco: lat=" << (pos.lat / 1e7)	1✔
93	<< ", lon=" << (pos.lon / 1e7);	1✔
94
95	intercept_called = true;	1✔
96	}
97	return true; // Keep the message	6✔
98	});
99
100	// Promise/future for interceptor telemetry
101	auto interceptor_prom = std::promise<Telemetry::Position>();	1✔
102	auto interceptor_fut = interceptor_prom.get_future();	1✔
103
104	// Promise/future for receiver telemetry
105	auto receiver_prom = std::promise<Telemetry::Position>();	1✔
106	auto receiver_fut = receiver_prom.get_future();	1✔
107
108	// Subscribe to position updates on interceptor
109	auto interceptor_handle =
110	telemetry_interceptor.subscribe_position([&interceptor_prom](Telemetry::Position position) {	3✔
111	LogInfo() << "Interceptor received position: lat=" << position.latitude_deg	2✔
112	<< ", lon=" << position.longitude_deg;	1✔
113	interceptor_prom.set_value(position);	1✔
114	});	1✔
115
116	// Subscribe to position updates on receiver
117	auto receiver_handle =
118	telemetry_receiver.subscribe_position([&receiver_prom](Telemetry::Position position) {	3✔
119	LogInfo() << "Receiver received position: lat=" << position.latitude_deg	2✔
120	<< ", lon=" << position.longitude_deg;	1✔
121	receiver_prom.set_value(position);	1✔
122	});	1✔
123
124	// Send position from sender
125	LogInfo() << "Publishing original position from sender...";	1✔
126	TelemetryServer::Position position{};	1✔
127	position.latitude_deg = original_lat;	1✔
128	position.longitude_deg = original_lon;	1✔
129	position.absolute_altitude_m = original_alt;	1✔
130	position.relative_altitude_m = original_alt;	1✔
131
132	// Empty velocity and heading for this test
133	TelemetryServer::VelocityNed velocity{};	1✔
134	TelemetryServer::Heading heading{};	1✔
135
136	ASSERT_EQ(	1✔
137	telemetry_server.publish_position(position, velocity, heading),
138	TelemetryServer::Result::Success);	1✔
139
140	// Wait for both telemetry callbacks
141	ASSERT_EQ(interceptor_fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);	1✔
142	ASSERT_EQ(receiver_fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);	1✔
143
144	// Give intercept callback time to complete
145	std::this_thread::sleep_for(std::chrono::milliseconds(500));	1✔
146
147	// Verify intercept was called
148	ASSERT_TRUE(intercept_called.load());	1✔
149
150	// Get the results
151	auto interceptor_position = interceptor_fut.get();	1✔
152	auto receiver_position = receiver_fut.get();	1✔
153
154	// Verify interceptor sees modified coordinates (San Francisco)
155	EXPECT_NEAR(interceptor_position.latitude_deg, modified_lat, 1e-6);	1✔
156	EXPECT_NEAR(interceptor_position.longitude_deg, modified_lon, 1e-6);	1✔
157	EXPECT_NEAR(interceptor_position.absolute_altitude_m, original_alt, 1.0f);	1✔
158
159	// FIXED BEHAVIOR: Receiver gets original coordinates (forwarded message not affected by
160	// intercept)
161	EXPECT_NEAR(	1✔
162	receiver_position.latitude_deg,
163	original_lat,
164	1e-6); // Should be original Zurich coordinates	1✔
165	EXPECT_NEAR(	1✔
166	receiver_position.longitude_deg,
167	original_lon,
168	1e-6); // Should be original Zurich coordinates	1✔
169	EXPECT_NEAR(receiver_position.absolute_altitude_m, original_alt, 1.0f);	1✔
170
171	LogInfo() << "Test completed successfully - intercept bug FIXED:";	1✔
172	LogInfo() << " - Interceptor saw modified coordinates (San Francisco)";	1✔
173	LogInfo() << " - Receiver saw original coordinates (Zurich)";	1✔
174	LogInfo() << " - This proves intercept affects local processing but NOT forwarding";	1✔
175
176	// Cleanup
177	telemetry_interceptor.unsubscribe_position(interceptor_handle);	1✔
178	telemetry_receiver.unsubscribe_position(receiver_handle);	1✔
179	mavsdk_interceptor.intercept_incoming_messages_async(nullptr);	1✔
180	}	1✔
181
182	TEST(SystemTest, InterceptJsonIncoming)	4✔
183	{
184	Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}};	1✔
185	Mavsdk mavsdk_autopilot{Mavsdk::Configuration{ComponentType::Autopilot}};	1✔
186
187	ASSERT_EQ(	1✔
188	mavsdk_groundstation.add_any_connection("udpin://0.0.0.0:17000"),
189	ConnectionResult::Success);	1✔
190	ASSERT_EQ(	1✔
191	mavsdk_autopilot.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);	1✔
192
193	// Ground station discovers the autopilot system
194	auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);	1✔
195	ASSERT_TRUE(maybe_system);	1✔
196	auto system = maybe_system.value();	1✔
197	ASSERT_TRUE(system->has_autopilot());	1✔
198
199	// Wait for autopilot instance to discover the connection to the ground station
200	LogInfo() << "Waiting for autopilot system to connect...";	1✔
201	while (mavsdk_autopilot.systems().size() == 0) {	2✔
202	std::this_thread::sleep_for(std::chrono::milliseconds(100));	1✔
203	}
204
205	// Create telemetry instances
206	auto telemetry = Telemetry{system};	1✔
207	auto telemetry_server = TelemetryServer{mavsdk_autopilot.server_component()};	1✔
208
209	// Set up incoming JSON message interception on ground station
210	auto json_prom = std::promise<Mavsdk::MavlinkMessage>();	1✔
211	auto json_fut = json_prom.get_future();	1✔
212	std::atomic<bool> intercept_called{false};	1✔
213
214	auto json_handle = mavsdk_groundstation.subscribe_incoming_messages_json(	1✔
215	[&json_prom, &intercept_called](Mavsdk::MavlinkMessage json_message) {	4✔
216	LogInfo() << "Intercepted incoming JSON message: " << json_message.message_name	4✔
217	<< " from system " << json_message.system_id	2✔
218	<< " with fields: " << json_message.fields_json;	2✔
219
220	if (json_message.message_name == "GLOBAL_POSITION_INT") {	2✔
221	intercept_called = true;	1✔
222	json_prom.set_value(json_message);	1✔
223	return true; // Don't drop the message	1✔
224	}
225	return true; // Let other messages through	1✔
226	});
227
228	// Publish position from autopilot
229	LogInfo() << "Publishing position from autopilot...";	1✔
230	TelemetryServer::Position position{};	1✔
231	position.latitude_deg = 47.3977421; // Zurich coordinates	1✔
232	position.longitude_deg = 8.5455938;	1✔
233	position.absolute_altitude_m = 488.0f;	1✔
234	position.relative_altitude_m = 488.0f;	1✔
235
236	TelemetryServer::VelocityNed velocity{};	1✔
237	velocity.north_m_s = 1.5f;	1✔
238	velocity.east_m_s = 2.0f;	1✔
239	velocity.down_m_s = -0.5f;	1✔
240
241	TelemetryServer::Heading heading{};	1✔
242	heading.heading_deg = 180.0f;	1✔
243
244	ASSERT_EQ(	1✔
245	telemetry_server.publish_position(position, velocity, heading),
246	TelemetryServer::Result::Success);	1✔
247
248	// Wait for JSON interception to occur
249	ASSERT_EQ(json_fut.wait_for(std::chrono::seconds(1)), std::future_status::ready);	1✔
250
251	// Verify intercept was called
252	ASSERT_TRUE(intercept_called.load());	1✔
253
254	auto intercepted_message = json_fut.get();	1✔
255
256	// Verify the intercepted JSON message structure
257	EXPECT_EQ(intercepted_message.message_name, "GLOBAL_POSITION_INT");	1✔
258	EXPECT_EQ(intercepted_message.system_id, 1u); // Autopilot system ID	1✔
259	EXPECT_GT(intercepted_message.fields_json.length(), 0u);	1✔
260
261	// Parse JSON to verify field values
262	Json::Value json;	1✔
263	Json::Reader reader;	1✔
264	ASSERT_TRUE(reader.parse(intercepted_message.fields_json, json));	1✔
265
266	// Verify position fields from the intercepted message
267	EXPECT_NEAR(json["lat"].asInt() / 1e7, position.latitude_deg, 1e-6);	1✔
268	EXPECT_NEAR(json["lon"].asInt() / 1e7, position.longitude_deg, 1e-6);	1✔
269	EXPECT_NEAR(json["alt"].asInt() / 1e3, position.absolute_altitude_m, 1.0);	1✔
270	EXPECT_NEAR(json["relative_alt"].asInt() / 1e3, position.relative_altitude_m, 1.0);	1✔
271
272	// Verify velocity fields
273	EXPECT_NEAR(json["vx"].asInt() / 1e2, velocity.north_m_s, 0.1);	1✔
274	EXPECT_NEAR(json["vy"].asInt() / 1e2, velocity.east_m_s, 0.1);	1✔
275	EXPECT_NEAR(json["vz"].asInt() / 1e2, velocity.down_m_s, 0.1);	1✔
276
277	// Verify heading field
278	EXPECT_NEAR(json["hdg"].asInt() / 1e2, heading.heading_deg, 1.0);	1✔
279
280	LogInfo() << "Successfully tested incoming JSON message interception";	1✔
281	LogInfo() << " - Message name: " << intercepted_message.message_name;	1✔
282	LogInfo() << " - System ID: " << intercepted_message.system_id;	1✔
283	LogInfo() << " - Fields JSON length: " << intercepted_message.fields_json.length();	1✔
284
285	// Cleanup
286	mavsdk_groundstation.unsubscribe_incoming_messages_json(json_handle);	1✔
287	}	1✔
288
289	TEST(SystemTest, InterceptJsonOutgoing)	4✔
290	{
291	Mavsdk mavsdk_groundstation{Mavsdk::Configuration{ComponentType::GroundStation}};	1✔
292	Mavsdk mavsdk_autopilot{Mavsdk::Configuration{ComponentType::Autopilot}};	1✔
293
294	ASSERT_EQ(	1✔
295	mavsdk_groundstation.add_any_connection("udpin://0.0.0.0:17000"),
296	ConnectionResult::Success);	1✔
297	ASSERT_EQ(	1✔
298	mavsdk_autopilot.add_any_connection("udpout://127.0.0.1:17000"), ConnectionResult::Success);	1✔
299
300	// Ground station discovers the autopilot system
301	auto maybe_system = mavsdk_groundstation.first_autopilot(10.0);	1✔
302	ASSERT_TRUE(maybe_system);	1✔
303	auto system = maybe_system.value();	1✔
304	ASSERT_TRUE(system->has_autopilot());	1✔
305
306	// Wait for autopilot instance to discover the connection to the ground station
307	LogInfo() << "Waiting for autopilot system to connect...";	1✔
308	while (mavsdk_autopilot.systems().size() == 0) {	2✔
309	std::this_thread::sleep_for(std::chrono::milliseconds(100));	1✔
310	}
311
312	// Create telemetry instances
313	auto telemetry = Telemetry{system};	1✔
314	auto telemetry_server = TelemetryServer{mavsdk_autopilot.server_component()};	1✔
315
316	// Set up outgoing JSON message interception on autopilot
317	auto json_prom = std::promise<Mavsdk::MavlinkMessage>();	1✔
318	auto json_fut = json_prom.get_future();	1✔
319	std::atomic<bool> intercept_called{false};	1✔
320
321	auto json_handle = mavsdk_autopilot.subscribe_outgoing_messages_json(	1✔
322	[&json_prom, &intercept_called](Mavsdk::MavlinkMessage json_message) {	3✔
323	LogInfo() << "Intercepted outgoing JSON message: " << json_message.message_name	2✔
324	<< " to system " << json_message.target_system_id	1✔
325	<< " with fields: " << json_message.fields_json;	1✔
326
327	if (json_message.message_name == "GPS_RAW_INT") {	1✔
328	intercept_called = true;	1✔
329	json_prom.set_value(json_message);	1✔
330	return true; // Don't drop the message	1✔
331	}
332	return true; // Let other messages through	×
333	});
334
335	// Publish GPS data from autopilot
336	LogInfo() << "Publishing GPS data from autopilot...";	1✔
337	TelemetryServer::RawGps raw_gps{};	1✔
338	raw_gps.timestamp_us = 1234567890;	1✔
339	raw_gps.latitude_deg = 47.3977421; // Zurich coordinates	1✔
340	raw_gps.longitude_deg = 8.5455938;	1✔
341	raw_gps.absolute_altitude_m = 488.0f;	1✔
342	raw_gps.hdop = 1.0f;	1✔
343	raw_gps.vdop = 1.5f;	1✔
344	raw_gps.velocity_m_s = 15.0f;	1✔
345	raw_gps.cog_deg = 90.0f;	1✔
346	raw_gps.altitude_ellipsoid_m = 490.0f;	1✔
347	raw_gps.horizontal_uncertainty_m = 2.0f;	1✔
348	raw_gps.vertical_uncertainty_m = 3.0f;	1✔
349	raw_gps.velocity_uncertainty_m_s = 0.5f;	1✔
350	raw_gps.heading_uncertainty_deg = 5.0f;	1✔
351	raw_gps.yaw_deg = 90.0f;	1✔
352
353	TelemetryServer::GpsInfo gps_info{};	1✔
354	gps_info.num_satellites = 12;	1✔
355	gps_info.fix_type = TelemetryServer::FixType::Fix3D;	1✔
356
357	ASSERT_EQ(	1✔
358	telemetry_server.publish_raw_gps(raw_gps, gps_info), TelemetryServer::Result::Success);	1✔
359
360	// Wait for JSON interception to occur
361	ASSERT_EQ(json_fut.wait_for(std::chrono::seconds(1)), std::future_status::ready);	1✔
362
363	// Verify intercept was called
364	ASSERT_TRUE(intercept_called.load());	1✔
365
366	auto intercepted_message = json_fut.get();	1✔
367
368	// Verify the intercepted JSON message structure
369	EXPECT_EQ(intercepted_message.message_name, "GPS_RAW_INT");	1✔
370	EXPECT_GT(intercepted_message.fields_json.length(), 0u);	1✔
371
372	// Parse JSON to verify field values
373	Json::Value json;	1✔
374	Json::Reader reader;	1✔
375	ASSERT_TRUE(reader.parse(intercepted_message.fields_json, json));	1✔
376
377	// Verify GPS fields from the intercepted message
378	EXPECT_EQ(json["time_usec"].asUInt64(), raw_gps.timestamp_us);	1✔
379	EXPECT_EQ(json["fix_type"].asUInt(), static_cast<uint8_t>(gps_info.fix_type));	1✔
380	EXPECT_NEAR(json["lat"].asInt() / 1e7, raw_gps.latitude_deg, 1e-6);	1✔
381	EXPECT_NEAR(json["lon"].asInt() / 1e7, raw_gps.longitude_deg, 1e-6);	1✔
382	EXPECT_NEAR(json["alt"].asInt() / 1e3, raw_gps.absolute_altitude_m, 1.0);	1✔
383	EXPECT_NEAR(json["eph"].asInt() / 1e2, raw_gps.hdop, 0.1);	1✔
384	EXPECT_NEAR(json["epv"].asInt() / 1e2, raw_gps.vdop, 0.1);	1✔
385	EXPECT_NEAR(json["vel"].asInt() / 1e2, raw_gps.velocity_m_s, 0.1);	1✔
386	EXPECT_NEAR(json["cog"].asInt() / 1e2, raw_gps.cog_deg, 1.0);	1✔
387	EXPECT_EQ(json["satellites_visible"].asUInt(), gps_info.num_satellites);	1✔
388
389	LogInfo() << "Successfully tested outgoing JSON message interception";	1✔
390	LogInfo() << " - Message name: " << intercepted_message.message_name;	1✔
391	LogInfo() << " - Target system: " << intercepted_message.target_system_id;	1✔
392	LogInfo() << " - Fields JSON length: " << intercepted_message.fields_json.length();	1✔
393
394	// Cleanup
395	mavsdk_autopilot.unsubscribe_outgoing_messages_json(json_handle);	1✔
396	}	1✔

mavlink / MAVSDK / 17083234422

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