diff --git a/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp b/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp index d7dde0a82e..3eef99ab8a 100644 --- a/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RCProtocol.cpp @@ -24,6 +24,7 @@ #include #include #include "RCInput_RCProtocol.h" +#include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE @@ -102,43 +103,44 @@ int RCInput_RCProtocol::open_115200(const char *path) } // constructor -RCInput_RCProtocol::RCInput_RCProtocol(const char *_dev_sbus, const char *_dev_115200) : - dev_sbus(_dev_sbus), +RCInput_RCProtocol::RCInput_RCProtocol(const char *_dev_inverted, const char *_dev_115200) : + dev_inverted(_dev_inverted), dev_115200(_dev_115200) { } void RCInput_RCProtocol::init() { - if (dev_sbus) { - fd_sbus = open_sbus(dev_sbus); + if (dev_inverted) { + fd_inverted = open_sbus(dev_inverted); } else { - fd_sbus = -1; + fd_inverted = -1; } + inverted_is_115200 = false; if (dev_115200) { fd_115200 = open_115200(dev_115200); } else { fd_115200 = -1; } AP::RC().init(); - printf("SBUS FD %d 115200 FD %d\n", fd_sbus, fd_115200); + printf("SBUS FD %d 115200 FD %d\n", fd_inverted, fd_115200); } void RCInput_RCProtocol::_timer_tick(void) { - uint8_t b[40]; + uint8_t b[80]; - if (fd_sbus != -1) { - ssize_t n = ::read(fd_sbus, &b[0], sizeof(b)); + if (fd_inverted != -1) { + ssize_t n = ::read(fd_inverted, &b[0], sizeof(b)); if (n > 0) { for (uint8_t i=0; i 0) { + if (n > 0 && !inverted_is_115200) { for (uint8_t i=0; i 2000) { + // no inverted data for 2s, flip baudrate + close(fd_inverted); + inverted_is_115200 = !inverted_is_115200; + if (inverted_is_115200) { + fd_inverted = open_115200(dev_inverted); + } else { + fd_inverted = open_sbus(dev_inverted); + } + last_frame_ms = now; + } } #endif // HAL diff --git a/libraries/AP_HAL_Linux/RCInput_RCProtocol.h b/libraries/AP_HAL_Linux/RCInput_RCProtocol.h index f9dd1d4a15..9211a4998b 100644 --- a/libraries/AP_HAL_Linux/RCInput_RCProtocol.h +++ b/libraries/AP_HAL_Linux/RCInput_RCProtocol.h @@ -38,11 +38,13 @@ private: int open_sbus(const char *path); int open_115200(const char *path); - const char *dev_sbus; + const char *dev_inverted; const char *dev_115200; - int fd_sbus; + int fd_inverted; int fd_115200; + uint32_t last_frame_ms; + bool inverted_is_115200; }; };