20150089450

Committed 11 Dec 2025 10:55PM UTC coverage: 48.021% (+0.06%) from 47.958%

Build # 20150089450

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2734 from mavlink/pr-parsing-fix

core: fix bug where invalid messages are parsed

Run Details

51 of 59 new or added lines in 8 files covered. (86.44%)

3 existing lines in 3 files now uncovered.

17664 of 36784 relevant lines covered (48.02%)

466.99 hits per line

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

0.0

/src/mavsdk/core/serial_connection.cpp

#include "serial_connection.h"
#include "log.h"

#if defined(APPLE) || defined(LINUX)
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <poll.h>

#include <utility>
#endif

#include <sstream>

namespace mavsdk {

#ifndef WINDOWS
#define GET_ERROR() strerror(errno)
#else
#define GET_ERROR() GetLastErrorStdStr()
// Taken from:
// https://coolcowstudio.wordpress.com/2012/10/19/getlasterror-as-stdstring/
std::string GetLastErrorStdStr()
{
    DWORD error = GetLastError();
    if (error == 0) {
        return "";
    }

    LPVOID lpMsgBuf = nullptr;
    DWORD bufLen = FormatMessage(
        FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
        NULL,
        error,
        MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
        (LPTSTR)&lpMsgBuf,
        0,
        NULL);

    if (bufLen) {
        LPCSTR lpMsgStr = (LPCSTR)lpMsgBuf;
        std::string result(lpMsgStr, lpMsgStr + bufLen);
        LocalFree(lpMsgBuf);

        // Remove trailing newline if present
        if (!result.empty() && result[result.length() - 1] == '\n') {
            result.erase(result.length() - 1);
        }
        if (!result.empty() && result[result.length() - 1] == '\r') {
            result.erase(result.length() - 1);
        }

        return result;
    }

    return std::to_string(error);
}
#endif

SerialConnection::SerialConnection(
    Connection::ReceiverCallback receiver_callback,
    Connection::LibmavReceiverCallback libmav_receiver_callback,
    MavsdkImpl& mavsdk_impl,
    std::string path,
    int baudrate,
    bool flow_control,
    ForwardingOption forwarding_option) :
    Connection(
        std::move(receiver_callback),
        std::move(libmav_receiver_callback),
        mavsdk_impl,
        forwarding_option),
    _serial_node(std::move(path)),
    _baudrate(baudrate),
    _flow_control(flow_control)
{}

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

ConnectionResult SerialConnection::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 SerialConnection::setup_port()
{
#if defined(LINUX) || defined(APPLE)
    // open() hangs on macOS or Linux devices(e.g. pocket beagle) unless you give it O_NONBLOCK
    _fd = open(_serial_node.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (_fd == -1) {
        LogErr() << "open failed: " << GET_ERROR();
        return ConnectionResult::ConnectionError;
    }
    // We need to clear the O_NONBLOCK again because we can block while reading
    // as we do it in a separate thread.
    if (fcntl(_fd, F_SETFL, 0) == -1) {
        LogErr() << "fcntl failed: " << GET_ERROR();
        return ConnectionResult::ConnectionError;
    }
#elif defined(WINDOWS)
    // Required for COM ports > 9.
    const auto full_serial_path = "\\\\.\\" + _serial_node;

    _handle = CreateFile(
        full_serial_path.c_str(),
        GENERIC_READ | GENERIC_WRITE,
        0, // exclusive-access
        NULL, //  default security attributes
        OPEN_EXISTING,
        0, //  not overlapped I/O
        NULL); //  hTemplate must be NULL for comm devices

    if (_handle == INVALID_HANDLE_VALUE) {
        LogErr() << "CreateFile failed with: " << GET_ERROR();
        return ConnectionResult::ConnectionError;
    }
#endif

#if defined(LINUX) || defined(APPLE)
    struct termios tc;
    bzero(&tc, sizeof(tc));

    if (tcgetattr(_fd, &tc) != 0) {
        LogErr() << "tcgetattr failed: " << GET_ERROR();
        close(_fd);
        return ConnectionResult::ConnectionError;
    }
#endif

#if defined(LINUX) || defined(APPLE)
    tc.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
    tc.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
    tc.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG | TOSTOP);
    tc.c_cflag &= ~(CSIZE | PARENB | CRTSCTS);
    tc.c_cflag |= CS8;

    tc.c_cc[VMIN] = 0; // We are ok with 0 bytes.
    tc.c_cc[VTIME] = 10; // Timeout after 1 second.

    if (_flow_control) {
        tc.c_cflag |= CRTSCTS;
    }
#endif

#if defined(LINUX) || defined(APPLE)
    tc.c_cflag |= CLOCAL; // Without this a write() blocks indefinitely.

#if defined(LINUX)
    const int baudrate_or_define = define_from_baudrate(_baudrate);
#elif defined(APPLE)
    const int baudrate_or_define = _baudrate;
#endif

    if (baudrate_or_define == -1) {
        return ConnectionResult::BaudrateUnknown;
    }

    if (cfsetispeed(&tc, baudrate_or_define) != 0) {
        LogErr() << "cfsetispeed failed: " << GET_ERROR();
        close(_fd);
        return ConnectionResult::ConnectionError;
    }

    if (cfsetospeed(&tc, baudrate_or_define) != 0) {
        LogErr() << "cfsetospeed failed: " << GET_ERROR();
        close(_fd);
        return ConnectionResult::ConnectionError;
    }

    if (tcsetattr(_fd, TCSANOW, &tc) != 0) {
        LogErr() << "tcsetattr failed: " << GET_ERROR();
        close(_fd);
        return ConnectionResult::ConnectionError;
    }
#endif

#if defined(WINDOWS)
    DCB dcb;
    SecureZeroMemory(&dcb, sizeof(DCB));
    dcb.DCBlength = sizeof(DCB);

    if (!GetCommState(_handle, &dcb)) {
        LogErr() << "GetCommState failed with error: " << GET_ERROR();
        return ConnectionResult::ConnectionError;
    }

    dcb.BaudRate = _baudrate;
    dcb.ByteSize = 8;
    dcb.Parity = NOPARITY;
    dcb.StopBits = ONESTOPBIT;
    if (_flow_control) {
        dcb.fOutxCtsFlow = TRUE;
        dcb.fDtrControl = DTR_CONTROL_HANDSHAKE;
        dcb.fRtsControl = RTS_CONTROL_HANDSHAKE;
    } else {
        dcb.fDtrControl = DTR_CONTROL_DISABLE;
        dcb.fRtsControl = RTS_CONTROL_DISABLE;
    }
    dcb.fOutX = FALSE;
    dcb.fInX = FALSE;
    dcb.fBinary = TRUE;
    dcb.fNull = FALSE;
    dcb.fDsrSensitivity = FALSE;

    if (!SetCommState(_handle, &dcb)) {
        LogErr() << "SetCommState failed with error: " << GET_ERROR();
        return ConnectionResult::ConnectionError;
    }

    COMMTIMEOUTS timeout = {0, 0, 0, 0, 0};
    timeout.ReadIntervalTimeout = 1;
    timeout.ReadTotalTimeoutConstant = 1;
    timeout.ReadTotalTimeoutMultiplier = 1;
    timeout.WriteTotalTimeoutConstant = 1;
    timeout.WriteTotalTimeoutMultiplier = 1;
    SetCommTimeouts(_handle, &timeout);

    if (!SetCommTimeouts(_handle, &timeout)) {
        LogErr() << "SetCommTimeouts failed with error: " << GET_ERROR();
        return ConnectionResult::ConnectionError;
    }

#endif

    return ConnectionResult::Success;
}

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

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

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

#if defined(LINUX) || defined(APPLE)
    close(_fd);
#elif defined(WINDOWS)
    CloseHandle(_handle);
#endif

