Permalink
Browse files

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.
  • Loading branch information...
1 parent 72eafe7 commit 8a58cf0daa75493abc605b8832f39d2f6ee08eac @LorenzMeier LorenzMeier committed Dec 26, 2016
Showing with 32 additions and 10 deletions.
  1. +12 −0 nuttx-patches/fix_CRTSCTS-define.patch
  2. +20 −10 src/modules/mavlink/mavlink_main.cpp
@@ -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) */
@@ -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);

0 comments on commit 8a58cf0

Please sign in to comment.