17565334070

Committed 08 Sep 2025 10:02PM UTC coverage: 47.262% (+0.02%) from 47.241%

Build # 17565334070

Build Type

Pull #2649

github

Committed by

web-flow

Commit Message

Merge 31bace7d2 into 57e1189fd

Pull Request Pull Request #2649: CI: Tell sonar we use C++17

Run Details

16788 of 35521 relevant lines covered (47.26%)

444.32 hits per line

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

76.85

/src/mavsdk/core/tcp_server_connection.cpp

#include "tcp_server_connection.h"
#include "log.h"

#include <cassert>
#include <fcntl.h>
#include <sstream>

#ifdef WINDOWS
#ifndef MINGW
#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
#endif
#include <Windows.h>
#include <Winsock2.h>
#include <ws2tcpip.h>
#else
#include <netinet/in.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <errno.h>
#include <netdb.h>
#endif

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

namespace mavsdk {
TcpServerConnection::TcpServerConnection(
    Connection::ReceiverCallback receiver_callback,
    Connection::LibmavReceiverCallback libmav_receiver_callback,
    MavsdkImpl& mavsdk_impl,
    std::string local_ip,
    int local_port,
    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(local_port)
{}

TcpServerConnection::~TcpServerConnection()
{
    stop();
}

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

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

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

    _server_socket_fd.reset(socket(AF_INET, SOCK_STREAM, 0));
    if (_server_socket_fd.empty()) {
        LogErr() << "socket error: " << GET_ERROR(errno);
        return ConnectionResult::SocketError;
    }

    sockaddr_in server_addr{};
    server_addr.sin_family = AF_INET;
    server_addr.sin_addr.s_addr = INADDR_ANY;
    server_addr.sin_port = htons(_local_port);

    if (bind(
            _server_socket_fd.get(),
            reinterpret_cast<sockaddr*>(&server_addr),
            sizeof(server_addr)) < 0) {
        LogErr() << "bind error: " << GET_ERROR(errno);
        return ConnectionResult::SocketError;
    }

    if (listen(_server_socket_fd.get(), 3) < 0) {
        LogErr() << "listen error: " << GET_ERROR(errno);
        return ConnectionResult::SocketError;
    }

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

#if defined(WINDOWS)
    setsockopt(
        _server_socket_fd.get(),
        SOL_SOCKET,
        SO_RCVTIMEO,
        (const char*)&timeout_ms,
        sizeof(timeout_ms));
    setsockopt(
        _client_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(_server_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const void*)&tv, sizeof(tv));
    setsockopt(_client_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const void*)&tv, sizeof(tv));
#endif

    _accept_receive_thread =
        std::make_unique<std::thread>(&TcpServerConnection::accept_client, this);

    return ConnectionResult::Success;
}

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

    if (_accept_receive_thread && _accept_receive_thread->joinable()) {
        _accept_receive_thread->join();
        _accept_receive_thread.reset();
    }

    _client_socket_fd.close();
    _server_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> TcpServerConnection::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);

    assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);

    return send_raw_bytes(reinterpret_cast<const char*>(buffer), buffer_len);
}

void TcpServerConnection::accept_client()
{
#ifdef WINDOWS
    // Set server socket to non-blocking
    u_long iMode = 1;
    int iResult = ioctlsocket(_server_socket_fd.get(), FIONBIO, &iMode);
    if (iResult != 0) {
        LogErr() << "ioctlsocket failed with error: " << WSAGetLastError();
    }
#else
    // Set server socket to non-blocking
    int flags = fcntl(_server_socket_fd.get(), F_GETFL, 0);
    fcntl(_server_socket_fd.get(), F_SETFL, flags | O_NONBLOCK);
#endif

    while (!_should_exit) {
        fd_set readfds;
        FD_ZERO(&readfds);
        FD_SET(_server_socket_fd.get(), &readfds);

        // Set timeout to 1 second
        timeval timeout;
        timeout.tv_sec = 1;
        timeout.tv_usec = 0;

        const int activity =
            select(_server_socket_fd.get() + 1, &readfds, nullptr, nullptr, &timeout);

        if (activity < 0 && errno != EINTR) {
            LogErr() << "select error: " << GET_ERROR(errno);
            continue;
        }

        if (activity == 0) {
            // Timeout, no incoming connection
            continue;
        }

        if (FD_ISSET(_server_socket_fd.get(), &readfds)) {
            sockaddr_in client_addr{};
            socklen_t client_addr_len = sizeof(client_addr);

            _client_socket_fd.reset(accept(
                _server_socket_fd.get(),
                reinterpret_cast<sockaddr*>(&client_addr),
                &client_addr_len));
            if (_client_socket_fd.empty()) {
                if (_should_exit) {
                    return;
                }
                LogErr() << "accept error: " << GET_ERROR(errno);
                continue;
            }

            receive();
        }
    }
}

