INSTINCT Code Coverage Report

Line	Branch	Exec	Source
1			/****************************************************************************
2			*
3			* Copyright (c) 2014 MAVlink Development Team. All rights reserved.
4			* Author: Trent Lukaczyk, <aerialhedgehog@gmail.com>
5			* Jaycee Lock, <jaycee.lock@gmail.com>
6			* Lorenz Meier, <lm@inf.ethz.ch>
7			*
8			* Redistribution and use in source and binary forms, with or without
9			* modification, are permitted provided that the following conditions
10			* are met:
11			*
12			* 1. Redistributions of source code must retain the above copyright
13			* notice, this list of conditions and the following disclaimer.
14			* 2. Redistributions in binary form must reproduce the above copyright
15			* notice, this list of conditions and the following disclaimer in
16			* the documentation and/or other materials provided with the
17			* distribution.
18			* 3. Neither the name PX4 nor the names of its contributors may be
19			* used to endorse or promote products derived from this software
20			* without specific prior written permission.
21			*
22			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
29			* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
30			* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33			* POSSIBILITY OF SUCH DAMAGE.
34			*
35			****************************************************************************/
36
37			/**
38			* @file serial_port.cpp
39			*
40			* @brief Serial interface functions
41			*
42			* Functions for opening, closing, reading and writing via serial ports
43			*
44			* @author Trent Lukaczyk, <aerialhedgehog@gmail.com>
45			* @author Jaycee Lock, <jaycee.lock@gmail.com>
46			* @author Lorenz Meier, <lm@inf.ethz.ch>
47			*
48			*/
49
50			// ------------------------------------------------------------------------------
51			// Includes
52			// ------------------------------------------------------------------------------
53
54			#if __linux__ || __APPLE__
55			#include "serial_port.hpp"
56			#include "util/Logger.hpp"
57
58			// ----------------------------------------------------------------------------------
59			// Serial Port Manager Class
60			// ----------------------------------------------------------------------------------
61
62			// ------------------------------------------------------------------------------
63			// Con/De structors
64			// ------------------------------------------------------------------------------
65		✗	Serial_Port::Serial_Port(const char* uart_name_, int baudrate_) : fd(-1), lastStatus(), lock(), debug(false), uart_name(uart_name_), baudrate(baudrate_), is_open(false)
66			{
67		✗	initialize_defaults();
68		✗	}
69
70		✗	Serial_Port::Serial_Port() : fd(-1), lastStatus(), lock(), debug(false), uart_name("/dev/ttyUSB0"), baudrate(57600), is_open(false)
71			{
72		✗	initialize_defaults();
73		✗	}
74
75		✗	Serial_Port::~Serial_Port()
76			{
77			// destroy mutex
78		✗	pthread_mutex_destroy(&lock);
79		✗	}
80
81		✗	void Serial_Port::initialize_defaults()
82			{
83			// Initialize attributes
84		✗	debug = false;
85		✗	fd = -1;
86		✗	is_open = false;
87
88		✗	uart_name = "/dev/ttyUSB0";
89		✗	baudrate = 57600;
90
91			// Start mutex
92		✗	int result = pthread_mutex_init(&lock, nullptr);
93		✗	if (result != 0)
94			{
95		✗	LOG_ERROR("serial_port.cpp - mutex init failed\n");
96			}
97		✗	}
98
99			// ------------------------------------------------------------------------------
100			// Read from Serial
101			// ------------------------------------------------------------------------------
102		✗	int Serial_Port::read_message(mavlink_message_t& message)
103			{
104		✗	uint8_t cp{};
105			mavlink_status_t status;
106		✗	uint8_t msgReceived = false;
107
108			// --------------------------------------------------------------------------
109			// READ FROM PORT
110			// --------------------------------------------------------------------------
111
112			// this function locks the port during read
113		✗	int result = _read_port(cp);
114
115			// --------------------------------------------------------------------------
116			// PARSE MESSAGE
117			// --------------------------------------------------------------------------
118		✗	if (result > 0)
119			{
120			// the parsing
121		✗	msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
122
123			// check for dropped packets
124		✗	if ((lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug)
125			{
126		✗	LOG_ERROR("serial_port.cpp - DROPPED {} PACKETS\n", status.packet_rx_drop_count);
127		✗	[[maybe_unused]] unsigned char v = cp;
128		✗	LOG_ERROR("serial_port.cpp - {}", v);
129			}
130		✗	lastStatus = status;
131			}
132
133			// Couldn't read from port
134			else
135			{
136		✗	cabelCheck = true;
137		✗	stop();
138		✗	LOG_ERROR("serial_port.cpp - Could not read from fd {}\n", fd);
139			}
140
141			// --------------------------------------------------------------------------
142			// DEBUGGING REPORTS
143			// --------------------------------------------------------------------------
144		✗	if (msgReceived && debug)
145			{
146			// Report info
147			// LOG_INFO("serial_port.cpp - Received message from serial with ID #{} (sys:{}|comp:{}):\n", message.msgid, message.sysid, message.compid);
148
149		✗	LOG_ERROR("serial_port.cpp - Received serial data");
150		✗	std::array<uint8_t, MAVLINK_MAX_PACKET_LEN> buffer{};
151
152			// check message is write length
153		✗	auto messageLength = mavlink_msg_to_send_buffer(buffer.data(), &message);
154
155			// message length error
156		✗	if (messageLength > MAVLINK_MAX_PACKET_LEN)
157			{
158		✗	LOG_ERROR("serial_port.cpp - MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
159			}
160
161			// print out the buffer
162			else
163			{
164		✗	for (size_t i = 0; i < messageLength; i++)
165			{
166		✗	[[maybe_unused]] auto v = buffer.at(i);
167		✗	LOG_ERROR("serial_port.cpp - {}", v);
168			}
169			}
170			}
171
172			// Done!
173		✗	return msgReceived;
174			}
175
176			// ------------------------------------------------------------------------------
177			// Write to Serial
178			// ------------------------------------------------------------------------------
179		✗	int Serial_Port::write_message(const mavlink_message_t& message)
180			{
181		✗	std::array<char, 300> buf{};
182
183			// Translate message to buffer
184		✗	auto len = mavlink_msg_to_send_buffer(reinterpret_cast<uint8_t*>(buf.data()), &message);
185
186			// Write buffer to serial port, locks port while writing
187		✗	auto bytesWritten = _write_port(buf.data(), len);
188
189		✗	return bytesWritten;
190			}
191
192			// ------------------------------------------------------------------------------
193			// Open Serial Port
194			// ------------------------------------------------------------------------------
195			/**
196			* throws EXIT_FAILURE if could not open the port
197			*/
198		✗	void Serial_Port::start()
199			{
200			// --------------------------------------------------------------------------
201			// OPEN PORT
202			// --------------------------------------------------------------------------
203		✗	LOG_INFO("serial_port.cpp - OPEN PORT\n");
204
205		✗	fd = _open_port(uart_name);
206
207			// Check success
208		✗	if (fd == -1)
209			{
210		✗	LOG_ERROR("serial_port.cpp - failure, could not open port.\n");
211		✗	throw EXIT_FAILURE;
212			}
213
214			// --------------------------------------------------------------------------
215			// SETUP PORT
216			// --------------------------------------------------------------------------
217		✗	auto success = _setup_port(baudrate, 8, 1, false, false);
218
219			// --------------------------------------------------------------------------
220			// CHECK STATUS
221			// --------------------------------------------------------------------------
222		✗	if (!success)
223			{
224		✗	LOG_ERROR("serial_port.cpp - failure, could not configure port.\n");
225		✗	throw EXIT_FAILURE;
226			}
227		✗	if (fd <= 0)
228			{
229		✗	LOG_ERROR("serial_port.cpp - Connection attempt to port {} with {} baud, 8N1 failed, exiting.\n", uart_name, baudrate);
230		✗	throw EXIT_FAILURE;
231			}
232
233			// --------------------------------------------------------------------------
234			// CONNECTED!
235			// --------------------------------------------------------------------------
236		✗	LOG_INFO("serial_port.cpp - Connected to {} with {} baud, 8 data bits, no parity, 1 stop bit (8N1)\n", uart_name, baudrate);
237		✗	lastStatus.packet_rx_drop_count = 0;
238
239		✗	is_open = true;
240		✗	}
241
242			// ------------------------------------------------------------------------------
243			// Close Serial Port
244			// ------------------------------------------------------------------------------
245		✗	void Serial_Port::stop()
246			{
247		✗	LOG_INFO("serial_port.cpp - CLOSE PORT\n");
248
249		✗	int result = close(fd);
250
251		✗	if (result)
252			{
253		✗	LOG_WARN("serial_port.cpp - Error on port close ({})\n", result);
254			}
255
256		✗	is_open = false;
257		✗	}
258
259			// ------------------------------------------------------------------------------
260			// Helper Function - Open Serial Port File Descriptor
261			// ------------------------------------------------------------------------------
262			// Where the actual port opening happens, returns file descriptor 'fd'
263		✗	int Serial_Port::_open_port(const char* port)
264			{
265			// Open serial port
266			// O_RDWR - Read and write
267			// O_NOCTTY - Ignore special chars like CTRL-C
268		✗	fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC);
269
270			// Check for Errors
271		✗	if (fd == -1)
272			{
273		✗	LOG_ERROR("serial_port.cpp - Could not open the port.");
274			}
275			// Finalize
276			else
277			{
278		✗	fcntl(fd, F_SETFL, 0);
279			}
280
281			// Done!
282		✗	return fd;
283			}
284
285			// ------------------------------------------------------------------------------
286			// Helper Function - Setup Serial Port
287			// ------------------------------------------------------------------------------
288			// Sets configuration, flags, and baud rate
289		✗	bool Serial_Port::_setup_port(int baud, int /* data_bits */, int /* stop_bits */, bool /* parity */, bool /* hardware_control */) const
290			{
291			// Check file descriptor
292		✗	if (!isatty(fd))
293			{
294		✗	LOG_ERROR("serial_port.cpp - file descriptor {} is NOT a serial port\n", fd);
295		✗	return false;
296			}
297
298			// Read file descritor configuration
299		✗	termios config{};
300
301		✗	if (tcgetattr(fd, &config) < 0)
302			{
303		✗	LOG_ERROR("serial_port.cpp - could not read configuration of fd {}\n", fd);
304		✗	return false;
305			}
306
307			// Input flags - Turn off input processing
308			// convert break to null byte, no CR to NL translation,
309			// no NL to CR translation, don't mark parity errors or breaks
310			// no input parity check, don't strip high bit off,
311			// no XON/XOFF software flow control
312		✗	config.c_iflag &= ~static_cast<tcflag_t>(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
313
314			// Output flags - Turn off output processing
315			// no CR to NL translation, no NL to CR-NL translation,
316			// no NL to CR translation, no column 0 CR suppression,
317			// no Ctrl-D suppression, no fill characters, no case mapping,
318			// no local output processing
319		✗	config.c_oflag &= ~static_cast<tcflag_t>(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
320
321			#ifdef OLCUC
322		✗	config.c_oflag &= ~static_cast<tcflag_t>(OLCUC);
323			#endif
324
325			#ifdef ONOEOT
326			config.c_oflag &= ~static_cast<tcflag_t>(ONOEOT);
327			#endif
328
329			// No line processing:
330			// echo off, echo newline off, canonical mode off,
331			// extended input processing off, signal chars off
332		✗	config.c_lflag &= ~static_cast<tcflag_t>(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
333
334			// Turn off character processing
335			// clear current char size mask, no parity checking,
336			// no output processing, force 8 bit input
337		✗	config.c_cflag &= ~static_cast<tcflag_t>(CSIZE | PARENB);
338		✗	config.c_cflag |= CS8;
339
340			// One input byte is enough to return from read()
341			// Inter-character timer off
342		✗	config.c_cc[VMIN] = 1;
343		✗	config.c_cc[VTIME] = 10; // was 0
344
345			// Get the current options for the port
346			////struct termios options;
347			////tcgetattr(fd, &options);
348
349			// Apply baudrate
350		✗	switch (baud)
351			{
352		✗	case 1200:
353		✗	if (cfsetispeed(&config, B1200) < 0 || cfsetospeed(&config, B1200) < 0)
354			{
355		✗	LOG_ERROR("serial_port.cpp - Could not set desired baud rate of {} Baud\n", baud);
356		✗	return false;
357			}
358		✗	break;
359		✗	case 1800:
360		✗	cfsetispeed(&config, B1800);
361		✗	cfsetospeed(&config, B1800);
362		✗	break;
363		✗	case 9600:
364		✗	cfsetispeed(&config, B9600);
365		✗	cfsetospeed(&config, B9600);
366		✗	break;
367		✗	case 19200:
368		✗	cfsetispeed(&config, B19200);
369		✗	cfsetospeed(&config, B19200);
370		✗	break;
371		✗	case 38400:
372		✗	if (cfsetispeed(&config, B38400) < 0 || cfsetospeed(&config, B38400) < 0)
373			{
374		✗	LOG_ERROR("serial_port.cpp - Could not set desired baud rate of {} Baud\n", baud);
375		✗	return false;
376			}
377		✗	break;
378		✗	case 57600:
379		✗	if (cfsetispeed(&config, B57600) < 0 || cfsetospeed(&config, B57600) < 0)
380			{
381		✗	LOG_ERROR("serial_port.cpp - Could not set desired baud rate of {} Baud\n", baud);
382		✗	return false;
383			}
384		✗	break;
385		✗	case 115200:
386		✗	if (cfsetispeed(&config, B115200) < 0 || cfsetospeed(&config, B115200) < 0)
387			{
388		✗	LOG_ERROR("serial_port.cpp - Could not set desired baud rate of {} Baud\n", baud);
389		✗	return false;
390			}
391		✗	break;
392
393			// These two non-standard (by the 70'ties ) rates are fully supported on
394			// current Debian and Mac OS versions (tested since 2010).
395		✗	case 460800:
396		✗	if (cfsetispeed(&config, B460800) < 0 || cfsetospeed(&config, B460800) < 0)
397			{
398		✗	LOG_ERROR("serial_port.cpp - Could not set desired baud rate of {} Baud\n", baud);
399		✗	return false;
400			}
401		✗	break;
402		✗	case 921600:
403		✗	if (cfsetispeed(&config, B921600) < 0 || cfsetospeed(&config, B921600) < 0)
404			{
405		✗	LOG_ERROR("serial_port.cpp - Could not set desired baud rate of {} Baud\n", baud);
406		✗	return false;
407			}
408		✗	break;
409			#ifndef __APPLE__
410		✗	case 1500000:
411		✗	if (cfsetispeed(&config, B1500000) < 0 || cfsetospeed(&config, B1500000) < 0)
412			{
413		✗	LOG_ERROR("serial_port.cpp - Could not set desired baud rate of {} Baud\n", baud);
414		✗	return false;
415			}
416		✗	break;
417			#endif
418		✗	default:
419		✗	LOG_ERROR("serial_port.cpp - Desired baud rate {} could not be set, aborting.\n", baud);
420		✗	return false;
421
422			break;
423			}
424
425			// Finally, apply the configuration
426		✗	if (tcsetattr(fd, TCSAFLUSH, &config) < 0)
427			{
428		✗	LOG_ERROR("serial_port.cpp - could not set configuration of fd {}\n", fd);
429		✗	return false;
430			}
431
432			// Done!
433		✗	return true;
434			}
435
436			// ------------------------------------------------------------------------------
437			// Read Port with Lock
438			// ------------------------------------------------------------------------------
439		✗	int Serial_Port::_read_port(uint8_t& cp)
440			{
441			// Lock
442		✗	pthread_mutex_lock(&lock);
443
444		✗	auto result = static_cast<int8_t>(read(fd, &cp, 1)); // NOLINT(bugprone-signed-char-misuse,cert-str34-c,clang-analyzer-unix.BlockInCriticalSection)
445
446			// Unlock
447		✗	pthread_mutex_unlock(&lock);
448
449		✗	return result;
450			}
451
452			// ------------------------------------------------------------------------------
453			// Write Port with Lock
454			// ------------------------------------------------------------------------------
455		✗	int Serial_Port::_write_port(char* buf, unsigned len)
456			{
457			// Lock
458		✗	pthread_mutex_lock(&lock);
459
460			// Write packet via serial link
461		✗	const int bytesWritten = static_cast<int>(write(fd, buf, len));
462
463			// Wait until all data has been written
464		✗	tcdrain(fd);
465
466			// Unlock
467		✗	pthread_mutex_unlock(&lock);
468
469		✗	return bytesWritten;
470			}
471
472			#endif
473