diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index cf66fdfff7..269fdb538a 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -31,6 +31,7 @@ #include "RCInput_115200.h" #include "RCInput_Multi.h" #include "RCInput_ZYNQ.h" +#include "RCInput_RCProtocol.h" #include "RCOutput_AioPRU.h" #include "RCOutput_Bebop.h" #include "RCOutput_Disco.h" @@ -150,7 +151,7 @@ static RCInput_ZYNQ rcinDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP static RCInput_UDP rcinDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO -static RCInput_Multi rcinDriver{3, new RCInput_SBUS, new RCInput_115200("/dev/uart-sumd"), new RCInput_UDP()}; +static RCInput_Multi rcinDriver{2, new RCInput_RCProtocol("/dev/uart-sbus", "/dev/uart-sumd"), new RCInput_UDP()}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO static RCInput_SoloLink rcinDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \ diff --git a/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp b/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp new file mode 100644 index 0000000000..2219c2e07d --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp @@ -0,0 +1,157 @@ +/* + This program 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 program 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 . + */ +/* + this is a driver for multiple RCInput methods on one board + */ + +#include +#include +#include +#include +#include +#include +#include +#include "RCInput_RCProtocol.h" + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE + +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +/* + open a SBUS UART + */ +int RCInput_RCProtocol::open_sbus(const char *path) +{ + int fd = open(path, O_RDWR | O_NONBLOCK | O_CLOEXEC); + if (fd == -1) { + return -1; + } + struct termios2 tio {}; + + if (ioctl(fd, TCGETS2, &tio) != 0) { + close(fd); + fd = -1; + return -1; + } + 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; + if (ioctl(fd, TCSETS2, &tio) != 0) { + close(fd); + fd = -1; + return -1; + } + + return fd; +} + +/* + open a 115200 UART + */ +int RCInput_RCProtocol::open_115200(const char *path) +{ + int fd = open(path, O_RDWR | O_NONBLOCK | O_CLOEXEC); + if (fd == -1) { + return -1; + } + struct termios2 tio {}; + + if (ioctl(fd, TCGETS2, &tio) != 0) { + close(fd); + fd = -1; + return -1; + } + + tio.c_cflag &= ~(PARENB|CSTOPB|CSIZE); + tio.c_cflag |= CS8 | B115200; + + tio.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG); + tio.c_iflag &= ~(IXON|IXOFF|IXANY); + tio.c_oflag &= ~OPOST; + + if (ioctl(fd, TCSETS2, &tio) != 0) { + close(fd); + fd = -1; + return -1; + } + + return fd; +} + +// constructor +RCInput_RCProtocol::RCInput_RCProtocol(const char *_dev_sbus, const char *_dev_115200) : + dev_sbus(_dev_sbus), + dev_115200(_dev_115200) +{ +} + +void RCInput_RCProtocol::init() +{ + if (dev_sbus) { + fd_sbus = open_sbus(dev_sbus); + } else { + fd_sbus = -1; + } + if (dev_115200) { + fd_115200 = open_115200(dev_115200); + } else { + fd_115200 = -1; + } + rcp.init(); + printf("SBUS FD %d 115200 FD %d\n", fd_sbus, fd_115200); +} + +void RCInput_RCProtocol::_timer_tick(void) +{ + uint8_t b[40]; + + if (fd_sbus != -1) { + ssize_t n = ::read(fd_sbus, &b[0], sizeof(b)); + if (n > 0) { + for (uint8_t i=0; i 0) { + for (uint8_t i=0; i. + */ +/* + RC input system that uses libraries/AP_RCProtocol with UART based inputs + with either SBUS protocol or 115200 based protocols (or both) + */ +#pragma once + +#include +#include +#include "RCInput.h" +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE + +namespace Linux { + +class RCInput_RCProtocol : public RCInput { +public: + RCInput_RCProtocol(const char *dev_sbus, const char *dev_115200); + void init() override; + void _timer_tick(void) override; + +private: + int open_sbus(const char *path); + int open_115200(const char *path); + + const char *dev_sbus; + const char *dev_115200; + + int fd_sbus; + int fd_115200; + AP_RCProtocol rcp; +}; +}; + +#endif