18329400312

Committed 07 Oct 2025 11:57PM UTC coverage: 47.68% (+0.07%) from 47.615%

Build # 18329400312

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2680 from mavlink/pr-int64-json

mavlink_direct: handle (u)int > 2^32

Run Details

62 of 67 new or added lines in 3 files covered. (92.54%)

27 existing lines in 10 files now uncovered.

17088 of 35839 relevant lines covered (47.68%)

446.34 hits per line

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

78.17

/src/mavsdk/core/udp_connection.cpp

#include "udp_connection.h"
#include "log.h"

#ifdef WINDOWS
#include <winsock2.h>
#include <Ws2tcpip.h> // For InetPton
#undef SOCKET_ERROR // conflicts with ConnectionResult::SocketError
#ifndef MINGW
#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
#endif
#else
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <arpa/inet.h>
#include <errno.h>
#endif

#include <algorithm>
#include <utility>
#include <sstream>

namespace mavsdk {

UdpConnection::UdpConnection(
    Connection::ReceiverCallback receiver_callback,
    Connection::LibmavReceiverCallback libmav_receiver_callback,
    MavsdkImpl& mavsdk_impl,
    std::string local_ip,
    int local_port_number,
    ForwardingOption forwarding_option) :
    Connection(
        std::move(receiver_callback),
        std::move(libmav_receiver_callback),
        mavsdk_impl,
        forwarding_option),
    _local_ip(std::move(local_ip)),
    _local_port_number(local_port_number)
{}

UdpConnection::~UdpConnection()
{
    // If no one explicitly called stop before, we should at least do it.
    stop();
}

ConnectionResult UdpConnection::start()
{
    if (!start_mavlink_receiver()) {
        return ConnectionResult::ConnectionsExhausted;
    }

    if (!start_libmav_receiver()) {
        return ConnectionResult::ConnectionsExhausted;
    }

    ConnectionResult ret = setup_port();
    if (ret != ConnectionResult::Success) {
        return ret;
    }

    start_recv_thread();

    return ConnectionResult::Success;
}

ConnectionResult UdpConnection::setup_port()
{
#ifdef WINDOWS
    WSADATA wsa;
    if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
        LogErr() << "Error: Winsock failed, error: " << get_socket_error_string(WSAGetLastError());
        return ConnectionResult::SocketError;
    }
#endif

    _socket_fd.reset(socket(AF_INET, SOCK_DGRAM, 0));

    if (_socket_fd.empty()) {
        LogErr() << "socket error" << strerror(errno);
        return ConnectionResult::SocketError;
    }

    struct sockaddr_in addr{};
    addr.sin_family = AF_INET;
    if (inet_pton(AF_INET, _local_ip.c_str(), &(addr.sin_addr)) != 1) {
        LogErr() << "inet_pton failure for address: " << _local_ip;
        return ConnectionResult::SocketError;
    }
    addr.sin_port = htons(_local_port_number);

    if (bind(_socket_fd.get(), reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) != 0) {
        LogErr() << "bind error: " << strerror(errno);
        return ConnectionResult::BindError;
    }

    // Set receive timeout cross-platform
    const unsigned timeout_ms = 500;

#if defined(WINDOWS)
    setsockopt(
        _socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_ms, sizeof(timeout_ms));
#else
    struct timeval tv;
    tv.tv_sec = 0;
    tv.tv_usec = timeout_ms * 1000;
    setsockopt(_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const void*)&tv, sizeof(tv));
#endif

    return ConnectionResult::Success;
}

void UdpConnection::start_recv_thread()
{
    _recv_thread = std::make_unique<std::thread>(&UdpConnection::receive, this);
}

ConnectionResult UdpConnection::stop()
{
    _should_exit = true;

    if (_recv_thread) {
        _recv_thread->join();
        _recv_thread.reset();
    }

    _socket_fd.close();

    // We need to stop this after stopping the receive thread, otherwise
    // it can happen that we interfere with the parsing of a message.
    stop_mavlink_receiver();

    return ConnectionResult::Success;
}

std::pair<bool, std::string> UdpConnection::send_message(const mavlink_message_t& message)
{
    // Convert message to raw bytes and use common send path
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);
    return send_raw_bytes(reinterpret_cast<const char*>(buffer), buffer_len);
}

