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);