14784553540

Committed 01 May 2025 10:18PM UTC coverage: 44.214% (+0.003%) from 44.211%

Build # 14784553540

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2557 from mavlink/pr-udp-drop-remotes

core: Add timeout mechanism for inactive UDP connections

Run Details

11 of 13 new or added lines in 1 file covered. (84.62%)

5 existing lines in 1 file now uncovered.

14614 of 33053 relevant lines covered (44.21%)

281.87 hits per line

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

73.85

/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 <arpa/inet.h>
#include <errno.h>
#endif

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

#ifdef WINDOWS
#define GET_ERROR(_x) WSAGetLastError()
#else
#define GET_ERROR(_x) strerror(_x)
#endif

namespace mavsdk {

UdpConnection::UdpConnection(
    Connection::ReceiverCallback receiver_callback,
    std::string local_ip,
    int local_port_number,
    ForwardingOption forwarding_option) :
    Connection(std::move(receiver_callback), 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;
    }

    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: %d", WSAGetLastError();
        return ConnectionResult::SocketError;
    }
#endif

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

    if (_socket_fd.empty()) {
        LogErr() << "socket error" << GET_ERROR(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: " << GET_ERROR(errno);
        return ConnectionResult::BindError;
    }

    return ConnectionResult::Success;
}

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

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

    _socket_fd.close();

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

    // 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)
{
    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) {
                auto elapsed = now - remote.last_activity;
                bool inactive = elapsed > REMOTE_TIMEOUT;

                if (inactive) {
                    LogInfo() << "Removing inactive remote: " << remote.ip << ":"
                              << remote.port_number;
                }

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

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

    // Send the message 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);

        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
        uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);

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

        if (send_len != buffer_len) {
            std::stringstream ss;
            ss << "sendto failure: " << GET_ERROR(errno) << " for: " << remote.ip << ":"
               << remote.port_number;
            LogErr() << ss.str();
            result.first = false;
            if (!result.second.empty()) {
                result.second += ", ";
            }
            result.second += ss.str();
            continue;
        }
    }

    return result;
}

void UdpConnection::add_remote(const std::string& remote_ip, const int remote_port)
{
    add_remote_with_remote_sysid(remote_ip, remote_port, 0);
}