std::pair<bool, std::string> UdpConnection::send_raw_bytes(const char* bytes, size_t length)
{
    std::pair<bool, std::string> result;

    std::lock_guard<std::mutex> lock(_remote_mutex);

    // Remove inactive remotes before sending messages
    auto now = std::chrono::steady_clock::now();

    _remotes.erase(
        std::remove_if(
            _remotes.begin(),
            _remotes.end(),
            [&now, this](const Remote& remote) {
                const auto elapsed = now - remote.last_activity;
                const bool inactive = elapsed > REMOTE_TIMEOUT;

                const bool should_remove = inactive && remote.remote_option == RemoteOption::Found;

                // We can cleanup old/previous remotes if we have
                if (should_remove) {
                    LogInfo() << "Removing inactive remote: " << remote.ip << ":"
                              << remote.port_number;
                }

                return should_remove;
            }),
        _remotes.end());

    if (_remotes.size() == 0) {
        result.first = false;
        result.second = "no remotes";
        return result;
    }

    // Send the raw bytes to all the remotes. A remote is a UDP endpoint
    // identified by its <ip, port>. This means that if we have two
    // systems on two different endpoints, then messages directed towards
    // only one system will be sent to both remotes. The systems are
    // then expected to ignore messages that are not directed to them.

    // For multiple remotes, we ignore errors, for just one, we bubble it up.
    result.first = true;

    for (auto& remote : _remotes) {
        struct sockaddr_in dest_addr{};
        dest_addr.sin_family = AF_INET;

        if (inet_pton(AF_INET, remote.ip.c_str(), &dest_addr.sin_addr.s_addr) != 1) {
            std::stringstream ss;
            ss << "inet_pton failure for: " << remote.ip << ":" << remote.port_number;
            LogErr() << ss.str();
            result.first = false;
            if (!result.second.empty()) {
                result.second += ", ";
            }
            result.second += ss.str();
            continue;
        }
        dest_addr.sin_port = htons(remote.port_number);

        const auto send_len = sendto(
            _socket_fd.get(),
            bytes,
            length,
            0,
            reinterpret_cast<const sockaddr*>(&dest_addr),
            sizeof(dest_addr));

        if (send_len != static_cast<std::remove_cv_t<decltype(send_len)>>(length)) {
            std::stringstream ss;
#ifdef WINDOWS
            int err = WSAGetLastError();
            ss << "sendto failure: " << get_socket_error_string(err) << " for: " << remote.ip << ":"
               << remote.port_number;
#else
            ss << "sendto failure: " << strerror(errno) << " for: " << remote.ip << ":"
               << remote.port_number;
#endif
            LogErr() << ss.str();
            result.first = false;
            if (!result.second.empty()) {
                result.second += ", ";
            }
            result.second += ss.str();
            continue;
        }
    }

    return result;
}

void UdpConnection::add_remote_to_keep(const std::string& remote_ip, const int remote_port)
{
    add_remote_impl(remote_ip, remote_port, 0, RemoteOption::Fixed);
}

void UdpConnection::add_remote_impl(
    const std::string& remote_ip,
    const int remote_port,
    const uint8_t remote_sysid,
    RemoteOption remote_option)
{
    std::lock_guard<std::mutex> lock(_remote_mutex);
    Remote new_remote;
    new_remote.ip = remote_ip;
    new_remote.port_number = remote_port;
    new_remote.last_activity = std::chrono::steady_clock::now();
    new_remote.remote_option = remote_option;

    auto existing_remote =
        std::find_if(_remotes.begin(), _remotes.end(), [&new_remote](Remote& remote) {
            return remote == new_remote;
        });

    if (existing_remote == _remotes.end()) {
        // System with sysid 0 is a bit special: it is a placeholder for a connection initiated
        // by MAVSDK. As such, it should not be advertised as a newly discovered system.
        if (static_cast<int>(remote_sysid) != 0) {
            LogInfo() << "New system on: " << new_remote.ip << ":" << new_remote.port_number
                      << " (with system ID: " << static_cast<int>(remote_sysid) << ")";
        }
        _remotes.push_back(new_remote);
    } else {
        // Update the timestamp for the existing remote
        existing_remote->last_activity = std::chrono::steady_clock::now();
    }
}

