diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 39f8d33191..ebff275a42 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -580,8 +580,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); } @@ -594,7 +593,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(); @@ -1883,6 +1885,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': @@ -2050,7 +2058,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; @@ -2088,19 +2096,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) @@ -2200,6 +2195,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 */