void UdpConnection::add_remote_with_remote_sysid(
    const std::string& remote_ip, const int remote_port, const uint8_t remote_sysid)
{
    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();

    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_with_remote_sysid(ip_str, ntohs(src_addr.sin_port), sysid);
                } else {
                    LogErr() << "inet_ntop failure for: " << strerror(errno);
                }
            }

            receive_message(_mavlink_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 <arpa/inet.h>
15	#include <errno.h>
16	#endif
17
18	#include <algorithm>
19	#include <utility>
20	#include <sstream>
21
22	#ifdef WINDOWS
23	#define GET_ERROR(_x) WSAGetLastError()
24	#else
25	#define GET_ERROR(_x) strerror(_x)
26	#endif
27
28	namespace mavsdk {
29
30	UdpConnection::UdpConnection(	84✔
31	Connection::ReceiverCallback receiver_callback,
32	std::string local_ip,
33	int local_port_number,
34	ForwardingOption forwarding_option) :	84✔
35	Connection(std::move(receiver_callback), forwarding_option),	84✔
36	_local_ip(std::move(local_ip)),	84✔
37	_local_port_number(local_port_number)	252✔
38	{}	84✔
39
40	UdpConnection::~UdpConnection()	84✔
41	{
42	// If no one explicitly called stop before, we should at least do it.
43	stop();	84✔
44	}	84✔
45
46	ConnectionResult UdpConnection::start()	84✔
47	{
48	if (!start_mavlink_receiver()) {	84✔
49	return ConnectionResult::ConnectionsExhausted;	×
50	}
51
52	ConnectionResult ret = setup_port();	84✔
53	if (ret != ConnectionResult::Success) {	84✔
54	return ret;	×
55	}
56
57	start_recv_thread();	84✔
58
59	return ConnectionResult::Success;	84✔
60	}
61
62	ConnectionResult UdpConnection::setup_port()	84✔
63	{
64	#ifdef WINDOWS
65	WSADATA wsa;
66	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
67	LogErr() << "Error: Winsock failed, error: %d", WSAGetLastError();
68	return ConnectionResult::SocketError;
69	}
70	#endif
71
72	_socket_fd.reset(socket(AF_INET, SOCK_DGRAM, 0));	84✔
73
74	if (_socket_fd.empty()) {	84✔
75	LogErr() << "socket error" << GET_ERROR(errno);	×
76	return ConnectionResult::SocketError;	×
77	}
78
79	struct sockaddr_in addr {};	84✔
80	addr.sin_family = AF_INET;	84✔
81	if (inet_pton(AF_INET, _local_ip.c_str(), &(addr.sin_addr)) != 1) {	84✔
82	LogErr() << "inet_pton failure for address: " << _local_ip;	×
83	return ConnectionResult::SocketError;	×
84	}
85	addr.sin_port = htons(_local_port_number);	84✔
86
87	if (bind(_socket_fd.get(), reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) != 0) {	84✔
88	LogErr() << "bind error: " << GET_ERROR(errno);	×
89	return ConnectionResult::BindError;	×
90	}
91
92	return ConnectionResult::Success;	84✔
93	}
94
95	void UdpConnection::start_recv_thread()	84✔
96	{
97	_recv_thread = std::make_unique<std::thread>(&UdpConnection::receive, this);	84✔
98	}	84✔
99
100	ConnectionResult UdpConnection::stop()	84✔
101	{
102	_should_exit = true;	84✔
103
104	_socket_fd.close();	84✔
105
106	if (_recv_thread) {	84✔
107	_recv_thread->join();	84✔
108	_recv_thread.reset();	84✔
109	}
110
111	// We need to stop this after stopping the receive thread, otherwise
112	// it can happen that we interfere with the parsing of a message.
113	stop_mavlink_receiver();	84✔
114
115	return ConnectionResult::Success;	84✔
116	}
117
118	std::pair<bool, std::string> UdpConnection::send_message(const mavlink_message_t& message)	1,917✔
119	{
120	std::pair<bool, std::string> result;	1,917✔
121
122	std::lock_guard<std::mutex> lock(_remote_mutex);	1,917✔
123
124	// Remove inactive remotes before sending messages
125	auto now = std::chrono::steady_clock::now();	1,916✔
126
127	_remotes.erase(	7,665✔
128	std::remove_if(	3,832✔
129	_remotes.begin(),
130	_remotes.end(),
131	[&now, this](const Remote& remote) {	3,832✔
132	auto elapsed = now - remote.last_activity;	1,916✔
133	bool inactive = elapsed > REMOTE_TIMEOUT;	1,917✔
134
135	if (inactive) {	1,916✔
NEW 136	LogInfo() << "Removing inactive remote: " << remote.ip << ":"	×
NEW 137	<< remote.port_number;	×
138	}
139
140	return inactive;	1,916✔
141	}),
142	_remotes.end());	3,833✔
143
144	if (_remotes.size() == 0) {	1,916✔
145	result.first = false;	×
146	result.second = "no remotes";	×
147	return result;	×
148	}
149
150	// Send the message to all the remotes. A remote is a UDP endpoint
151	// identified by its <ip, port>. This means that if we have two
152	// systems on two different endpoints, then messages directed towards
153	// only one system will be sent to both remotes. The systems are
154	// then expected to ignore messages that are not directed to them.
155
156	// For multiple remotes, we ignore errors, for just one, we bubble it up.
157	result.first = true;	1,916✔
158
159	for (auto& remote : _remotes) {	3,833✔
160	struct sockaddr_in dest_addr {};	1,917✔
161	dest_addr.sin_family = AF_INET;	1,917✔
162
163	if (inet_pton(AF_INET, remote.ip.c_str(), &dest_addr.sin_addr.s_addr) != 1) {	1,917✔
164	std::stringstream ss;	×
165	ss << "inet_pton failure for: " << remote.ip << ":" << remote.port_number;	×
166	LogErr() << ss.str();	×
167	result.first = false;	×
168	if (!result.second.empty()) {	×
169	result.second += ", ";	×
170	}
171	result.second += ss.str();	×
172	continue;	×
173	}	×
174	dest_addr.sin_port = htons(remote.port_number);	1,917✔
175
176	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];	1,917✔
177	uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);	1,917✔
178
179	const auto send_len = sendto(	1,917✔
180	_socket_fd.get(),
181	reinterpret_cast<char*>(buffer),
182	buffer_len,
183	0,
184	reinterpret_cast<const sockaddr*>(&dest_addr),
185	sizeof(dest_addr));
186
187	if (send_len != buffer_len) {	1,917✔
188	std::stringstream ss;	×
189	ss << "sendto failure: " << GET_ERROR(errno) << " for: " << remote.ip << ":"	×
190	<< remote.port_number;	×
191	LogErr() << ss.str();	×
192	result.first = false;	×
193	if (!result.second.empty()) {	×
194	result.second += ", ";	×
195	}
196	result.second += ss.str();	×
197	continue;	×
198	}	×
199	}
200
201	return result;
202	}	1,917✔
203
204	void UdpConnection::add_remote(const std::string& remote_ip, const int remote_port)	42✔
205	{
206	add_remote_with_remote_sysid(remote_ip, remote_port, 0);	42✔
207	}	42✔
208
209	void UdpConnection::add_remote_with_remote_sysid(	1,958✔
210	const std::string& remote_ip, const int remote_port, const uint8_t remote_sysid)
211	{
212	std::lock_guard<std::mutex> lock(_remote_mutex);	1,958✔
213	Remote new_remote;	1,957✔
214	new_remote.ip = remote_ip;	1,959✔
215	new_remote.port_number = remote_port;	1,959✔
216	new_remote.last_activity = std::chrono::steady_clock::now();	1,959✔
217
218	auto existing_remote =	1,959✔
219	std::find_if(_remotes.begin(), _remotes.end(), [&new_remote](Remote& remote) {	1,959✔
220	return remote == new_remote;	1,875✔
221	});
222
223	if (existing_remote == _remotes.end()) {	1,959✔
224	// System with sysid 0 is a bit special: it is a placeholder for a connection initiated
225	// by MAVSDK. As such, it should not be advertised as a newly discovered system.
226	if (static_cast<int>(remote_sysid) != 0) {	84✔
227	LogInfo() << "New system on: " << new_remote.ip << ":" << new_remote.port_number	84✔
228	<< " (with system ID: " << static_cast<int>(remote_sysid) << ")";	84✔
229	}
230	_remotes.push_back(new_remote);	84✔
231	} else {
232	// Update the timestamp for the existing remote
233	existing_remote->last_activity = std::chrono::steady_clock::now();	1,875✔
234	}
235	}	1,956✔
236
237	void UdpConnection::receive()	84✔
238	{
239	// Enough for MTU 1500 bytes.
240	char buffer[2048];	84✔
241
242	while (!_should_exit) {	2,085✔
243	struct sockaddr_in src_addr = {};	2,001✔
244	socklen_t src_addr_len = sizeof(src_addr);	2,001✔
245	const auto recv_len = recvfrom(	2,001✔
246	_socket_fd.get(),
247	buffer,
248	sizeof(buffer),
249	0,
250	reinterpret_cast<struct sockaddr*>(&src_addr),
251	&src_addr_len);
252
253	if (recv_len == 0) {	2,001✔
254	// This can happen when shutdown is called on the socket,
255	// therefore we check _should_exit again.
256	continue;	84✔
257	}
258
259	if (recv_len < 0) {	1,917✔
260	// This happens on destruction when _socket_fd.close() is called,
261	// therefore be quiet.
262	// LogErr() << "recvfrom error: " << GET_ERROR(errno);
263	continue;	×
264	}
265
266	_mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));	1,917✔
267
268	// Parse all mavlink messages in one datagram. Once exhausted, we'll exit while.
269	while (_mavlink_receiver->parse_message()) {	3,833✔
270	const uint8_t sysid = _mavlink_receiver->get_last_message().sysid;	1,917✔
271
272	if (sysid != 0) {	1,916✔
273	char ip_str[INET_ADDRSTRLEN];	1,916✔
274	if (inet_ntop(AF_INET, &src_addr.sin_addr, ip_str, INET_ADDRSTRLEN) != nullptr) {	1,916✔
275	add_remote_with_remote_sysid(ip_str, ntohs(src_addr.sin_port), sysid);	1,917✔
276	} else {
277	LogErr() << "inet_ntop failure for: " << strerror(errno);	×
278	}
279	}
280
281	receive_message(_mavlink_receiver->get_last_message(), this);	1,915✔
282	}
283	}
284	}	84✔
285
286	} // namespace mavsdk

mavlink / MAVSDK / 14784553540

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