From 51665a2ea987e9dd567cc8657df3523c19d787b6 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Tue, 30 May 2017 10:47:14 -0700 Subject: [PATCH] AP_HAL: RCOutput_Tap: split baudrate set for Linux and Nuttx On Linux (and in most of posix-like systems) the baudrate set in struct termios via cfset[io]speed is actually the index of the standard baudrates, it's not the baudrate per se (hence the use of B* macros). On Linux it's possible to set the baudrate by passing the baudrate itself to TCSETS2 ioctl. Unfortunately it's not possible to include asm/termbits.h and termios.h in the same compilation unit, so we need to split it. Here we split into a Linux and a NuttX implementation. --- libraries/AP_HAL/utility/RCOutput_Tap.cpp | 14 ++--- libraries/AP_HAL/utility/RCOutput_Tap.h | 1 + .../AP_HAL/utility/RCOutput_Tap_Linux.cpp | 63 +++++++++++++++++++ .../AP_HAL/utility/RCOutput_Tap_Nuttx.cpp | 52 +++++++++++++++ 4 files changed, 122 insertions(+), 8 deletions(-) create mode 100644 libraries/AP_HAL/utility/RCOutput_Tap_Linux.cpp create mode 100644 libraries/AP_HAL/utility/RCOutput_Tap_Nuttx.cpp diff --git a/libraries/AP_HAL/utility/RCOutput_Tap.cpp b/libraries/AP_HAL/utility/RCOutput_Tap.cpp index 83ac470c26a..9dbe87a1b9c 100644 --- a/libraries/AP_HAL/utility/RCOutput_Tap.cpp +++ b/libraries/AP_HAL/utility/RCOutput_Tap.cpp @@ -322,24 +322,22 @@ bool RCOutput_Tap::_uart_open() return -1; } - // set baud rate - int speed = 250000; struct termios uart_config; + memset(&uart_config, 0, sizeof(uart_config)); tcgetattr(_uart_fd, &uart_config); // clear ONLCR flag (which appends a CR for every LF) uart_config.c_oflag &= ~ONLCR; - // set baud rate - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - ::fprintf(stderr, "failed to set baudrate for %s: %d\n", - HAL_RCOUTPUT_TAP_DEVICE, termios_state); + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + ::fprintf(stderr, "tcsetattr failed for %s\n", HAL_RCOUTPUT_TAP_DEVICE); _uart_close(); return false; } - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "tcsetattr failed for %s\n", HAL_RCOUTPUT_TAP_DEVICE); + if (!_uart_set_speed(250000)) { + ::fprintf(stderr, "failed to set baudrate for %s: %m\n", + HAL_RCOUTPUT_TAP_DEVICE); _uart_close(); return false; } diff --git a/libraries/AP_HAL/utility/RCOutput_Tap.h b/libraries/AP_HAL/utility/RCOutput_Tap.h index b101a40c5d2..ee6138a766b 100644 --- a/libraries/AP_HAL/utility/RCOutput_Tap.h +++ b/libraries/AP_HAL/utility/RCOutput_Tap.h @@ -82,6 +82,7 @@ private: int _send_packet(EscPacket &p); bool _uart_open(); + bool _uart_set_speed(int speed); void _uart_close(); AP_HAL::Util::perf_counter_t _perf_rcout; diff --git a/libraries/AP_HAL/utility/RCOutput_Tap_Linux.cpp b/libraries/AP_HAL/utility/RCOutput_Tap_Linux.cpp new file mode 100644 index 00000000000..eb8abb92705 --- /dev/null +++ b/libraries/AP_HAL/utility/RCOutput_Tap_Linux.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2017 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include "RCOutput_Tap.h" + +#include +#include +#include +#include +#include + +namespace ap { + +bool RCOutput_Tap::_uart_set_speed(int speed) +{ + struct termios2 tc; + + if (_uart_fd < 0) { + return false; + } + + memset(&tc, 0, sizeof(tc)); + if (ioctl(_uart_fd, TCGETS2, &tc) == -1) { + return false; + } + + /* speed is configured by c_[io]speed */ + tc.c_cflag &= ~CBAUD; + tc.c_cflag |= BOTHER; + tc.c_ispeed = speed; + tc.c_ospeed = speed; + + if (ioctl(_uart_fd, TCSETS2, &tc) == -1) { + return false; + } + + if (ioctl(_uart_fd, TCFLSH, TCIOFLUSH) == -1) { + return false; + } + + return true; +} + +} + +#endif diff --git a/libraries/AP_HAL/utility/RCOutput_Tap_Nuttx.cpp b/libraries/AP_HAL/utility/RCOutput_Tap_Nuttx.cpp new file mode 100644 index 00000000000..ff0a5bb2b99 --- /dev/null +++ b/libraries/AP_HAL/utility/RCOutput_Tap_Nuttx.cpp @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2017 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_AEROFC_V1 + +#include "RCOutput_Tap.h" + +#include +#include + +namespace ap { + +bool RCOutput_Tap::_uart_set_speed(int speed) +{ + if (_uart_fd < 0) { + return false; + } + + struct termios uart_config; + memset(&uart_config, 0, sizeof(uart_config)); + tcgetattr(_uart_fd, &uart_config); + + // set baud rate + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + return false; + } + + if (tcsetattr(_uart_fd, TCSANOW, &uart_config) < 0) { + return false; + } + + return true; +} + +} + +#endif