    // 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> SerialConnection::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);
}

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

#if defined(LINUX) || defined(APPLE)
    struct pollfd fds[1];
    fds[0].fd = _fd;
    fds[0].events = POLLIN;
#endif

    while (!_should_exit) {
        int recv_len;
#if defined(LINUX) || defined(APPLE)
        int pollrc = poll(fds, 1, 1000);
        if (pollrc == 0 || !(fds[0].revents & POLLIN)) {
            continue;
        } else if (pollrc == -1) {
            LogErr() << "read poll failure: " << GET_ERROR();
        }
        // We enter here if (fds[0].revents & POLLIN) == true
        recv_len = static_cast<int>(read(_fd, buffer, sizeof(buffer)));
        if (recv_len < -1) {
            LogErr() << "read failure: " << GET_ERROR();
        }
#else
        if (!ReadFile(_handle, buffer, sizeof(buffer), LPDWORD(&recv_len), NULL)) {
            LogErr() << "ReadFile failure: " << GET_ERROR();
            continue;
        }
#endif
        if (recv_len > static_cast<int>(sizeof(buffer)) || recv_len == 0) {
            continue;
        }
        _mavlink_receiver->set_new_datagram(buffer, recv_len);
        // Parse all mavlink messages in one data packet. Once exhausted, we'll exit loop.
        auto parse_result = _mavlink_receiver->parse_message();
        while (parse_result != MavlinkReceiver::ParseResult::NoneAvailable) {
            receive_message(parse_result, _mavlink_receiver->get_last_message(), this);
            parse_result = _mavlink_receiver->parse_message();
        }

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

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

#if defined(LINUX)
int SerialConnection::define_from_baudrate(int baudrate)
{
    switch (baudrate) {
        case 9600:
            return B9600;
        case 19200:
            return B19200;
        case 38400:
            return B38400;
        case 57600:
            return B57600;
        case 115200:
            return B115200;
        case 230400:
            return B230400;
        case 460800:
            return B460800;
        case 500000:
            return B500000;
        case 576000:
            return B576000;
        case 921600:
            return B921600;
        case 1000000:
            return B1000000;
        case 1152000:
            return B1152000;
        case 1500000:
            return B1500000;
        case 2000000:
            return B2000000;
        case 2500000:
            return B2500000;
        case 3000000:
            return B3000000;
        case 3500000:
            return B3500000;
        case 4000000:
            return B4000000;
        default: {
            LogErr() << "Unknown baudrate";
            return -1;
        }
    }
}
#endif

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

    if (_serial_node.empty()) {
        LogErr() << "Dev Path unknown";
        result.first = false;
        result.second = "Dev Path unknown";
        return result;
    }

    if (_baudrate == 0) {
        result.first = false;
        result.second = "Baudrate unknown";
        return result;
    }

    int send_len;
#if defined(LINUX) || defined(APPLE)
    send_len = static_cast<int>(write(_fd, bytes, length));
#else
    if (!WriteFile(_handle, bytes, static_cast<DWORD>(length), LPDWORD(&send_len), NULL)) {
        std::stringstream ss;
        ss << "WriteFile failure: " << GET_ERROR();
        LogErr() << ss.str();
        result.first = false;
        result.second = ss.str();
        return result;
    }
#endif

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

    result.first = true;
    return result;
}

} // namespace mavsdk

