diff --git a/src/lib/rc/sbus.cpp b/src/lib/rc/sbus.cpp index dfcf2f4dd8..8ea00aeab3 100644 --- a/src/lib/rc/sbus.cpp +++ b/src/lib/rc/sbus.cpp @@ -54,6 +54,11 @@ #define SBUS_DEBUG_LEVEL 0 /* Set debug output level */ +#if defined(__PX4_POSIX_OCPOC) +#include +#include +#endif + #define SBUS_START_SYMBOL 0x0f #define SBUS_INPUT_CHANNELS 16 @@ -152,6 +157,54 @@ sbus_init(const char *device, bool singlewire) int sbus_config(int sbus_fd, bool singlewire) { +#if defined(__PX4_POSIX_OCPOC) + 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 (sbus_fd >= 0) { @@ -180,6 +233,7 @@ sbus_config(int sbus_fd, bool singlewire) } return ret; +#endif } void @@ -231,6 +285,31 @@ bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { +#if defined(__PX4_POSIX_OCPOC) + hrt_abstime now; + now = hrt_absolute_time(); + last_rx_time = now; + partial_frame_count = 0; + + ssize_t n; + uint8_t buf[SBUS_FRAME_SIZE * 4]; + if ((n = read(sbus_fd, &buf[0], SBUS_FRAME_SIZE*4)) <= 0){ + return false; + } + + for (int i=0; i= 5) { + return true; + } + } + } + + return false; +#else int ret = 1; hrt_abstime now; @@ -275,8 +354,10 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails } return sbus_decoded; +#endif } +#if !defined(__PX4_POSIX_OCPOC) bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels) @@ -482,6 +563,7 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, /* return false as default */ return decode_ret; } +#endif /* * S.bus decoder matrix.