From 29135a0158ad11f7afd82de64e77f5010f0e8dc6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 8 Jun 2016 10:30:13 +1000
Subject: [PATCH] HAL_Linux: use termios2 for SBUS RCOUT on Disco

---
 libraries/AP_HAL_Linux/RCInput_SBUS.cpp | 36 ++++++++++++++++---------
 1 file changed, 23 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_HAL_Linux/RCInput_SBUS.cpp b/libraries/AP_HAL_Linux/RCInput_SBUS.cpp
index 2490b86950..ffdf7af06c 100644
--- a/libraries/AP_HAL_Linux/RCInput_SBUS.cpp
+++ b/libraries/AP_HAL_Linux/RCInput_SBUS.cpp
@@ -26,8 +26,9 @@
 #include <stdio.h>
 #include <fcntl.h>
 #include <unistd.h>
-#include <termios.h>
 #include <errno.h>
+#include <sys/ioctl.h>
+#include <asm/termbits.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -62,27 +63,36 @@ void RCInput_SBUS::_timer_tick(void)
         if (fd != -1) {
             printf("Opened SBUS input %s fd=%d\n", device_path, (int)fd);
             fflush(stdout);
-            struct termios t;
-            memset(&t, 0, sizeof(t));
-            tcgetattr(fd, &t);
-            t.c_ispeed = B100000;
-            t.c_ospeed = B100000;
-            t.c_cc[VTIME] = 0;
-            t.c_cflag |= (CSTOPB | PARENB);
-            t.c_cflag &= ~(BRKINT | PARODD);
-            t.c_iflag &= ~(INLCR | ICRNL | IMAXBEL | IXON | IXOFF | ICANON | IEXTEN | ECHO);
-            tcsetattr(fd, TCSANOW, &t);
+            struct termios2 tio {};
+
+            int ret = ioctl(fd, TCGETS2, &tio);
+            printf("ret1=%d\n", ret);
+
+            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_ispeed = B100000;
+            tio.c_ospeed = B100000;
+            tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
+            tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);
+            tio.c_cc[VMIN] = 25;
+            tio.c_cc[VTIME] = 0;
+            ret = ioctl(fd, TCSETS2, &tio);
+            printf("SBUS ret=%d %s\n", ret, strerror(errno));
         }
     }
     if (fd != -1) {
         uint8_t bytes[25];
         int32_t nread;
+        uint8_t counter = 100;
         do {
             nread = ::read(fd, bytes, sizeof(bytes));
-            if (nread > 0) {
+            if (nread > 0 && bytes[0] == 0x0f) {
                 add_sbus_input(bytes, nread);
             }
-        } while (nread == 25);
+        } while (nread == 25 && counter--);
     }
 }
 #endif // CONFIG_HAL_BOARD_SUBTYPE