void UdpConnection::receive()
{
    // Enough for MTU 1500 bytes.
    char buffer[2048];

    while (!_should_exit) {
        struct sockaddr_in src_addr = {};
        socklen_t src_addr_len = sizeof(src_addr);
        const auto recv_len = recvfrom(
            _socket_fd.get(),
            buffer,
            sizeof(buffer),
            0,
            reinterpret_cast<struct sockaddr*>(&src_addr),
            &src_addr_len);

        if (recv_len == 0) {
            // This can happen when shutdown is called on the socket,
            // therefore we check _should_exit again.
            continue;
        }

        if (recv_len < 0) {
            // This happens on destruction when _socket_fd.close() is called,
            // therefore be quiet.
            // LogErr() << "recvfrom error: " << GET_ERROR(errno);
            continue;
        }

        _mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));

        // Parse all mavlink messages in one datagram. Once exhausted, we'll exit while.
        while (_mavlink_receiver->parse_message()) {
            const uint8_t sysid = _mavlink_receiver->get_last_message().sysid;

            if (sysid != 0) {
                char ip_str[INET_ADDRSTRLEN];
                if (inet_ntop(AF_INET, &src_addr.sin_addr, ip_str, INET_ADDRSTRLEN) != nullptr) {
                    add_remote_impl(ip_str, ntohs(src_addr.sin_port), sysid, RemoteOption::Found);
                } else {
                    LogErr() << "inet_ntop failure for: " << strerror(errno);
                }
            }

            // Handle parsed message
            receive_message(_mavlink_receiver->get_last_message(), this);
        }

        // Also parse with libmav if available
        if (_libmav_receiver) {
            _libmav_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));

            while (_libmav_receiver->parse_message()) {
                receive_libmav_message(_libmav_receiver->get_last_message(), this);
            }
        }
    }
}

} // namespace mavsdk

