16247112396

Committed 13 Jul 2025 08:18AM UTC coverage: 46.221% (+1.0%) from 45.212%

Build # 16247112396

Build Type

Pull #2610

github

Committed by

web-flow

Commit Message

Merge a15009557 into 6c112e71f

Pull Request Pull Request #2610: Integrate parts of libmav into MAVSDK and add MavlinkDirect plugin

Run Details

727 of 980 new or added lines in 15 files covered. (74.18%)

27 existing lines in 5 files now uncovered.

16217 of 35086 relevant lines covered (46.22%)

148063.0 hits per line

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

67.62

/src/mavsdk/core/tcp_client_connection.cpp

#include "tcp_client_connection.h"
#include "log.h"

#include <cassert>
#include <sstream>
#include <utility>
#include <thread>
#include <sstream>

#ifdef WINDOWS
#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>
#include <netdb.h>
#endif

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

namespace mavsdk {

/* change to remote_ip and remote_port */
TcpClientConnection::TcpClientConnection(
    Connection::ReceiverCallback receiver_callback,
    std::string remote_ip,
    int remote_port,
    ForwardingOption forwarding_option) :
    Connection(std::move(receiver_callback), forwarding_option),
    _remote_ip(std::move(remote_ip)),
    _remote_port_number(remote_port),
    _should_exit(false)
{}

TcpClientConnection::TcpClientConnection(
    Connection::ReceiverCallback receiver_callback,
    Connection::LibmavReceiverCallback libmav_receiver_callback,
    std::string remote_ip,
    int remote_port,
    ForwardingOption forwarding_option) :
    Connection(
        std::move(receiver_callback), std::move(libmav_receiver_callback), forwarding_option),
    _remote_ip(std::move(remote_ip)),
    _remote_port_number(remote_port),
    _should_exit(false)
{}

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

ConnectionResult TcpClientConnection::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 TcpClientConnection::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_STREAM, 0));

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

    struct sockaddr_in remote_addr {};
    remote_addr.sin_family = AF_INET;
    remote_addr.sin_port = htons(_remote_port_number);

    struct hostent* hp;
    hp = gethostbyname(_remote_ip.c_str());
    if (hp == nullptr) {
        LogErr() << "Could not get host by name";
        return ConnectionResult::SocketConnectionError;
    }

    memcpy(&remote_addr.sin_addr, hp->h_addr, hp->h_length);

    if (connect(
            _socket_fd.get(),
            reinterpret_cast<sockaddr*>(&remote_addr),
            sizeof(struct sockaddr_in)) < 0) {
        LogErr() << "connect error: " << GET_ERROR(errno);
        return ConnectionResult::SocketConnectionError;
    }

    // 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 TcpClientConnection::start_recv_thread()
{
    _recv_thread = std::make_unique<std::thread>(&TcpClientConnection::receive, this);
}

ConnectionResult TcpClientConnection::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> TcpClientConnection::send_message(const mavlink_message_t& message)
{
    std::pair<bool, std::string> result;

    if (_remote_ip.empty()) {
        result.first = false;
        result.second = "Remote IP unknown";
        LogErr() << result.second;
        return result;
    }

    if (_remote_port_number == 0) {
        result.first = false;
        result.second = "Remote port unknown";
        LogErr() << result.second;
        return result;
    }

    struct sockaddr_in dest_addr {};
    dest_addr.sin_family = AF_INET;

    inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr);

    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);

    // TODO: remove this assert again
    assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);

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

    const auto send_len =
        send(_socket_fd.get(), reinterpret_cast<const char*>(buffer), buffer_len, flags);

    if (send_len != buffer_len) {
        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;
}

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

    while (!_should_exit) {
        const auto recv_len = recv(_socket_fd.get(), buffer, sizeof(buffer), 0);

        if (recv_len == 0 || (recv_len < 0 && (errno == EAGAIN || errno == ETIMEDOUT))) {
            // Timeout, just try again.
            continue;
        }

        if (recv_len < 0) {
            LogErr() << "TCP receive error: " << GET_ERROR(errno) << ", trying to reeconnect...";
            std::this_thread::sleep_for(std::chrono::seconds(1));
            setup_port();
            continue;
        }

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

        while (_mavlink_receiver->parse_message()) {
            receive_message(_mavlink_receiver->get_last_message(), this);
        }
    }
}

} // namespace mavsdk

