INSTINCT Code Coverage Report

Line	Branch	Exec	Source
1			/****************************************************************************
2			*
3			* Copyright (c) 2018 MAVlink Development Team. All rights reserved.
4			* Author: Hannes Diethelm, <hannes.diethelm@gmail.com>
5			* Trent Lukaczyk, <aerialhedgehog@gmail.com>
6			* Jaycee Lock, <jaycee.lock@gmail.com>
7			* Lorenz Meier, <lm@inf.ethz.ch>
8			*
9			* Redistribution and use in source and binary forms, with or without
10			* modification, are permitted provided that the following conditions
11			* are met:
12			*
13			* 1. Redistributions of source code must retain the above copyright
14			* notice, this list of conditions and the following disclaimer.
15			* 2. Redistributions in binary form must reproduce the above copyright
16			* notice, this list of conditions and the following disclaimer in
17			* the documentation and/or other materials provided with the
18			* distribution.
19			* 3. Neither the name PX4 nor the names of its contributors may be
20			* used to endorse or promote products derived from this software
21			* without specific prior written permission.
22			*
23			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
30			* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
31			* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34			* POSSIBILITY OF SUCH DAMAGE.
35			*
36			****************************************************************************/
37
38			/**
39			* @file udp_port.cpp
40			*
41			* @brief UDP interface functions
42			*
43			* Functions for opening, closing, reading and writing via UDP ports
44			*
45			* @author Hannes Diethelm, <hannes.diethelm@gmail.com>
46			* @author Trent Lukaczyk, <aerialhedgehog@gmail.com>
47			* @author Jaycee Lock, <jaycee.lock@gmail.com>
48			* @author Lorenz Meier, <lm@inf.ethz.ch>
49			*
50			*/
51
52			// ------------------------------------------------------------------------------
53			// Includes
54			// ------------------------------------------------------------------------------
55
56			#if __linux__ || __APPLE__
57
58			#include "udp_port.hpp"
59			#include "util/Logger.hpp"
60
61			// ----------------------------------------------------------------------------------
62			// UDP Port Manager Class
63			// ----------------------------------------------------------------------------------
64
65			// ------------------------------------------------------------------------------
66			// Con/De structors
67			// ------------------------------------------------------------------------------
68		✗	UDP_Port::UDP_Port(const char* target_ip_, int udp_port_) : lastStatus(), lock(), buff(), target_ip(target_ip_), rx_port(udp_port_)
69			{
70		✗	initialize_defaults();
71		✗	}
72
73		✗	UDP_Port::UDP_Port()
74			{
75		✗	initialize_defaults();
76		✗	}
77
78		✗	UDP_Port::~UDP_Port()
79			{
80			// destroy mutex
81		✗	pthread_mutex_destroy(&lock);
82		✗	}
83
84		✗	void UDP_Port::initialize_defaults()
85			{
86			// Start mutex
87		✗	int result = pthread_mutex_init(&lock, nullptr);
88		✗	if (result != 0)
89			{
90		✗	LOG_DEBUG("Mavlink - udp_port.cpp: mutex init failed");
91			}
92		✗	}
93
94			// ------------------------------------------------------------------------------
95			// Read from UDP
96			// ------------------------------------------------------------------------------
97		✗	int UDP_Port::read_message(mavlink_message_t& message)
98			{
99		✗	uint8_t cp{};
100			mavlink_status_t status;
101		✗	uint8_t msgReceived = false;
102
103			// --------------------------------------------------------------------------
104			// READ FROM PORT
105			// --------------------------------------------------------------------------
106
107			// this function locks the port during read
108		✗	int result = _read_port(cp);
109
110			// --------------------------------------------------------------------------
111			// PARSE MESSAGE
112			// --------------------------------------------------------------------------
113		✗	if (result > 0)
114			{
115			// the parsing
116		✗	msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
117
118			// check for dropped packets
119		✗	if ((lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug)
120			{
121			// printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
122		✗	LOG_ERROR("Mavlink - udp_port.cpp: DROPPED {} PACKETS\n", status.packet_rx_drop_count);
123			// unsigned char v = cp;
124			// fprintf(stderr, "%02x ", v);
125			}
126		✗	lastStatus = status;
127			}
128
129			// Couldn't read from port
130			else
131			{
132			// fprintf(stderr, "ERROR: Could not read, res = %d, errno = %d : %m\n", result, errno);
133		✗	LOG_ERROR("Mavlink - udp_port.cpp: Could not read, res = {}, errno = {} \n", result, errno);
134			}
135
136			// --------------------------------------------------------------------------
137			// DEBUGGING REPORTS
138			// --------------------------------------------------------------------------
139		✗	if (msgReceived && debug)
140			{
141			// Report info
142			// printf("Received message from UDP with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
143			// LOG_INFO("Mavlink - udp_port.cpp: Received message from UDP with ID #{} (sys:{}|comp:{}):\n", message.msgid, message.sysid, message.compid); // FIXME: cannot bind bit-field 'message.__mavlink_message::msgid' to 'unsigned int&'
144
145		✗	LOG_INFO("Mavlink - udp_port.cpp: Received UDP data: ");
146		✗	std::array<uint8_t, MAVLINK_MAX_PACKET_LEN> buffer{};
147
148			// check message is write length
149		✗	auto messageLength = mavlink_msg_to_send_buffer(buffer.data(), &message);
150
151			// message length error
152		✗	if (messageLength > MAVLINK_MAX_PACKET_LEN)
153			{
154			// fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
155		✗	LOG_ERROR("Mavlink - udp_port.cpp: FATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE");
156			}
157
158			// print out the buffer
159			else
160			{
161		✗	for (size_t i = 0; i < static_cast<size_t>(messageLength); i++)
162			{
163		✗	[[maybe_unused]] unsigned char v = buffer.at(i);
164			// fprintf(stderr, "%02x ", v);
165		✗	LOG_INFO("Mavlink - udp_port.cpp: {}", v);
166			}
167			}
168			}
169
170			// Done!
171		✗	return msgReceived;
172			}
173
174			// ------------------------------------------------------------------------------
175			// Write to UDP
176			// ------------------------------------------------------------------------------
177		✗	int UDP_Port::write_message(const mavlink_message_t& message)
178			{
179		✗	std::array<char, 300> buf{};
180
181			// Translate message to buffer
182		✗	unsigned len = mavlink_msg_to_send_buffer(reinterpret_cast<uint8_t*>(buf.data()), &message);
183
184			// Write buffer to UDP port, locks port while writing
185		✗	int bytesWritten = _write_port(buf.data(), len);
186		✗	if (bytesWritten < 0)
187			{
188			// fprintf(stderr, "ERROR: Could not write, res = %d, errno = %d : %m\n", bytesWritten, errno);
189		✗	LOG_ERROR("Mavlink - udp_port.cpp: Could not write, res = {}, errno = {}\n", bytesWritten, errno);
190			}
191
192		✗	return bytesWritten;
193			}
194
195			// ------------------------------------------------------------------------------
196			// Open UDP Port
197			// ------------------------------------------------------------------------------
198			/**
199			* throws EXIT_FAILURE if could not open the port
200			*/
201		✗	void UDP_Port::start()
202			{
203			// --------------------------------------------------------------------------
204			// OPEN PORT
205			// --------------------------------------------------------------------------
206
207			/* Create socket */
208		✗	sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
209		✗	if (sock < 0)
210			{
211		✗	LOG_ERROR("Mavlink - udp_port.cpp: error socket failed");
212			}
213
214			/* Bind the socket to rx_port - necessary to receive packets */
215		✗	struct sockaddr_in addr
216			{};
217		✗	memset(&addr, 0, sizeof(addr));
218		✗	addr.sin_family = AF_INET;
219		✗	addr.sin_addr.s_addr = inet_addr(target_ip);
220			;
221		✗	addr.sin_port = htons(static_cast<uint16_t>(rx_port));
222
223		✗	if (bind(sock, reinterpret_cast<struct sockaddr*>(&addr), sizeof(struct sockaddr)))
224			{
225		✗	LOG_ERROR("Mavlink - udp_port.cpp: error bind failed");
226		✗	close(sock);
227		✗	sock = -1;
228			}
229			/*if (fcntl(sock, F_SETFL, O_NONBLOCK | O_ASYNC) < 0)
230			{
231			fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
232			close(sock);
233			sock = -1;
234			throw EXIT_FAILURE;
235			}*/
236
237			// --------------------------------------------------------------------------
238			// CONNECTED!
239			// --------------------------------------------------------------------------
240			// printf("Listening to %s:%i\n", target_ip, rx_port);
241		✗	LOG_INFO("Mavlink - udp_port.cpp: Listening to {}:{}\n", target_ip, rx_port);
242		✗	lastStatus.packet_rx_drop_count = 0;
243
244		✗	is_open = true;
245		✗	}
246
247			// ------------------------------------------------------------------------------
248			// Close UDP Port
249			// ------------------------------------------------------------------------------
250		✗	void UDP_Port::stop()
251			{
252			// printf("CLOSE PORT\n");
253		✗	LOG_INFO("Mavlink - udp_port.cpp: CLOSE PORT");
254
255		✗	int result = close(sock);
256		✗	sock = -1;
257
258		✗	if (result)
259			{
260			// fprintf(stderr, "WARNING: Error on port close (%i)\n", result);
261		✗	LOG_WARN("Mavlink - udp_port.cpp: Error on port close ({})\n", result);
262			}
263
264		✗	is_open = false;
265
266			// printf("\n");
267		✗	}
268
269			// ------------------------------------------------------------------------------
270			// Read Port with Lock
271			// ------------------------------------------------------------------------------
272		✗	int UDP_Port::_read_port(uint8_t& cp)
273			{
274		✗	socklen_t len{};
275
276			// Lock
277		✗	pthread_mutex_lock(&lock);
278
279		✗	int result = -1;
280		✗	if (buff_ptr < buff_len)
281			{
282		✗	cp = buff.at(static_cast<uint8_t>(buff_ptr));
283		✗	buff_ptr++;
284		✗	result = 1;
285			}
286			else
287			{
288		✗	struct sockaddr_in addr
289			{};
290		✗	len = sizeof(struct sockaddr_in);
291		✗	result = static_cast<int>(recvfrom(sock, &buff, BUFF_LEN, 0, reinterpret_cast<struct sockaddr*>(&addr), &len));
292		✗	if (tx_port < 0)
293			{
294		✗	if (strcmp(inet_ntoa(addr.sin_addr), target_ip) == 0) // NOLINT(concurrency-mt-unsafe)
295			{
296		✗	tx_port = ntohs(addr.sin_port);
297			// printf("Got first packet, sending to %s:%i\n", target_ip, rx_port);
298		✗	LOG_INFO("Mavlink - udp_port.cpp: Got first packet, sending to {}:{}\n", target_ip, rx_port);
299			}
300			else
301			{
302			// printf("ERROR: Got packet from %s:%i but listening on %s\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port), target_ip);
303		✗	LOG_ERROR("Mavlink - udp_port.cpp: Got packet from {}:{} but listening on {}\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port), target_ip); // NOLINT(concurrency-mt-unsafe)
304			}
305			}
306		✗	if (result > 0)
307			{
308		✗	buff_len = result;
309		✗	buff_ptr = 0;
310		✗	cp = buff.at(static_cast<uint8_t>(buff_ptr));
311		✗	buff_ptr++;
312			// printf("recvfrom: %i %i\n", result, cp);
313			}
314			}
315
316			// Unlock
317		✗	pthread_mutex_unlock(&lock);
318
319		✗	return result;
320			}
321
322			// ------------------------------------------------------------------------------
323			// Write Port with Lock
324			// ------------------------------------------------------------------------------
325		✗	int UDP_Port::_write_port(char* buf, unsigned len)
326			{
327			// Lock
328		✗	pthread_mutex_lock(&lock);
329
330			// Write packet via UDP link
331		✗	int bytesWritten = 0;
332		✗	if (tx_port > 0)
333			{
334		✗	struct sockaddr_in addr
335			{};
336		✗	memset(&addr, 0, sizeof(addr));
337		✗	addr.sin_family = AF_INET;
338		✗	addr.sin_addr.s_addr = inet_addr(target_ip);
339		✗	addr.sin_port = htons(static_cast<uint16_t>(tx_port));
340		✗	bytesWritten = static_cast<unsigned char>(sendto(sock, buf, len, 0, reinterpret_cast<struct sockaddr*>(&addr), sizeof(struct sockaddr_in))); // NOLINT(bugprone-signed-char-misuse)
341			// printf("sendto: %i\n", bytesWritten);
342			}
343			else
344			{
345		✗	LOG_ERROR("Mavlink - udp_port.cpp: Sending before first packet received!\n");
346		✗	bytesWritten = -1;
347			}
348
349			// Unlock
350		✗	pthread_mutex_unlock(&lock);
351
352		✗	return bytesWritten;
353			}
354
355			#endif
356