From 8a58cf0daa75493abc605b8832f39d2f6ee08eac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Dec 2016 11:04:59 +0100 Subject: [PATCH] Fix usage of CRTSCTS define from termios.h NuttX had the CRTSCTS define incorrectly set for only output flow control, which broke our flow control logic. This commit patches NuttX and puts in addition a guard in place to prevent any future issue with the non-POSIX define being incorrect. This has been debugged and identified by @ecmnet, which was the main contribution for this patch. --- nuttx-patches/fix_CRTSCTS-define.patch | 12 +++++++++++ src/modules/mavlink/mavlink_main.cpp | 30 +++++++++++++++++--------- 2 files changed, 32 insertions(+), 10 deletions(-) create mode 100644 nuttx-patches/fix_CRTSCTS-define.patch diff --git a/nuttx-patches/fix_CRTSCTS-define.patch b/nuttx-patches/fix_CRTSCTS-define.patch new file mode 100644 index 0000000000..2c173118f6 --- /dev/null +++ b/nuttx-patches/fix_CRTSCTS-define.patch @@ -0,0 +1,12 @@ +diff -ruN NuttX/nuttx/include/termios.h NuttX/nuttx/include/termios.h +--- NuttX/nuttx/include/termios.h 1969-12-31 14:00:00.000000000 -1000 ++++ NuttX/nuttx/include/termios.h 2014-12-25 17:33:53.404950574 -1000 +@@ -110,8 +110,8 @@ + #define HUPCL (1 << 6) /* Bit 6: Hang up on last close */ + #define CLOCAL (1 << 7) /* Bit 7: Ignore modem status lines */ + #define CCTS_OFLOW (1 << 8) /* Bit 8: CTS flow control of output */ +-#define CRTSCTS CCTS_OFLOW + #define CRTS_IFLOW (1 << 9) /* Bit 9: RTS flow control of input */ ++#define CRTSCTS (CRTS_IFLOW | CCTS_OFLOW) + + /* Local Modes (c_lflag in the termios structure) */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f4473bbcf6..03a68b2f82 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -87,10 +87,16 @@ #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" +// Guard against MAVLink misconfiguration #ifndef MAVLINK_CRC_EXTRA #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems #endif +// Guard against flow control misconfiguration +#if defined (CRTSCTS) && defined (__PX4_NUTTX) && (CRTSCTS != (CRTS_IFLOW | CCTS_OFLOW)) +#error The non-standard CRTSCTS define is incorrect. Fix this in the OS or replace with (CRTS_IFLOW | CCTS_OFLOW) +#endif + #define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec #define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s @@ -705,10 +711,12 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name) case 1000000: speed = B1000000; break; #ifdef B1500000 + case 1500000: speed = B1500000; break; #endif #ifdef B3000000 + case 3000000: speed = B3000000; break; #endif @@ -839,18 +847,11 @@ Mavlink::enable_flow_control(bool enabled) int ret = tcgetattr(_uart_fd, &uart_config); if (enabled) { -#ifdef CRTS_IFLOW - uart_config.c_cflag |= CRTS_IFLOW; -#else uart_config.c_cflag |= CRTSCTS; -#endif } else { -#ifdef CRTS_IFLOW - uart_config.c_cflag &= ~CRTS_IFLOW; -#else uart_config.c_cflag &= ~CRTSCTS; -#endif + } ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); @@ -1630,6 +1631,7 @@ Mavlink::update_rate_mult() } float mavlink_ulog_streaming_rate_inv = 1.0f; + if (_mavlink_ulog) { mavlink_ulog_streaming_rate_inv = 1.f - _mavlink_ulog->current_data_rate(); } @@ -2202,6 +2204,7 @@ Mavlink::task_main(int argc, char *argv[]) /* send command ACK */ uint16_t current_command_ack = 0; + if (ack_sub->update(&ack_time, &command_ack)) { mavlink_command_ack_t msg; msg.result = command_ack.result; @@ -2234,15 +2237,19 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_ulog->stop(); _mavlink_ulog = nullptr; _mavlink_ulog_stop_requested = false; + } else { if (current_command_ack == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { _mavlink_ulog->start_ack_received(); } + int ret = _mavlink_ulog->handle_update(get_channel()); + if (ret < 0) { //abort the streaming on error if (ret != -1) { PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret); } + _mavlink_ulog->stop(); _mavlink_ulog = nullptr; } @@ -2534,14 +2541,17 @@ Mavlink::display_status() printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); printf("\trx: %.3f kB/s\n", (double)_rate_rx); printf("\trate mult: %.3f\n", (double)_rate_mult); + if (_mavlink_ulog) { - printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate()*100., - (double)_mavlink_ulog->maximum_data_rate()*100.); + printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100., + (double)_mavlink_ulog->maximum_data_rate() * 100.); } + printf("\taccepting commands: %s\n", (accepting_commands()) ? "YES" : "NO"); printf("\tMAVLink version: %i\n", _protocol_version); printf("\ttransport protocol: "); + switch (_protocol) { case UDP: printf("UDP (%i)\n", _network_port);