1	#include "tcp_client_connection.h"
2	#include "log.h"
3
4	#include <cassert>
5	#include <sstream>
6	#include <utility>
7	#include <thread>
8	#include <sstream>
9
10	#ifdef WINDOWS
11	#ifndef MINGW
12	#pragma comment(lib, "Ws2_32.lib") // Without this, Ws2_32.lib is not included in static library.
13	#endif
14	#else
15	#include <netinet/in.h>
16	#include <sys/socket.h>
17	#include <arpa/inet.h>
18	#include <errno.h>
19	#include <netdb.h>
20	#endif
21
22	#ifndef WINDOWS
23	#define GET_ERROR(_x) strerror(_x)
24	#else
25	#define GET_ERROR(_x) WSAGetLastError()
26	#endif
27
28	namespace mavsdk {
29
30	/* change to remote_ip and remote_port */
UNCOV 31	TcpClientConnection::TcpClientConnection(	×
32	Connection::ReceiverCallback receiver_callback,
33	std::string remote_ip,
34	int remote_port,
UNCOV 35	ForwardingOption forwarding_option) :	×
UNCOV 36	Connection(std::move(receiver_callback), forwarding_option),	×
UNCOV 37	_remote_ip(std::move(remote_ip)),	×
UNCOV 38	_remote_port_number(remote_port),	×
UNCOV 39	_should_exit(false)	×
UNCOV 40	{}	×
41
42	TcpClientConnection::TcpClientConnection(	1✔
43	Connection::ReceiverCallback receiver_callback,
44	Connection::LibmavReceiverCallback libmav_receiver_callback,
45	std::string remote_ip,
46	int remote_port,
47	ForwardingOption forwarding_option) :	1✔
48	Connection(
49	std::move(receiver_callback), std::move(libmav_receiver_callback), forwarding_option),	2✔
50	_remote_ip(std::move(remote_ip)),	1✔
51	_remote_port_number(remote_port),	1✔
52	_should_exit(false)	4✔
53	{}	1✔
54
55	TcpClientConnection::~TcpClientConnection()	2✔
56	{
57	// If no one explicitly called stop before, we should at least do it.
58	stop();	1✔
59	}	2✔
60
61	ConnectionResult TcpClientConnection::start()	1✔
62	{
63	if (!start_mavlink_receiver()) {	1✔
64	return ConnectionResult::ConnectionsExhausted;	×
65	}
66
67	ConnectionResult ret = setup_port();	1✔
68	if (ret != ConnectionResult::Success) {	1✔
69	return ret;	×
70	}
71
72	start_recv_thread();	1✔
73
74	return ConnectionResult::Success;	1✔
75	}
76
77	ConnectionResult TcpClientConnection::setup_port()	1✔
78	{
79	#ifdef WINDOWS
80	WSADATA wsa;
81	if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
82	LogErr() << "Error: Winsock failed, error: %d", WSAGetLastError();
83	return ConnectionResult::SocketError;
84	}
85	#endif
86
87	_socket_fd.reset(socket(AF_INET, SOCK_STREAM, 0));	1✔
88
89	if (_socket_fd.empty()) {	1✔
90	LogErr() << "socket error" << GET_ERROR(errno);	×
91	return ConnectionResult::SocketError;	×
92	}
93
94	struct sockaddr_in remote_addr {};	1✔
95	remote_addr.sin_family = AF_INET;	1✔
96	remote_addr.sin_port = htons(_remote_port_number);	1✔
97
98	struct hostent* hp;
99	hp = gethostbyname(_remote_ip.c_str());	1✔
100	if (hp == nullptr) {	1✔
101	LogErr() << "Could not get host by name";	×
102	return ConnectionResult::SocketConnectionError;	×
103	}
104
105	memcpy(&remote_addr.sin_addr, hp->h_addr, hp->h_length);	1✔
106
107	if (connect(	1✔
108	_socket_fd.get(),
109	reinterpret_cast<sockaddr*>(&remote_addr),
110	sizeof(struct sockaddr_in)) < 0) {	1✔
111	LogErr() << "connect error: " << GET_ERROR(errno);	×
112	return ConnectionResult::SocketConnectionError;	×
113	}
114
115	// Set receive timeout cross-platform
116	const unsigned timeout_ms = 500;	1✔
117
118	#if defined(WINDOWS)
119	setsockopt(
120	_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_ms, sizeof(timeout_ms));
121	#else
122	struct timeval tv;	1✔
123	tv.tv_sec = 0;	1✔
124	tv.tv_usec = timeout_ms * 1000;	1✔
125	setsockopt(_socket_fd.get(), SOL_SOCKET, SO_RCVTIMEO, (const void*)&tv, sizeof(tv));	1✔
126	#endif
127
128	return ConnectionResult::Success;	1✔
129	}
130
131	void TcpClientConnection::start_recv_thread()	1✔
132	{
133	_recv_thread = std::make_unique<std::thread>(&TcpClientConnection::receive, this);	1✔
134	}	1✔
135
136	ConnectionResult TcpClientConnection::stop()	1✔
137	{
138	_should_exit = true;	1✔
139
140	if (_recv_thread) {	1✔
141	_recv_thread->join();	1✔
142	_recv_thread.reset();	1✔
143	}
144
145	_socket_fd.close();	1✔
146
147	// We need to stop this after stopping the receive thread, otherwise
148	// it can happen that we interfere with the parsing of a message.
149	stop_mavlink_receiver();	1✔
150
151	return ConnectionResult::Success;	1✔
152	}
153
154	std::pair<bool, std::string> TcpClientConnection::send_message(const mavlink_message_t& message)	13✔
155	{
156	std::pair<bool, std::string> result;	13✔
157
158	if (_remote_ip.empty()) {	13✔
159	result.first = false;	×
160	result.second = "Remote IP unknown";	×
161	LogErr() << result.second;	×
162	return result;	×
163	}
164
165	if (_remote_port_number == 0) {	13✔
166	result.first = false;	×
167	result.second = "Remote port unknown";	×
168	LogErr() << result.second;	×
169	return result;	×
170	}
171
172	struct sockaddr_in dest_addr {};	13✔
173	dest_addr.sin_family = AF_INET;	13✔
174
175	inet_pton(AF_INET, _remote_ip.c_str(), &dest_addr.sin_addr.s_addr);	13✔
176
177	dest_addr.sin_port = htons(_remote_port_number);	13✔
178
179	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];	13✔
180	uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);	13✔
181
182	// TODO: remove this assert again
183	assert(buffer_len <= MAVLINK_MAX_PACKET_LEN);	13✔
184
185	#if !defined(MSG_NOSIGNAL)
186	auto flags = 0;
187	#else
188	auto flags = MSG_NOSIGNAL;	13✔
189	#endif
190
191	const auto send_len =
192	send(_socket_fd.get(), reinterpret_cast<const char*>(buffer), buffer_len, flags);	13✔
193
194	if (send_len != buffer_len) {	13✔
195	std::stringstream ss;	×
196	ss << "Send failure: " << GET_ERROR(errno);	×
197	LogErr() << ss.str();	×
198	result.first = false;	×
199	result.second = ss.str();	×
200	return result;	×
201	}	×
202
203	result.first = true;	13✔
204	return result;	13✔
205	}
206
207	void TcpClientConnection::receive()	1✔
208	{
209	// Enough for MTU 1500 bytes.
210	char buffer[2048];	1✔
211
212	while (!_should_exit) {	10✔
213	const auto recv_len = recv(_socket_fd.get(), buffer, sizeof(buffer), 0);	9✔
214
215	if (recv_len == 0 \|\| (recv_len < 0 && (errno == EAGAIN \|\| errno == ETIMEDOUT))) {	9✔
216	// Timeout, just try again.
217	continue;	2✔
218	}
219
220	if (recv_len < 0) {	7✔
221	LogErr() << "TCP receive error: " << GET_ERROR(errno) << ", trying to reeconnect...";	×
222	std::this_thread::sleep_for(std::chrono::seconds(1));	×
223	setup_port();	×
224	continue;	×
225	}
226
227	_mavlink_receiver->set_new_datagram(buffer, static_cast<int>(recv_len));	7✔
228
229	while (_mavlink_receiver->parse_message()) {	14✔
230	receive_message(_mavlink_receiver->get_last_message(), this);	7✔
231	}
232	}
233	}	1✔
234
235	} // namespace mavsdk

mavlink / MAVSDK / 16247112396

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