1	#include "udp_connection.h"
2	#include "log.h"
3
4	#ifdef WINDOWS
5	#include <winsock2.h>
6	#include <Ws2tcpip.h> // For InetPton
7	#undef SOCKET_ERROR // conflicts with ConnectionResult::SocketError
8	#ifndef MINGW
9	#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
10	#endif
11	#else
12	#include <netinet/in.h>
13	#include <sys/socket.h>
14	#include <sys/time.h>
15	#include <arpa/inet.h>
16	#include <errno.h>
17	#endif
18
19	#include <algorithm>
20	#include <utility>
21	#include <sstream>
22
23	namespace mavsdk {
24
25	UdpConnection::UdpConnection(	124✔
26	Connection::ReceiverCallback receiver_callback,
27	Connection::LibmavReceiverCallback libmav_receiver_callback,
28	MavsdkImpl& mavsdk_impl,
29	std::string local_ip,
30	int local_port_number,
31	ForwardingOption forwarding_option) :	124✔
32	Connection(
33	std::move(receiver_callback),	124✔
34	std::move(libmav_receiver_callback),	124✔
35	mavsdk_impl,
36	forwarding_option),
37	_local_ip(std::move(local_ip)),	124✔
38	_local_port_number(local_port_number)	372✔
39	{}	124✔
40
41	UdpConnection::~UdpConnection()	248✔
42	{
43	// If no one explicitly called stop before, we should at least do it.
44	stop();	124✔
45	}	248✔
46
47	ConnectionResult UdpConnection::start()	124✔
48	{
49	if (!start_mavlink_receiver()) {	124✔
50	return ConnectionResult::ConnectionsExhausted;	×
51	}
52
53	if (!start_libmav_receiver()) {	124✔
54	return ConnectionResult::ConnectionsExhausted;	×
55	}
56
57	ConnectionResult ret = setup_port();	124✔
58	if (ret != ConnectionResult::Success) {	124✔
59	return ret;	×
60	}
61
62	start_recv_thread();	124✔
63
64	return ConnectionResult::Success;	124✔
65	}
66
67	ConnectionResult UdpConnection::setup_port()	124✔
68	{
69	#ifdef WINDOWS
70	WSADATA wsa;
71	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
72	LogErr() << "Error: Winsock failed, error: " << get_socket_error_string(WSAGetLastError());
73	return ConnectionResult::SocketError;
74	}
75	#endif
76
77	_socket_fd.reset(socket(AF_INET, SOCK_DGRAM, 0));	124✔
78
79	if (_socket_fd.empty()) {	124✔
80	LogErr() << "socket error" << strerror(errno);	×
81	return ConnectionResult::SocketError;	×
82	}
83
84	struct sockaddr_in addr{};	124✔
85	addr.sin_family = AF_INET;	124✔
86	if (inet_pton(AF_INET, _local_ip.c_str(), &(addr.sin_addr)) != 1) {	124✔
87	LogErr() << "inet_pton failure for address: " << _local_ip;	×
88	return ConnectionResult::SocketError;	×
89	}
90	addr.sin_port = htons(_local_port_number);	124✔
91
92	if (bind(_socket_fd.get(), reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) != 0) {	124✔
93	LogErr() << "bind error: " << strerror(errno);	×
94	return ConnectionResult::BindError;	×
95	}
96
97	// Set receive timeout cross-platform
98	const unsigned timeout_ms = 500;	124✔
99
100	#if defined(WINDOWS)
101	setsockopt(
102	_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_ms, sizeof(timeout_ms));
103	#else
104	struct timeval tv;
105	tv.tv_sec = 0;	124✔
106	tv.tv_usec = timeout_ms * 1000;	124✔
107	setsockopt(_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const void*)&tv, sizeof(tv));	124✔
108	#endif
109
110	return ConnectionResult::Success;	124✔
111	}
112
113	void UdpConnection::start_recv_thread()	124✔
114	{
115	_recv_thread = std::make_unique<std::thread>(&UdpConnection::receive, this);	124✔
116	}	124✔
117
118	ConnectionResult UdpConnection::stop()	124✔
119	{
120	_should_exit = true;	124✔
121
122	if (_recv_thread) {	124✔
123	_recv_thread->join();	124✔
124	_recv_thread.reset();	124✔
125	}
126
127	_socket_fd.close();	124✔
128
129	// We need to stop this after stopping the receive thread, otherwise
130	// it can happen that we interfere with the parsing of a message.
131	stop_mavlink_receiver();	124✔
132
133	return ConnectionResult::Success;	124✔
134	}
135
136	std::pair<bool, std::string> UdpConnection::send_message(const mavlink_message_t& message)	2,350✔
137	{
138	// Convert message to raw bytes and use common send path
139	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
140	uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);	2,350✔
141	return send_raw_bytes(reinterpret_cast<const char*>(buffer), buffer_len);	2,346✔
142	}
143
144	std::pair<bool, std::string> UdpConnection::send_raw_bytes(const char* bytes, size_t length)	2,347✔
145	{
146	std::pair<bool, std::string> result;	2,347✔
147
148	std::lock_guard<std::mutex> lock(_remote_mutex);	2,347✔
149
150	// Remove inactive remotes before sending messages
151	auto now = std::chrono::steady_clock::now();	2,347✔
152
153	_remotes.erase(	4,697✔
154	std::remove_if(	2,342✔
155	_remotes.begin(),
156	_remotes.end(),
157	[&now, this](const Remote& remote) {	2,340✔
158	const auto elapsed = now - remote.last_activity;	2,340✔
159	const bool inactive = elapsed > REMOTE_TIMEOUT;	2,346✔
160
161	const bool should_remove = inactive && remote.remote_option == RemoteOption::Found;	2,343✔
162
163	// We can cleanup old/previous remotes if we have
164	if (should_remove) {	2,343✔
165	LogInfo() << "Removing inactive remote: " << remote.ip << ":"	×
166	<< remote.port_number;	×
167	}
168
169	return should_remove;	2,342✔
170	}),
171	_remotes.end());	2,346✔
172
173	if (_remotes.size() == 0) {	2,345✔
174	result.first = false;	3✔
175	result.second = "no remotes";	3✔
176	return result;	3✔
177	}
178
179	// Send the raw bytes to all the remotes. A remote is a UDP endpoint
180	// identified by its <ip, port>. This means that if we have two
181	// systems on two different endpoints, then messages directed towards
182	// only one system will be sent to both remotes. The systems are
183	// then expected to ignore messages that are not directed to them.
184
185	// For multiple remotes, we ignore errors, for just one, we bubble it up.
186	result.first = true;	2,341✔
187
188	for (auto& remote : _remotes) {	4,688✔
189	struct sockaddr_in dest_addr{};	2,343✔
190	dest_addr.sin_family = AF_INET;	2,343✔
191
192	if (inet_pton(AF_INET, remote.ip.c_str(), &dest_addr.sin_addr.s_addr) != 1) {	2,343✔
193	std::stringstream ss;	×
194	ss << "inet_pton failure for: " << remote.ip << ":" << remote.port_number;	×
195	LogErr() << ss.str();	×
196	result.first = false;	×
197	if (!result.second.empty()) {	×
198	result.second += ", ";	×
199	}
200	result.second += ss.str();	×
201	continue;	×
202	}	×
203	dest_addr.sin_port = htons(remote.port_number);	2,345✔
204
205	const auto send_len = sendto(	2,345✔
206	_socket_fd.get(),
207	bytes,
208	length,
209	0,
210	reinterpret_cast<const sockaddr*>(&dest_addr),
211	sizeof(dest_addr));
212
213	if (send_len != static_cast<std::remove_cv_t<decltype(send_len)>>(length)) {	2,347✔
214	std::stringstream ss;	×
215	#ifdef WINDOWS
216	int err = WSAGetLastError();
217	ss << "sendto failure: " << get_socket_error_string(err) << " for: " << remote.ip << ":"
218	<< remote.port_number;
219	#else
220	ss << "sendto failure: " << strerror(errno) << " for: " << remote.ip << ":"	×
221	<< remote.port_number;	×
222	#endif
223	LogErr() << ss.str();	×
224	result.first = false;	×
225	if (!result.second.empty()) {	×
226	result.second += ", ";	×
227	}
228	result.second += ss.str();	×
229	continue;	×
230	}	×
231	}
232
233	return result;	2,347✔
234	}	2,350✔
235
236	void UdpConnection::add_remote_to_keep(const std::string& remote_ip, const int remote_port)	62✔
237	{
238	add_remote_impl(remote_ip, remote_port, 0, RemoteOption::Fixed);	62✔
239	}	62✔
240
241	void UdpConnection::add_remote_impl(	2,408✔
242	const std::string& remote_ip,
243	const int remote_port,
244	const uint8_t remote_sysid,
245	RemoteOption remote_option)
246	{
247	std::lock_guard<std::mutex> lock(_remote_mutex);	2,408✔
248	Remote new_remote;	2,408✔
249	new_remote.ip = remote_ip;	2,408✔
250	new_remote.port_number = remote_port;	2,408✔
251	new_remote.last_activity = std::chrono::steady_clock::now();	2,408✔
252	new_remote.remote_option = remote_option;	2,407✔
253
254	auto existing_remote =
255	std::find_if(_remotes.begin(), _remotes.end(), [&new_remote](Remote& remote) {	2,407✔
256	return remote == new_remote;	2,282✔
257	});
258
259	if (existing_remote == _remotes.end()) {	2,407✔
260	// System with sysid 0 is a bit special: it is a placeholder for a connection initiated
261	// by MAVSDK. As such, it should not be advertised as a newly discovered system.
262	if (static_cast<int>(remote_sysid) != 0) {	124✔
263	LogInfo() << "New system on: " << new_remote.ip << ":" << new_remote.port_number	124✔
264	<< " (with system ID: " << static_cast<int>(remote_sysid) << ")";	62✔
265	}
266	_remotes.push_back(new_remote);	124✔
267	} else {
268	// Update the timestamp for the existing remote
269	existing_remote->last_activity = std::chrono::steady_clock::now();	2,282✔
270	}
271	}	2,404✔
272
273	void UdpConnection::receive()	124✔
274	{
275	// Enough for MTU 1500 bytes.
276	char buffer[2048];
277
278	while (!_should_exit) {	2,798✔
279	struct sockaddr_in src_addr = {};	2,674✔
280	socklen_t src_addr_len = sizeof(src_addr);	2,674✔
281	const auto recv_len = recvfrom(	2,674✔
282	_socket_fd.get(),
283	buffer,
284	sizeof(buffer),
285	0,
286	reinterpret_cast<struct sockaddr*>(&src_addr),
287	&src_addr_len);
288
289	if (recv_len == 0) {	2,675✔
290	// This can happen when shutdown is called on the socket,
291	// therefore we check _should_exit again.
292	continue;	329✔
293	}
294
295	if (recv_len < 0) {	2,675✔
296	// This happens on destruction when _socket_fd.close() is called,
297	// therefore be quiet.
298	// LogErr() << "recvfrom error: " << GET_ERROR(errno);
299	continue;	329✔
300	}
301
302	_mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));	2,346✔
303
304	// Parse all mavlink messages in one datagram. Once exhausted, we'll exit while.
305	while (_mavlink_receiver->parse_message()) {	4,690✔
306	const uint8_t sysid = _mavlink_receiver->get_last_message().sysid;	2,346✔
307
308	if (sysid != 0) {	2,346✔
309	char ip_str[INET_ADDRSTRLEN];
310	if (inet_ntop(AF_INET, &src_addr.sin_addr, ip_str, INET_ADDRSTRLEN) != nullptr) {	2,346✔
311	add_remote_impl(ip_str, ntohs(src_addr.sin_port), sysid, RemoteOption::Found);	2,346✔
312	} else {
UNCOV 313	LogErr() << "inet_ntop failure for: " << strerror(errno);	×
314	}
315	}
316
317	// Handle parsed message
318	receive_message(_mavlink_receiver->get_last_message(), this);	2,344✔
319	}
320
321	// Also parse with libmav if available
322	if (_libmav_receiver) {	2,346✔
323	_libmav_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));	2,346✔
324
325	while (_libmav_receiver->parse_message()) {	4,687✔
326	receive_libmav_message(_libmav_receiver->get_last_message(), this);	2,345✔
327	}
328	}
329	}
330	}	115✔
331
332	} // namespace mavsdk

mavlink / MAVSDK / 18329400312

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