diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index a38c640fa3..d5eab4170d 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -79,6 +79,7 @@ static constexpr wq_config_t UART5{"wq:UART5", 1400, -21}; static constexpr wq_config_t UART6{"wq:UART6", 1400, -22}; static constexpr wq_config_t UART7{"wq:UART7", 1400, -23}; static constexpr wq_config_t UART8{"wq:UART8", 1400, -24}; +static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -25}; static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 966d15829e..4c537fd49c 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -196,9 +196,9 @@ serial_port_to_wq(const char *serial) return wq_configurations::UART8; } - PX4_ERR("unknown serial port: %s", serial); + PX4_DEBUG("unknown serial port: %s", serial); - return wq_configurations::hp_default; + return wq_configurations::UART_UNKNOWN; } static void * diff --git a/src/lib/rc/sbus.cpp b/src/lib/rc/sbus.cpp index 469dfcd20a..8c882329b5 100644 --- a/src/lib/rc/sbus.cpp +++ b/src/lib/rc/sbus.cpp @@ -41,7 +41,6 @@ #include #include -#include #include #ifdef TIOCSSINGLEWIRE @@ -58,7 +57,9 @@ using namespace time_literals; #if defined(__PX4_LINUX) #include -#include +#include +#else +#include #endif #define SBUS_START_SYMBOL 0x0f @@ -152,57 +153,41 @@ sbus_init(const char *device, bool singlewire) int sbus_config(int sbus_fd, bool singlewire) { -#if defined(__PX4_LINUX) - struct termios options; - - if (tcgetattr(sbus_fd, &options) != 0) { - return -1; - } - - tcflush(sbus_fd, TCIFLUSH); - bzero(&options, sizeof(options)); - - options.c_cflag |= (CLOCAL | CREAD); - options.c_cflag &= ~CSIZE; - options.c_cflag |= CS8; - options.c_cflag |= PARENB; - options.c_cflag &= ~PARODD; - options.c_iflag |= INPCK; - options.c_cflag |= CSTOPB; - - options.c_cc[VTIME] = 0; - options.c_cc[VMIN] = 0; - - cfsetispeed(&options, B38400); - cfsetospeed(&options, B38400); - - tcflush(sbus_fd, TCIFLUSH); - - if ((tcsetattr(sbus_fd, TCSANOW, &options)) != 0) { - return -1; - } - - int baud = 100000; - struct serial_struct serials; - - if ((ioctl(sbus_fd, TIOCGSERIAL, &serials)) < 0) { - return -1; - } - - serials.flags = ASYNC_SPD_CUST; - serials.custom_divisor = serials.baud_base / baud; - - if ((ioctl(sbus_fd, TIOCSSERIAL, &serials)) < 0) { - return -1; - } - - ioctl(sbus_fd, TIOCGSERIAL, &serials); - - tcflush(sbus_fd, TCIFLUSH); - return 0; -#else int ret = -1; +#if defined(__PX4_LINUX) + + struct termios2 tio = {}; + + if (0 != ioctl(sbus_fd, TCGETS2, &tio)) { + return ret; + } + + /** + * Setting serial port,8E2, non-blocking.100Kbps + */ + tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL + | IXON); + tio.c_iflag |= (INPCK | IGNPAR); + tio.c_oflag &= ~OPOST; + tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD); + /** + * use BOTHER to specify speed directly in c_[io]speed member + */ + tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD); + tio.c_ispeed = 100000; + tio.c_ospeed = 100000; + tio.c_cc[VMIN] = 25; + tio.c_cc[VTIME] = 0; + + if (0 != ioctl(sbus_fd, TCSETS2, &tio)) { + return ret; + } + + ret = 0; +#else + if (sbus_fd >= 0) { struct termios t; @@ -221,16 +206,16 @@ sbus_config(int sbus_fd, bool singlewire) #endif } - /* initialise the decoder */ - partial_frame_count = 0; - last_rx_time = hrt_absolute_time(); - sbus_frame_drops = 0; - ret = 0; } - return ret; #endif + /* initialise the decoder */ + partial_frame_count = 0; + last_rx_time = hrt_absolute_time(); + sbus_frame_drops = 0; + + return ret; } void