diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h
index fbade066f2..875a04c5ea 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.h
+++ b/libraries/AP_HAL_SITL/UARTDriver.h
@@ -61,10 +61,9 @@ public:
enum flow_control get_flow_control(void) { return FLOW_CONTROL_ENABLE; }
- virtual bool set_speed(int speed) { return true; }
- virtual void configure_parity(uint8_t v) { }
- virtual void set_stop_bits(int n) { }
- bool set_unbuffered_writes(bool on);
+ void configure_parity(uint8_t v) override;
+ void set_stop_bits(int n) override;
+ bool set_unbuffered_writes(bool on) override;
void _timer_tick(void);
@@ -92,6 +91,7 @@ private:
void _check_connection(void);
static bool _select_check(int );
static void _set_nonblocking(int );
+ bool set_speed(int speed);
SITL_State *_sitlState;
diff --git a/libraries/AP_HAL_SITL/UART_utils.cpp b/libraries/AP_HAL_SITL/UART_utils.cpp
index 8e8025b5de..e89ca4e396 100644
--- a/libraries/AP_HAL_SITL/UART_utils.cpp
+++ b/libraries/AP_HAL_SITL/UART_utils.cpp
@@ -15,11 +15,16 @@
* with this program. If not, see .
*/
#include
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(__CYGWIN__)
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include "UARTDriver.h"
+#ifdef __CYGWIN__
#include
+#else
+#include
+#endif
+
#include
#include
#include
@@ -27,86 +32,102 @@
bool HALSITL::UARTDriver::set_speed(int speed)
{
- struct termios2 tc;
-
if (_fd < 0) {
return false;
}
-
+#ifdef __CYGWIN__
+ struct termios t;
+ tcgetattr(_fd, &t);
+ cfsetspeed(&t, speed);
+ tcsetattr(_fd, TCSANOW, &t);
+#else
+ struct termios2 tc;
memset(&tc, 0, sizeof(tc));
if (ioctl(_fd, TCGETS2, &tc) == -1) {
return false;
}
-
+
/* speed is configured by c_[io]speed */
tc.c_cflag &= ~CBAUD;
tc.c_cflag |= BOTHER;
tc.c_ispeed = speed;
tc.c_ospeed = speed;
-
if (ioctl(_fd, TCSETS2, &tc) == -1) {
return false;
}
-
if (ioctl(_fd, TCFLSH, TCIOFLUSH) == -1) {
return false;
}
+#endif
return true;
}
-void HALSITL::UARTDriver::configure_parity(uint8_t v) {
+void HALSITL::UARTDriver::configure_parity(uint8_t v)
+{
if (_fd < 0) {
return;
}
- struct termios2 tc;
+#ifdef __CYGWIN__
+ struct termios t;
- memset(&tc, 0, sizeof(tc));
- if (ioctl(_fd, TCGETS2, &tc) == -1) {
+ tcgetattr(_fd, &t);
+#else
+ struct termios2 t;
+ memset(&t, 0, sizeof(t));
+ if (ioctl(_fd, TCGETS2, &t) == -1) {
return;
}
+#endif
if (v != 0) {
// enable parity
- tc.c_cflag |= PARENB;
+ t.c_cflag |= PARENB;
if (v == 1) {
- tc.c_cflag |= PARODD;
+ t.c_cflag |= PARODD;
} else {
- tc.c_cflag &= ~PARODD;
+ t.c_cflag &= ~PARODD;
}
}
else {
// disable parity
- tc.c_cflag &= ~PARENB;
- }
- if (ioctl(_fd, TCSETS2, &tc) == -1) {
- return;
+ t.c_cflag &= ~PARENB;
}
- if (ioctl(_fd, TCFLSH, TCIOFLUSH) == -1) {
- return;
- }
+#ifdef __CYGWIN__
+ tcsetattr(_fd, TCSANOW, &t);
+#else
+ ioctl(_fd, TCSETS2, &t);
+#endif
}
-void HALSITL::UARTDriver::set_stop_bits(int n) {
+void HALSITL::UARTDriver::set_stop_bits(int n)
+{
if (_fd < 0) {
return;
}
- struct termios2 tc;
+#ifdef __CYGWIN__
+ struct termios t;
- memset(&tc, 0, sizeof(tc));
- if (ioctl(_fd, TCGETS2, &tc) == -1) {
+ tcgetattr(_fd, &t);
+#else
+ struct termios2 t;
+ memset(&t, 0, sizeof(t));
+ if (ioctl(_fd, TCGETS2, &t) == -1) {
return;
}
- if (n > 1) tc.c_cflag |= CSTOPB;
- else tc.c_cflag &= ~CSTOPB;
+#endif
- if (ioctl(_fd, TCSETS2, &tc) == -1) {
- return;
+ if (n > 1) {
+ t.c_cflag |= CSTOPB;
+ } else {
+ t.c_cflag &= ~CSTOPB;
}
- if (ioctl(_fd, TCFLSH, TCIOFLUSH) == -1) {
- return;
- }
+#ifdef __CYGWIN__
+ tcsetattr(_fd, TCSANOW, &t);
+#else
+ ioctl(_fd, TCSETS2, &t);
+#endif
}
#endif