From fbcc6a96bc89d14806126c14d70b9a0585dcdb6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 5 Dec 2019 15:06:11 +0100 Subject: [PATCH] fix mavlink: fixes for mavlink on USB instance Open the UART after adding the instance: mavlink_open_uart() might block, and in that case the parent task waiting for mavlink to be started times out. And while waiting for the USB UART device to come up: - check for _task_should_exit - check for check_requested_subscriptions() Side-effects when USB is not plugged in during boot: - reduces boot duration by 100ms - fixes duplicate instance name in 'top': 201 mavlink_if0 1 0.000 1328/ 2572 100 (100) w:sig 3 204 mavlink_if0 572 4.221 1632/ 2540 100 (100) w:sig 4 - 'mavlink stop-all' now stops the usb instance as well --- src/modules/mavlink/mavlink_main.cpp | 43 +++++++++++++++++----------- 1 file changed, 26 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5ca2c091e4..b04a77a13f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -578,8 +578,7 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool for } /* back off 1800 ms to avoid running into the USB setup timing */ - while (_is_usb_uart && - hrt_absolute_time() < 1800U * 1000U) { + while (_is_usb_uart && hrt_absolute_time() < 1800U * 1000U) { px4_usleep(50000); } @@ -592,7 +591,10 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool for uORB::SubscriptionData armed_sub{ORB_ID(actuator_armed)}; /* get the system arming state and abort on arming */ - while (_uart_fd < 0) { + while (_uart_fd < 0 && !_task_should_exit) { + + /* another task might have requested subscriptions: make sure we handle it */ + check_requested_subscriptions(); /* abort if an arming topic is published and system is armed */ armed_sub.update(); @@ -1881,6 +1883,12 @@ Mavlink::task_main(int argc, char *argv[]) case 'd': _device_name = myoptarg; set_protocol(Protocol::SERIAL); + + if (access(_device_name, F_OK) == -1) { + PX4_ERR("Device %s does not exist", _device_name); + err_flag = true; + } + break; case 'n': @@ -2048,7 +2056,7 @@ Mavlink::task_main(int argc, char *argv[]) return PX4_ERROR; } - /* USB serial is indicated by /dev/ttyACM0*/ + /* USB serial is indicated by /dev/ttyACMx */ if (strcmp(_device_name, "/dev/ttyACM0") == OK || strcmp(_device_name, "/dev/ttyACM1") == OK) { if (_datarate == 0) { _datarate = 800000; @@ -2086,19 +2094,6 @@ Mavlink::task_main(int argc, char *argv[]) /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); - - /* default values for arguments */ - _uart_fd = mavlink_open_uart(_baudrate, _device_name, _force_flow_control); - - if (_uart_fd < 0 && !_is_usb_uart) { - PX4_ERR("could not open %s", _device_name); - return PX4_ERROR; - - } else if (_uart_fd < 0 && _is_usb_uart) { - /* the config link is optional */ - return OK; - } - } #if defined(MAVLINK_UDP) @@ -2198,6 +2193,20 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); + /* open the UART device after setting the instance, as it might block */ + if (get_protocol() == Protocol::SERIAL) { + _uart_fd = mavlink_open_uart(_baudrate, _device_name, _force_flow_control); + + if (_uart_fd < 0 && !_is_usb_uart) { + PX4_ERR("could not open %s", _device_name); + return PX4_ERROR; + + } else if (_uart_fd < 0 && _is_usb_uart) { + /* the config link is optional */ + return PX4_OK; + } + } + #if defined(MAVLINK_UDP) /* init socket if necessary */