void TcpServerConnection::receive()
{
    std::array<char, 2048> buffer{};

    bool dataReceived = false;
    while (!dataReceived && !_should_exit) {
        const auto recv_len = recv(_client_socket_fd.get(), buffer.data(), buffer.size(), 0);

#ifdef WINDOWS
        if (recv_len == SOCKET_ERROR) {
            // On Windows, on the first try, select says there is something
            // but recv doesn't succeed yet, and we just need to try again.
            if (WSAGetLastError() == WSAEWOULDBLOCK) {
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            }
            // And at the end, we get an abort that we can silently ignore.
            if (WSAGetLastError() == WSAECONNABORTED) {
                return;
            }
        }
#else
        if (recv_len < 0) {
            // On macOS we presumably see the same thing, and have to try again.
            if (errno == EAGAIN) {
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            }

            LogErr() << "recv failed: " << GET_ERROR(errno);
            return;
        }
#endif

        if (recv_len == 0) {
            continue;
        }

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

        // Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.
        while (_mavlink_receiver->parse_message()) {
            receive_message(_mavlink_receiver->get_last_message(), this);
        }

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

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

std::pair<bool, std::string> TcpServerConnection::send_raw_bytes(const char* bytes, size_t length)
{
    // Basic implementation for TCP server connections
    std::pair<bool, std::string> result;

#if !defined(MSG_NOSIGNAL)
    auto flags = 0;
#else
    auto flags = MSG_NOSIGNAL;
#endif

    const auto send_len = send(_client_socket_fd.get(), bytes, length, flags);

    if (send_len != static_cast<std::remove_cv_t<decltype(send_len)>>(length)) {
        std::stringstream ss;
        ss << "Send failure: " << GET_ERROR(errno);
        LogErr() << ss.str();
        result.first = false;
        result.second = ss.str();
        return result;
    }

    result.first = true;
    return result;
}

} // namespace mavsdk

1	#include "tcp_server_connection.h"
2	#include "log.h"
3
4	#include <cassert>
5	#include <fcntl.h>
6	#include <sstream>
7
8	#ifdef WINDOWS
9	#ifndef MINGW
10	#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
11	#endif
12	#include <Windows.h>
13	#include <Winsock2.h>
14	#include <ws2tcpip.h>
15	#else
16	#include <netinet/in.h>
17	#include <sys/select.h>
18	#include <sys/socket.h>
19	#include <arpa/inet.h>
20	#include <errno.h>
21	#include <netdb.h>
22	#endif
23
24	#ifndef WINDOWS
25	#define GET_ERROR(_x) strerror(_x)
26	#else
27	#define GET_ERROR(_x) WSAGetLastError()
28	#endif
29
30	namespace mavsdk {
31	TcpServerConnection::TcpServerConnection(	1✔
32	Connection::ReceiverCallback receiver_callback,
33	Connection::LibmavReceiverCallback libmav_receiver_callback,
34	MavsdkImpl& mavsdk_impl,
35	std::string local_ip,
36	int local_port,
37	ForwardingOption forwarding_option) :	1✔
38	Connection(
39	std::move(receiver_callback),	1✔
40	std::move(libmav_receiver_callback),	1✔
41	mavsdk_impl,
42	forwarding_option),
43	_local_ip(std::move(local_ip)),	1✔
44	_local_port(local_port)	3✔
45	{}	1✔
46
47	TcpServerConnection::~TcpServerConnection()	2✔
48	{
49	stop();	1✔
50	}	2✔
51
52	ConnectionResult TcpServerConnection::start()	1✔
53	{
54	if (!start_mavlink_receiver()) {	1✔
55	return ConnectionResult::ConnectionsExhausted;	×
56	}
57
58	if (!start_libmav_receiver()) {	1✔
59	return ConnectionResult::ConnectionsExhausted;	×
60	}
61
62	#ifdef WINDOWS
63	WSADATA wsa;
64	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
65	LogErr() << "Error: Winsock failed, error: " << WSAGetLastError();
66	return ConnectionResult::SocketError;
67	}
68	#endif
69
70	_server_socket_fd.reset(socket(AF_INET, SOCK_STREAM, 0));	1✔
71	if (_server_socket_fd.empty()) {	1✔
72	LogErr() << "socket error: " << GET_ERROR(errno);	×
73	return ConnectionResult::SocketError;	×
74	}
75
76	sockaddr_in server_addr{};	1✔
77	server_addr.sin_family = AF_INET;	1✔
78	server_addr.sin_addr.s_addr = INADDR_ANY;	1✔
79	server_addr.sin_port = htons(_local_port);	1✔
80
81	if (bind(	1✔
82	_server_socket_fd.get(),
83	reinterpret_cast<sockaddr*>(&server_addr),
84	sizeof(server_addr)) < 0) {	1✔
85	LogErr() << "bind error: " << GET_ERROR(errno);	×
86	return ConnectionResult::SocketError;	×
87	}
88
89	if (listen(_server_socket_fd.get(), 3) < 0) {	1✔
90	LogErr() << "listen error: " << GET_ERROR(errno);	×
91	return ConnectionResult::SocketError;	×
92	}
93
94	// Set receive timeout cross-platform
95	const unsigned timeout_ms = 500;	1✔
96
97	#if defined(WINDOWS)
98	setsockopt(
99	_server_socket_fd.get(),
100	SOL_SOCKET,
101	SO_RCVTIMEO,
102	(const char*)&timeout_ms,
103	sizeof(timeout_ms));
104	setsockopt(
105	_client_socket_fd.get(),
106	SOL_SOCKET,
107	SO_RCVTIMEO,
108	(const char*)&timeout_ms,
109	sizeof(timeout_ms));
110	#else
111	struct timeval tv;
112	tv.tv_sec = 0;	1✔
113	tv.tv_usec = timeout_ms * 1000;	1✔
114	setsockopt(_server_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const void*)&tv, sizeof(tv));	1✔
115	setsockopt(_client_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const void*)&tv, sizeof(tv));	1✔
116	#endif
117
118	_accept_receive_thread =
119	std::make_unique<std::thread>(&TcpServerConnection::accept_client, this);	1✔
120
121	return ConnectionResult::Success;	1✔
122	}
123
124	ConnectionResult TcpServerConnection::stop()	1✔
125	{
126	_should_exit = true;	1✔
127
128	if (_accept_receive_thread && _accept_receive_thread->joinable()) {	1✔
129	_accept_receive_thread->join();	1✔
130	_accept_receive_thread.reset();	1✔
131	}
132
133	_client_socket_fd.close();	1✔
134	_server_socket_fd.close();	1✔
135
136	// We need to stop this after stopping the receive thread, otherwise
137	// it can happen that we interfere with the parsing of a message.
138	stop_mavlink_receiver();	1✔
139
140	return ConnectionResult::Success;	1✔
141	}
142
143	std::pair<bool, std::string> TcpServerConnection::send_message(const mavlink_message_t& message)	7✔
144	{
145	// Convert message to raw bytes and use common send path
146	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
147	uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);	7✔
148
149	assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);	7✔
150
151	return send_raw_bytes(reinterpret_cast<const char*>(buffer), buffer_len);	7✔
152	}
153
154	void TcpServerConnection::accept_client()	1✔
155	{
156	#ifdef WINDOWS
157	// Set server socket to non-blocking
158	u_long iMode = 1;
159	int iResult = ioctlsocket(_server_socket_fd.get(), FIONBIO, &iMode);
160	if (iResult != 0) {
161	LogErr() << "ioctlsocket failed with error: " << WSAGetLastError();
162	}
163	#else
164	// Set server socket to non-blocking
165	int flags = fcntl(_server_socket_fd.get(), F_GETFL, 0);	1✔
166	fcntl(_server_socket_fd.get(), F_SETFL, flags \| O_NONBLOCK);	1✔
167	#endif
168
169	while (!_should_exit) {	2✔
170	fd_set readfds;
171	FD_ZERO(&readfds);	17✔
172	FD_SET(_server_socket_fd.get(), &readfds);	1✔
173
174	// Set timeout to 1 second
175	timeval timeout;
176	timeout.tv_sec = 1;	1✔
177	timeout.tv_usec = 0;	1✔
178
179	const int activity =
180	select(_server_socket_fd.get() + 1, &readfds, nullptr, nullptr, &timeout);	1✔
181
182	if (activity < 0 && errno != EINTR) {	1✔
183	LogErr() << "select error: " << GET_ERROR(errno);	×
184	continue;	×
185	}
186
187	if (activity == 0) {	1✔
188	// Timeout, no incoming connection
189	continue;	×
190	}
191
192	if (FD_ISSET(_server_socket_fd.get(), &readfds)) {	1✔
193	sockaddr_in client_addr{};	1✔
194	socklen_t client_addr_len = sizeof(client_addr);	1✔
195
196	_client_socket_fd.reset(accept(	1✔
197	_server_socket_fd.get(),
198	reinterpret_cast<sockaddr*>(&client_addr),
199	&client_addr_len));
200	if (_client_socket_fd.empty()) {	1✔
201	if (_should_exit) {	×
202	return;	×
203	}
204	LogErr() << "accept error: " << GET_ERROR(errno);	×
205	continue;	×
206	}
207
208	receive();	1✔
209	}
210	}
211	}
212
213	void TcpServerConnection::receive()	1✔
214	{
215	std::array<char, 2048> buffer{};	1✔
216
217	bool dataReceived = false;	1✔
218	while (!dataReceived && !_should_exit) {	14✔
219	const auto recv_len = recv(_client_socket_fd.get(), buffer.data(), buffer.size(), 0);	13✔
220
221	#ifdef WINDOWS
222	if (recv_len == SOCKET_ERROR) {
223	// On Windows, on the first try, select says there is something
224	// but recv doesn't succeed yet, and we just need to try again.
225	if (WSAGetLastError() == WSAEWOULDBLOCK) {
226	std::this_thread::sleep_for(std::chrono::milliseconds(10));
227	continue;
228	}
229	// And at the end, we get an abort that we can silently ignore.
230	if (WSAGetLastError() == WSAECONNABORTED) {
231	return;
232	}
233	}
234	#else
235	if (recv_len < 0) {	13✔
236	// On macOS we presumably see the same thing, and have to try again.
237	if (errno == EAGAIN) {	2✔
238	std::this_thread::sleep_for(std::chrono::milliseconds(10));	2✔
239	continue;	2✔
240	}
241
242	LogErr() << "recv failed: " << GET_ERROR(errno);	×
243	return;	×
244	}
245	#endif
246
247	if (recv_len == 0) {	11✔
248	continue;	×
249	}
250
251	_mavlink_receiver->set_new_datagram(buffer.data(), static_cast<int>(recv_len));	11✔
252
253	// Parse all mavlink messages in one data packet. Once exhausted, we'll exit while.
254	while (_mavlink_receiver->parse_message()) {	23✔
255	receive_message(_mavlink_receiver->get_last_message(), this);	12✔
256	}
257
258	// Also parse with libmav if available
259	if (_libmav_receiver) {	11✔
260	_libmav_receiver->set_new_datagram(buffer.data(), static_cast<int>(recv_len));	11✔
261
262	while (_libmav_receiver->parse_message()) {	22✔
263	receive_libmav_message(_libmav_receiver->get_last_message(), this);	11✔
264	}
265	}
266	}
267	}
268
269	std::pair<bool, std::string> TcpServerConnection::send_raw_bytes(const char* bytes, size_t length)	7✔
270	{
271	// Basic implementation for TCP server connections
272	std::pair<bool, std::string> result;	7✔
273
274	#if !defined(MSG_NOSIGNAL)
275	auto flags = 0;
276	#else
277	auto flags = MSG_NOSIGNAL;	7✔
278	#endif
279
280	const auto send_len = send(_client_socket_fd.get(), bytes, length, flags);	7✔
281
282	if (send_len != static_cast<std::remove_cv_t<decltype(send_len)>>(length)) {	7✔
283	std::stringstream ss;	×
284	ss << "Send failure: " << GET_ERROR(errno);	×
285	LogErr() << ss.str();	×
286	result.first = false;	×
287	result.second = ss.str();	×
288	return result;	×
289	}	×
290
291	result.first = true;	7✔
292	return result;	7✔
293	}
294
295	} // namespace mavsdk

mavlink / MAVSDK / 17565334070

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