GCC Code Coverage Report

Directory: ./
File: src/usb_stream.cpp
Date: 2022-06-29 13:58:11
Exec Total Coverage
Lines: 1 246 0.4%
Branches: 2 129 1.6%
Line Branch Exec Source
1 #include "real_time_tools/usb_stream.hpp"
2 #include <algorithm>
3 #include <exception>
4 #include "real_time_tools/iostream.hpp"
5 #include "real_time_tools/timer.hpp"
6
7 #if defined(XENOMAI)
8 #elif defined(NON_REAL_TIME) || defined(RT_PREEMPT)
9 #include <errno.h> // Error number definitions
10 #include <fcntl.h> // File control definitions: open
11 #include <string.h> // parse errno message
12 #include <termios.h> // terminal io (serial port) interface
13 #include <unistd.h> // UNIX standard function definitions: write, read
14 #endif
15
16 namespace real_time_tools
17 {
18 ✗ UsbStream::UsbStream()
19 {
20 // important initialization
21 ✗ timeout_set_ = false;
22 ✗ buffer_.resize(100);
23
24 // some default value
25 ✗ file_name_ = "";
26 ✗ file_id_ = 0;
27 ✗ return_value_ = 0;
28 ✗ timeout_ = 0.0;
29
30 #if defined(XENOMAI)
31 #elif defined(RT_PREEMPT) || defined(NON_REAL_TIME)
32 // config_ : nothing to be done. Initilized upon port openning.
33 ✗ timeout_posix_.tv_sec = 0;
34 ✗ timeout_posix_.tv_nsec = 0;
35 ✗ FD_ZERO(&file_id_set_);
36 #endif
37 }
38
39 ✗ UsbStream::~UsbStream()
40 {
41 ✗ close_device();
42 }
43
44 ✗ bool UsbStream::open_device(const std::string& file_name)
45 {
46 ✗ file_name_ = file_name;
47 #if defined(XENOMAI)
48 // fd_ = rt_dev_open(port_, O_RDWR);
49 // if (fd_ < 0)
50 // {
51 // rt_printf("ERROR >> Failed to open real-time USB port %s. "
52 // "Are you sure you've loaded the correct drivers?\n", port_);
53 // return false;
54 // }
55 #elif defined(RT_PREEMPT) || defined(NON_REAL_TIME)
56 // http://man7.org/linux/man-pages/man2/open.2.html
57 // blocking mode by default, unless O_NONBLOCK is passed
58 // We open the device in read and write mode: O_RDWR.
59 // Whenever we write in the device, the function will be blockant until the
60 // device received the message.
61 ✗ file_id_ = open(file_name_.c_str(), O_RDWR | O_NOCTTY);
62 ✗ if (file_id_ < 0)
63 {
64 // here errno is a POSIX global variable containing the errors of the
65 // last POSIX function call.
66 ✗ int errsv = errno;
67 ✗ printf("ERROR >> Failed to open device port %s with error:\n \t%s\n",
68 file_name_.c_str(),
69 strerror(errsv));
70 ✗ return false;
71 }
72 #endif
73 ✗ return true;
74 }
75
76 ✗ bool UsbStream::set_port_config(const PortConfig& user_config)
77 {
78 ✗ flush();
79 #if defined(XENOMAI)
80 #elif defined(RT_PREEMPT) || defined(NON_REAL_TIME)
81 /**
82 * Update the port setting using teh POSIX api.
83 * https://linux.die.net/man/3/tcgetattr
84 */
85 // Get the current port settings
86 ✗ tcgetattr(file_id_, &config_);
87
88 /**
89 * set port control modes:
90 * CREAD: Enable receiver.
91 * CLOCAL: Ignore modem control lines.
92 */
93 ✗ if (user_config.rts_cts_enabled_)
94 {
95 ✗ config_.c_cflag |= (CRTSCTS | CREAD);
96 }
97 else
98 {
99 ✗ config_.c_cflag |= (CLOCAL | CREAD);
100 }
101
102 // set to 8N1 (eight data bits, no parity bit, one stop bit):
103 // here "~XXX" means "no XXXX"
104 ✗ if (user_config.parity_)
105 {
106 ✗ config_.c_cflag &= PARENB;
107 }
108 else
109 {
110 ✗ config_.c_cflag &= ~PARENB;
111 }
112
113 ✗ if (user_config.stop_bits_ == PortConfig::StopBits::one)
114 {
115 ✗ config_.c_cflag &= ~CSTOPB; // 1 stop bit
116 }
117 else
118 {
119 ✗ config_.c_cflag &= CSTOPB; // 2 stop bit
120 }
121
122 ✗ if (user_config.prepare_size_definition_)
123 {
124 ✗ config_.c_cflag &= CSIZE;
125 }
126 else
127 {
128 ✗ config_.c_cflag &= ~CSIZE;
129 }
130
131 ✗ switch (user_config.data_bits_)
132 {
133 ✗ case PortConfig::DataBits::cs7:
134 ✗ config_.c_cflag |= CS7; // define the size 7N1, or 7 data bits
135 ✗ break;
136 ✗ case PortConfig::DataBits::cs8:
137 ✗ config_.c_cflag |= CS8; // define the size 8N1, or 8 data bits
138 ✗ break;
139
140 ✗ default:
141 ✗ config_.c_cflag |= CS8;
142 ✗ break;
143 }
144
145 // set to baudrate 115200.
146 // https://www.setra.com/blog/what-is-baud-rate-and-what-cable-length-is-required-1
147 // Convert specified baud to hardware specific value
148 ✗ int hardware_bit_baud = 0;
149 ✗ switch (user_config.baude_rate_)
150 {
151 ✗ case 0:
152 ✗ hardware_bit_baud = B0;
153 ✗ break;
154 ✗ case 50:
155 ✗ hardware_bit_baud = B50;
156 ✗ break;
157 ✗ case 75:
158 ✗ hardware_bit_baud = B75;
159 ✗ break;
160 ✗ case 110:
161 ✗ hardware_bit_baud = B110;
162 ✗ break;
163 ✗ case 134:
164 ✗ hardware_bit_baud = B134;
165 ✗ break;
166 ✗ case 150:
167 ✗ hardware_bit_baud = B150;
168 ✗ break;
169 ✗ case 200:
170 ✗ hardware_bit_baud = B200;
171 ✗ break;
172 ✗ case 300:
173 ✗ hardware_bit_baud = B300;
174 ✗ break;
175 ✗ case 600:
176 ✗ hardware_bit_baud = B600;
177 ✗ break;
178 ✗ case 1200:
179 ✗ hardware_bit_baud = B1200;
180 ✗ break;
181 ✗ case 1800:
182 ✗ hardware_bit_baud = B1800;
183 ✗ break;
184 ✗ case 2400:
185 ✗ hardware_bit_baud = B2400;
186 ✗ break;
187 ✗ case 4800:
188 ✗ hardware_bit_baud = B4800;
189 ✗ break;
190 ✗ case 9600:
191 ✗ hardware_bit_baud = B9600;
192 ✗ break;
193 ✗ case 19200:
194 ✗ hardware_bit_baud = B19200;
195 ✗ break;
196 ✗ case 38400:
197 ✗ hardware_bit_baud = B38400;
198 ✗ break;
199 #ifdef B7200
200 case 7200:
201 hardware_bit_baud = B7200;
202 break;
203 #endif
204 #ifdef B14400
205 case 14400:
206 hardware_bit_baud = B14400;
207 break;
208 #endif
209 #ifdef B57600
210 ✗ case 57600:
211 ✗ hardware_bit_baud = B57600;
212 ✗ break;
213 #endif
214 #ifdef B115200
215 ✗ case 115200:
216 ✗ hardware_bit_baud = B115200;
217 ✗ break;
218 #endif
219 #ifdef B230400
220 ✗ case 230400:
221 ✗ hardware_bit_baud = B230400;
222 ✗ break;
223 #endif
224 #ifdef B460800
225 ✗ case 460800:
226 ✗ hardware_bit_baud = B460800;
227 ✗ break;
228 #endif
229 #ifdef B500000
230 ✗ case 500000:
231 ✗ hardware_bit_baud = B500000;
232 ✗ break;
233 #endif
234 #ifdef B576000
235 ✗ case 576000:
236 ✗ hardware_bit_baud = B576000;
237 ✗ break;
238 #endif
239 #ifdef B921600
240 ✗ case 921600:
241 ✗ hardware_bit_baud = B921600;
242 ✗ break;
243 #endif
244 #ifdef B1000000
245 ✗ case 1000000:
246 ✗ hardware_bit_baud = B1000000;
247 ✗ break;
248 #endif
249 #ifdef B1152000
250 ✗ case 1152000:
251 ✗ hardware_bit_baud = B1152000;
252 ✗ break;
253 #endif
254 #ifdef B2000000
255 ✗ case 2000000:
256 ✗ hardware_bit_baud = B2000000;
257 ✗ break;
258 #endif
259 #ifdef B3000000
260 ✗ case 3000000:
261 ✗ hardware_bit_baud = B3000000;
262 ✗ break;
263 #endif
264 #ifdef B3500000
265 ✗ case 3500000:
266 ✗ hardware_bit_baud = B3500000;
267 ✗ break;
268 #endif
269 #ifdef B4000000
270 ✗ case 4000000:
271 ✗ hardware_bit_baud = B4000000;
272 ✗ break;
273 #endif
274 // Unsupported baud specified
275 ✗ default:
276 throw std::runtime_error(
277 "UsbStream::open_device : Baude rate not yet "
278 ✗ "supported, fix the code or correct baude rate");
279 break;
280 }
281
282 // set the baud rate
283 ✗ cfsetospeed(&config_, hardware_bit_baud);
284 ✗ cfsetispeed(&config_, hardware_bit_baud);
285
286 // from the imu drivers...
287 // set for non-canonical (raw processing, no echo, etc.)
288 ✗ config_.c_iflag = IGNPAR; // ignore parity check close_port(int
289 ✗ config_.c_oflag = 0; // raw output
290 ✗ config_.c_lflag = 0; // raw input
291 // Time-Outs -- won't work with NDELAY option in the call to open
292 ✗ config_.c_cc[VMIN] = 0; // block reading until RX x characers. If x = 0,
293 // it is non-blocking.
294 ✗ config_.c_cc[VTIME] = 1; // Inter-Character Timer -- i.e. timeout= x*.1 s
295
296 ✗ if (!flush())
297 {
298 ✗ rt_printf(
299 "UsbStream::open_device : Flushing old serial buffer data "
300 "failed\n");
301 ✗ return false;
302 }
303
304 // set port properties after flushing buffer
305 ✗ if (tcsetattr(file_id_, TCSANOW, &config_) < 0)
306 {
307 ✗ rt_printf("UsbStream::open_device : Failed to configure port.\n");
308 ✗ return false;
309 }
310
311 ✗ if (!flush())
312 {
313 ✗ rt_printf(
314 "UsbStream::open_device : Flushing old serial buffer data "
315 "failed\n");
316 ✗ return false;
317 }
318 #endif
319 ✗ return true;
320 }
321
322 ✗ bool UsbStream::close_device()
323 {
324 #if defined(XENOMAI)
325 return_value_ = rt_dev_close(file_id_);
326 if (return_value_ != 0)
327 {
328 rt_printf("ERROR >> Failed to close port.\n");
329 return false;
330 }
331 #elif defined(RT_PREEMPT) || defined(NON_REAL_TIME)
332 ✗ if (file_id_ != 0)
333 {
334 ✗ return_value_ = close(file_id_);
335 ✗ if (return_value_ != 0)
336 {
337 ✗ int errsv = errno;
338 ✗ printf(
339 "ERROR >> Failed to close port %s with error:\n"
340 "\t%s\n",
341 file_name_.c_str(),
342 strerror(errsv));
343 ✗ return false;
344 }
345 ✗ file_id_ = 0;
346 }
347 #endif
348 ✗ return true;
349 }
350
351 ✗ bool UsbStream::read_device(std::vector<uint8_t>& msg, const bool stream_on)
352 {
353 // read device and then store number of bytes read
354 ✗ return_value_ = UsbStream::read_device_raw(msg, stream_on);
355 /**
356 * Check the potential error:
357 *
358 * - First we check if the port could be read at all.
359 * - Then we check if the port was read before the timeout
360 */
361
362 // Port reading failure
363 ✗ if (return_value_ < 0)
364 {
365 ✗ int errsv = errno;
366 ✗ rt_printf(
367 "UsbStream::read_device: "
368 "Failed to read port %s with error\n\t%s\n",
369 file_name_.c_str(),
370 strerror(errsv));
371 ✗ return false;
372 }
373 // Timeout failure
374 ✗ else if (return_value_ != static_cast<ssize_t>(msg.size()))
375 {
376 ✗ rt_printf(
377 "UsbStream::read_device: "
378 "Failed to read port %s. Requested %ld bytes and "
379 "received %ld bytes: %s\n",
380 file_name_.c_str(),
381 msg.size(),
382 return_value_,
383 ✗ msg_debug_string(buffer_, return_value_).c_str());
384 ✗ return false;
385 }
386 // Here we copy the message inside the buffer in order to use a bigger
387 // memory buffer than the message itself
388 ✗ std::copy_n(buffer_.begin(), msg.size(), msg.begin());
389 ✗ return true;
390 }
391
392 ✗ ssize_t UsbStream::read_device_raw(std::vector<uint8_t>& msg, const bool stream_on, const size_t start_location)
393 {
394 // We make sure that the internal buffer is big enough, while avoiding too
395 // many resizes. Theoretically the default size is good enough.
396 ✗ if (msg.size() - start_location > buffer_.size())
397 {
398 ✗ rt_printf(
399 "UsbStream::read_device: Warning internal buffer needs resizing,"
400 "This operation is not real-time safe");
401 ✗ buffer_.resize(10 * msg.size());
402 }
403 // inefficient but safer
404 ✗ std::fill(buffer_.begin(), buffer_.end(), 0);
405
406 #if defined(XENOMAI)
407 return_value_ = rt_dev_read(file_id_, buffer_.data(), msg.size());
408 #elif defined(RT_PREEMPT) || defined(NON_REAL_TIME)
409 /**
410 * Poll Mode
411 */
412 ✗ if (!stream_on)
413 {
414 ✗ if (!timeout_set_)
415 {
416 throw std::runtime_error(
417 "UsbStream::read_device : Poll mode requested "
418 "but no timeout set. Please use "
419 ✗ "UsbStream::set_poll_mode_timeout");
420 }
421 // here we acquire the port access.
422 ✗ return_value_ = pselect(file_id_ + 1,
423 &file_id_set_, // writefds
424 nullptr, // readfds
425 nullptr, // exceptfds
426 ✗ &timeout_posix_, // timeout
427 nullptr); // sigmask
428
429 // an error occured during the resource access
430 ✗ if (return_value_ == -1)
431 {
432 ✗ int errsv = errno;
433 ✗ rt_printf(
434 "UsbStream::read_device: "
435 "Failed to access port %s with error\n\t%s\n",
436 file_name_.c_str(),
437 strerror(errsv));
438 ✗ return -1; // return_value_ == -1
439 }
440 // the timeout has expired
441 ✗ else if (return_value_ == 0)
442 {
443 ✗ int errsv = errno;
444 ✗ rt_printf(
445 "UsbStream::read_device: "
446 "Failed to access port %s before timeout with "
447 "error\n\t%s\n",
448 file_name_.c_str(),
449 strerror(errsv));
450 ✗ return 0; // return_value == 0
451 }
452 // Nothing wrong happened: access the data.
453 else
454 {
455 ✗ return_value_ = read(file_id_, buffer_.data(), msg.size() - start_location);
456 }
457 }
458 /**
459 * Stream Mode
460 */
461 else
462 {
463 ✗ return_value_ = read(file_id_, buffer_.data(), msg.size() - start_location);
464 }
465 #endif
466 ✗ std::copy_n(buffer_.begin(), msg.size() - start_location, msg.begin() + start_location);
467 ✗ return return_value_;
468 }
469
470
471 ✗ bool UsbStream::write_device(const std::vector<uint8_t>& msg)
472 {
473 ✗ if (msg.size() > buffer_.size())
474 {
475 ✗ rt_printf(
476 "UsbStream::write_device: Warning internal buffer needs resizing,"
477 "This operation is not real-time safe");
478 ✗ buffer_.resize(10 * msg.size());
479 }
480 // inefficient but safer
481 ✗ std::fill(buffer_.begin(), buffer_.end(), 0);
482
483 // Here we copy the message inside the buffer in order to use a bigger
484 // memory buffer than the message itself
485 ✗ std::copy(msg.begin(), msg.end(), buffer_.begin());
486
487 #if defined(XENOMAI)
488 return_value_ = rt_dev_write(file_id_, buffer_.data(), msg.size());
489 #elif defined(RT_PREEMPT) || defined(NON_REAL_TIME)
490 ✗ return_value_ = write(file_id_, buffer_.data(), msg.size());
491 #endif
492
493 ✗ if (return_value_ < 0)
494 {
495 ✗ int errsv = errno;
496 ✗ rt_printf(
497 "UsbStream::write_device: Failed to write in port %s with "
498 "command %s and error\n\t%s\n",
499 file_name_.c_str(),
500 ✗ msg_debug_string(msg).c_str(),
501 strerror(errsv));
502 ✗ return false;
503 }
504 ✗ else if (return_value_ != static_cast<ssize_t>(msg.size()))
505 {
506 ✗ rt_printf(
507 "UsbStream::write_device: Failed to write in port %s, the "
508 "requested amount of bytes is %ld, could only write %ld bytes\n",
509 file_name_.c_str(),
510 msg.size(),
511 return_value_);
512 ✗ return false;
513 }
514
515 ✗ return true;
516 }
517
518 ✗ bool UsbStream::set_poll_mode_timeout(double timeout_in_second)
519 {
520 #ifdef XENOMAI
521 /*
522 // Set read timeout
523 rt_config_.config_mask = RTSER_SET_TIMEOUT_RX | RTSER_SET_BAUD;
524 rt_config_.rx_timeout = (nanosecs_rel_t)(timeout * 1000000000); //
525 rx_timeout in ns rt_config_.baud_rate = 921600; res_ = rt_dev_ioctl(fd_,
526 RTSER_RTIOC_SET_CONFIG, &rt_config_); if (res_ != 0)
527 {
528 rt_printf("ERROR >> Failed to set read timeout.\n");
529 return false;
530 }*/
531 throw std::runtime_error(
532 "set_poll_mode_timeout not implemented for Xenomai");
533 #else
534 ✗ FD_ZERO(&file_id_set_);
535 ✗ FD_SET(file_id_, &file_id_set_);
536 ✗ long int tv_sec = timeout_in_second;
537 ✗ long int tv_nsec = (timeout_in_second - tv_sec) * 1000000000 /* 1e9 */;
538 ✗ timeout_posix_.tv_sec = tv_sec;
539 ✗ timeout_posix_.tv_nsec = tv_nsec;
540 #endif
541 ✗ timeout_ = timeout_in_second;
542 ✗ timeout_set_ = true;
543 ✗ return true;
544 }
545
546 ✗ std::string UsbStream::msg_debug_string(const std::vector<uint8_t>& msg,
547 long int until)
548 {
549 ✗ long int msg_size = static_cast<long int>(msg.size());
550 ✗ if (until < 0)
551 {
552 ✗ until = msg_size;
553 }
554 ✗ std::ostringstream cmd_debug_string;
555 ✗ cmd_debug_string << "[ ";
556 ✗ for (long int i = 0; i < std::min(msg_size, until); ++i)
557 {
558 ✗ cmd_debug_string << std::hex << std::setfill('0') << std::setw(2)
559 ✗ << std::uppercase << (msg[i] & 0xFF) << " ";
560 }
561 ✗ cmd_debug_string << "]";
562 ✗ return cmd_debug_string.str();
563 }
564
565 ✗ bool UsbStream::test_msg_equal(const std::vector<uint8_t>& msg1,
566 const std::vector<uint8_t>& msg2)
567 {
568 ✗ if (msg1.size() != msg2.size())
569 {
570 ✗ return false;
571 }
572 ✗ bool test = true;
573 ✗ for (unsigned i = 0; i < msg1.size(); ++i)
574 {
575 ✗ test = test && (msg1[i] == msg2[i]);
576 }
577 ✗ return test;
578 }
579
580 ✗ bool UsbStream::flush(int)
581 {
582 #ifdef XENOMAI
583 #else
584 // fcntl(file_id_, F_SETFL, 0);
585 // return_value_ = fcntl(file_id_, F_SETFL, (O_RDWR | O_NONBLOCK));
586
587 // int i = duration_ms;
588 // while (--i > 0) {
589 // real_time_tools::Timer::sleep_ms(1.0);
590 // while ((return_value_ = read(file_id_, buffer_.data(), buffer_.size()))
591 // > 0)
592 // { // flush buffer and make sure it's cleared for while.
593 // i = 100;
594 // }
595 // }
596 // fcntl(file_id_, F_SETFL, 0);
597 // return_value_ = fcntl(file_id_, F_SETFL, O_RDWR);
598 // real_time_tools::Timer::sleep_ms(500);
599
600 ✗ if (tcflush(file_id_, TCIOFLUSH) == -1)
601 {
602 ✗ printf("flush failed\n");
603 ✗ return false;
604 }
605 ✗ return true;
606 #endif
607 }
608
609
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
 42 } // namespace real_time_tools
610