1	#include "serial_connection.h"
2	#include "log.h"
3
4	#if defined(APPLE) \|\| defined(LINUX)
5	#include <unistd.h>
6	#include <fcntl.h>
7	#include <termios.h>
8	#include <poll.h>
9
10	#include <utility>
11	#endif
12
13	#include <sstream>
14
15	namespace mavsdk {
16
17	#ifndef WINDOWS
18	#define GET_ERROR() strerror(errno)
19	#else
20	#define GET_ERROR() GetLastErrorStdStr()
21	// Taken from:
22	// https://coolcowstudio.wordpress.com/2012/10/19/getlasterror-as-stdstring/
23	std::string GetLastErrorStdStr()
24	{
25	DWORD error = GetLastError();
26	if (error == 0) {
27	return "";
28	}
29
30	LPVOID lpMsgBuf = nullptr;
31	DWORD bufLen = FormatMessage(
32	FORMAT_MESSAGE_ALLOCATE_BUFFER \| FORMAT_MESSAGE_FROM_SYSTEM \| FORMAT_MESSAGE_IGNORE_INSERTS,
33	NULL,
34	error,
35	MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
36	(LPTSTR)&lpMsgBuf,
37	0,
38	NULL);
39
40	if (bufLen) {
41	LPCSTR lpMsgStr = (LPCSTR)lpMsgBuf;
42	std::string result(lpMsgStr, lpMsgStr + bufLen);
43	LocalFree(lpMsgBuf);
44
45	// Remove trailing newline if present
46	if (!result.empty() && result[result.length() - 1] == '\n') {
47	result.erase(result.length() - 1);
48	}
49	if (!result.empty() && result[result.length() - 1] == '\r') {
50	result.erase(result.length() - 1);
51	}
52
53	return result;
54	}
55
56	return std::to_string(error);
57	}
58	#endif
59
60	SerialConnection::SerialConnection(	×
61	Connection::ReceiverCallback receiver_callback,
62	Connection::LibmavReceiverCallback libmav_receiver_callback,
63	MavsdkImpl& mavsdk_impl,
64	std::string path,
65	int baudrate,
66	bool flow_control,
67	ForwardingOption forwarding_option) :	×
68	Connection(
69	std::move(receiver_callback),	×
70	std::move(libmav_receiver_callback),	×
71	mavsdk_impl,
72	forwarding_option),
73	_serial_node(std::move(path)),	×
74	_baudrate(baudrate),	×
75	_flow_control(flow_control)	×
76	{}	×
77
78	SerialConnection::~SerialConnection()	×
79	{
80	// If no one explicitly called stop before, we should at least do it.
81	stop();	×
82	}	×
83
84	ConnectionResult SerialConnection::start()	×
85	{
86	if (!start_mavlink_receiver()) {	×
87	return ConnectionResult::ConnectionsExhausted;	×
88	}
89
90	if (!start_libmav_receiver()) {	×
91	return ConnectionResult::ConnectionsExhausted;	×
92	}
93
94	ConnectionResult ret = setup_port();	×
95	if (ret != ConnectionResult::Success) {	×
96	return ret;	×
97	}
98
99	start_recv_thread();	×
100
101	return ConnectionResult::Success;	×
102	}
103
104	ConnectionResult SerialConnection::setup_port()	×
105	{
106	#if defined(LINUX) \|\| defined(APPLE)
107	// open() hangs on macOS or Linux devices(e.g. pocket beagle) unless you give it O_NONBLOCK
108	_fd = open(_serial_node.c_str(), O_RDWR \| O_NOCTTY \| O_NONBLOCK);	×
109	if (_fd == -1) {	×
110	LogErr() << "open failed: " << GET_ERROR();	×
111	return ConnectionResult::ConnectionError;	×
112	}
113	// We need to clear the O_NONBLOCK again because we can block while reading
114	// as we do it in a separate thread.
115	if (fcntl(_fd, F_SETFL, 0) == -1) {	×
116	LogErr() << "fcntl failed: " << GET_ERROR();	×
117	return ConnectionResult::ConnectionError;	×
118	}
119	#elif defined(WINDOWS)
120	// Required for COM ports > 9.
121	const auto full_serial_path = "\\\\.\\" + _serial_node;
122
123	_handle = CreateFile(
124	full_serial_path.c_str(),
125	GENERIC_READ \| GENERIC_WRITE,
126	0, // exclusive-access
127	NULL, // default security attributes
128	OPEN_EXISTING,
129	0, // not overlapped I/O
130	NULL); // hTemplate must be NULL for comm devices
131
132	if (_handle == INVALID_HANDLE_VALUE) {
133	LogErr() << "CreateFile failed with: " << GET_ERROR();
134	return ConnectionResult::ConnectionError;
135	}
136	#endif
137
138	#if defined(LINUX) \|\| defined(APPLE)
139	struct termios tc;
140	bzero(&tc, sizeof(tc));	×
141
142	if (tcgetattr(_fd, &tc) != 0) {	×
143	LogErr() << "tcgetattr failed: " << GET_ERROR();	×
144	close(_fd);	×
145	return ConnectionResult::ConnectionError;	×
146	}
147	#endif
148
149	#if defined(LINUX) \|\| defined(APPLE)
150	tc.c_iflag &= ~(IGNBRK \| BRKINT \| ICRNL \| INLCR \| PARMRK \| INPCK \| ISTRIP \| IXON);	×
151	tc.c_oflag &= ~(OCRNL \| ONLCR \| ONLRET \| ONOCR \| OFILL \| OPOST);	×
152	tc.c_lflag &= ~(ECHO \| ECHONL \| ICANON \| IEXTEN \| ISIG \| TOSTOP);	×
153	tc.c_cflag &= ~(CSIZE \| PARENB \| CRTSCTS);	×
154	tc.c_cflag \|= CS8;	×
155
156	tc.c_cc[VMIN] = 0; // We are ok with 0 bytes.	×
157	tc.c_cc[VTIME] = 10; // Timeout after 1 second.	×
158
159	if (_flow_control) {	×
160	tc.c_cflag \|= CRTSCTS;	×
161	}
162	#endif
163
164	#if defined(LINUX) \|\| defined(APPLE)
165	tc.c_cflag \|= CLOCAL; // Without this a write() blocks indefinitely.	×
166
167	#if defined(LINUX)
168	const int baudrate_or_define = define_from_baudrate(_baudrate);	×
169	#elif defined(APPLE)
170	const int baudrate_or_define = _baudrate;
171	#endif
172
173	if (baudrate_or_define == -1) {	×
174	return ConnectionResult::BaudrateUnknown;	×
175	}
176
177	if (cfsetispeed(&tc, baudrate_or_define) != 0) {	×
178	LogErr() << "cfsetispeed failed: " << GET_ERROR();	×
179	close(_fd);	×
180	return ConnectionResult::ConnectionError;	×
181	}
182
183	if (cfsetospeed(&tc, baudrate_or_define) != 0) {	×
184	LogErr() << "cfsetospeed failed: " << GET_ERROR();	×
185	close(_fd);	×
186	return ConnectionResult::ConnectionError;	×
187	}
188
189	if (tcsetattr(_fd, TCSANOW, &tc) != 0) {	×
190	LogErr() << "tcsetattr failed: " << GET_ERROR();	×
191	close(_fd);	×
192	return ConnectionResult::ConnectionError;	×
193	}
194	#endif
195
196	#if defined(WINDOWS)
197	DCB dcb;
198	SecureZeroMemory(&dcb, sizeof(DCB));
199	dcb.DCBlength = sizeof(DCB);
200
201	if (!GetCommState(_handle, &dcb)) {
202	LogErr() << "GetCommState failed with error: " << GET_ERROR();
203	return ConnectionResult::ConnectionError;
204	}
205
206	dcb.BaudRate = _baudrate;
207	dcb.ByteSize = 8;
208	dcb.Parity = NOPARITY;
209	dcb.StopBits = ONESTOPBIT;
210	if (_flow_control) {
211	dcb.fOutxCtsFlow = TRUE;
212	dcb.fDtrControl = DTR_CONTROL_HANDSHAKE;
213	dcb.fRtsControl = RTS_CONTROL_HANDSHAKE;
214	} else {
215	dcb.fDtrControl = DTR_CONTROL_DISABLE;
216	dcb.fRtsControl = RTS_CONTROL_DISABLE;
217	}
218	dcb.fOutX = FALSE;
219	dcb.fInX = FALSE;
220	dcb.fBinary = TRUE;
221	dcb.fNull = FALSE;
222	dcb.fDsrSensitivity = FALSE;
223
224	if (!SetCommState(_handle, &dcb)) {
225	LogErr() << "SetCommState failed with error: " << GET_ERROR();
226	return ConnectionResult::ConnectionError;
227	}
228
229	COMMTIMEOUTS timeout = {0, 0, 0, 0, 0};
230	timeout.ReadIntervalTimeout = 1;
231	timeout.ReadTotalTimeoutConstant = 1;
232	timeout.ReadTotalTimeoutMultiplier = 1;
233	timeout.WriteTotalTimeoutConstant = 1;
234	timeout.WriteTotalTimeoutMultiplier = 1;
235	SetCommTimeouts(_handle, &timeout);
236
237	if (!SetCommTimeouts(_handle, &timeout)) {
238	LogErr() << "SetCommTimeouts failed with error: " << GET_ERROR();
239	return ConnectionResult::ConnectionError;
240	}
241
242	#endif
243
244	return ConnectionResult::Success;	×
245	}
246
247	void SerialConnection::start_recv_thread()	×
248	{
249	_recv_thread = std::make_unique<std::thread>(&SerialConnection::receive, this);	×
250	}	×
251
252	ConnectionResult SerialConnection::stop()	×
253	{
254	_should_exit = true;	×
255
256	if (_recv_thread) {	×
257	_recv_thread->join();	×
258	_recv_thread.reset();	×
259	}
260
261	#if defined(LINUX) \|\| defined(APPLE)
262	close(_fd);	×
263	#elif defined(WINDOWS)
264	CloseHandle(_handle);
265	#endif
266
267	// We need to stop this after stopping the receive thread, otherwise
268	// it can happen that we interfere with the parsing of a message.
269	stop_mavlink_receiver();	×
270
271	return ConnectionResult::Success;	×
272	}
273
274	std::pair<bool, std::string> SerialConnection::send_message(const mavlink_message_t& message)	×
275	{
276	// Convert message to raw bytes and use common send path
277	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
278	uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message);	×
279
280	return send_raw_bytes(reinterpret_cast<const char*>(buffer), buffer_len);	×
281	}
282
283	void SerialConnection::receive()	×
284	{
285	// Enough for MTU 1500 bytes.
286	char buffer[2048];
287
288	#if defined(LINUX) \|\| defined(APPLE)
289	struct pollfd fds[1];
290	fds[0].fd = _fd;	×
291	fds[0].events = POLLIN;	×
292	#endif
293
294	while (!_should_exit) {	×
295	int recv_len;
296	#if defined(LINUX) \|\| defined(APPLE)
297	int pollrc = poll(fds, 1, 1000);	×
298	if (pollrc == 0 \|\| !(fds[0].revents & POLLIN)) {	×
299	continue;	×
300	} else if (pollrc == -1) {	×
301	LogErr() << "read poll failure: " << GET_ERROR();	×
302	}
303	// We enter here if (fds[0].revents & POLLIN) == true
304	recv_len = static_cast<int>(read(_fd, buffer, sizeof(buffer)));	×
305	if (recv_len < -1) {	×
306	LogErr() << "read failure: " << GET_ERROR();	×
307	}
308	#else
309	if (!ReadFile(_handle, buffer, sizeof(buffer), LPDWORD(&recv_len), NULL)) {
310	LogErr() << "ReadFile failure: " << GET_ERROR();
311	continue;
312	}
313	#endif
314	if (recv_len > static_cast<int>(sizeof(buffer)) \|\| recv_len == 0) {	×
315	continue;	×
316	}
317	_mavlink_receiver->set_new_datagram(buffer, recv_len);	×
318	// Parse all mavlink messages in one data packet. Once exhausted, we'll exit loop.
NEW 319	auto parse_result = _mavlink_receiver->parse_message();	×
NEW 320	while (parse_result != MavlinkReceiver::ParseResult::NoneAvailable) {	×
NEW 321	receive_message(parse_result, _mavlink_receiver->get_last_message(), this);	×
NEW 322	parse_result = _mavlink_receiver->parse_message();	×
323	}
324
325	// Also parse with libmav if available
326	if (_libmav_receiver) {	×
327	_libmav_receiver->set_new_datagram(buffer, recv_len);	×
328
329	while (_libmav_receiver->parse_message()) {	×
330	receive_libmav_message(_libmav_receiver->get_last_message(), this);	×
331	}
332	}
333	}
334	}	×
335
336	#if defined(LINUX)
337	int SerialConnection::define_from_baudrate(int baudrate)	×
338	{
339	switch (baudrate) {	×
340	case 9600:	×
341	return B9600;	×
342	case 19200:	×
343	return B19200;	×
344	case 38400:	×
345	return B38400;	×
346	case 57600:	×
347	return B57600;	×
348	case 115200:	×
349	return B115200;	×
350	case 230400:	×
351	return B230400;	×
352	case 460800:	×
353	return B460800;	×
354	case 500000:	×
355	return B500000;	×
356	case 576000:	×
357	return B576000;	×
358	case 921600:	×
359	return B921600;	×
360	case 1000000:	×
361	return B1000000;	×
362	case 1152000:	×
363	return B1152000;	×
364	case 1500000:	×
365	return B1500000;	×
366	case 2000000:	×
367	return B2000000;	×
368	case 2500000:	×
369	return B2500000;	×
370	case 3000000:	×
371	return B3000000;	×
372	case 3500000:	×
373	return B3500000;	×
374	case 4000000:	×
375	return B4000000;	×
376	default: {	×
377	LogErr() << "Unknown baudrate";	×
378	return -1;	×
379	}
380	}
381	}
382	#endif
383
384	std::pair<bool, std::string> SerialConnection::send_raw_bytes(const char* bytes, size_t length)	×
385	{
386	std::pair<bool, std::string> result;	×
387
388	if (_serial_node.empty()) {	×
389	LogErr() << "Dev Path unknown";	×
390	result.first = false;	×
391	result.second = "Dev Path unknown";	×
392	return result;	×
393	}
394
395	if (_baudrate == 0) {	×
396	result.first = false;	×
397	result.second = "Baudrate unknown";	×
398	return result;	×
399	}
400
401	int send_len;
402	#if defined(LINUX) \|\| defined(APPLE)
403	send_len = static_cast<int>(write(_fd, bytes, length));	×
404	#else
405	if (!WriteFile(_handle, bytes, static_cast<DWORD>(length), LPDWORD(&send_len), NULL)) {
406	std::stringstream ss;
407	ss << "WriteFile failure: " << GET_ERROR();
408	LogErr() << ss.str();
409	result.first = false;
410	result.second = ss.str();
411	return result;
412	}
413	#endif
414
415	if (send_len != static_cast<int>(length)) {	×
416	std::stringstream ss;	×
417	ss << "write failure: " << GET_ERROR();	×
418	LogErr() << ss.str();	×
419	result.first = false;	×
420	result.second = ss.str();	×
421	return result;	×
422	}	×
423
424	result.first = true;	×
425	return result;	×
426	}
427
428	} // namespace mavsdk

mavlink / MAVSDK / 20150089450

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