forked from Archive/PX4-Autopilot
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
This commit is contained in:
parent
08e36de020
commit
7ee74099ab
|
@ -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 */
|
/* back off 1800 ms to avoid running into the USB setup timing */
|
||||||
while (_is_usb_uart &&
|
while (_is_usb_uart && hrt_absolute_time() < 1800U * 1000U) {
|
||||||
hrt_absolute_time() < 1800U * 1000U) {
|
|
||||||
px4_usleep(50000);
|
px4_usleep(50000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -594,7 +593,10 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool for
|
||||||
uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
|
uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
|
||||||
|
|
||||||
/* get the system arming state and abort on arming */
|
/* 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 */
|
/* abort if an arming topic is published and system is armed */
|
||||||
armed_sub.update();
|
armed_sub.update();
|
||||||
|
@ -1883,6 +1885,12 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
case 'd':
|
case 'd':
|
||||||
_device_name = myoptarg;
|
_device_name = myoptarg;
|
||||||
set_protocol(Protocol::SERIAL);
|
set_protocol(Protocol::SERIAL);
|
||||||
|
|
||||||
|
if (access(_device_name, F_OK) == -1) {
|
||||||
|
PX4_ERR("Device %s does not exist", _device_name);
|
||||||
|
err_flag = true;
|
||||||
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'n':
|
case 'n':
|
||||||
|
@ -2050,7 +2058,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
return PX4_ERROR;
|
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 (strcmp(_device_name, "/dev/ttyACM0") == OK || strcmp(_device_name, "/dev/ttyACM1") == OK) {
|
||||||
if (_datarate == 0) {
|
if (_datarate == 0) {
|
||||||
_datarate = 800000;
|
_datarate = 800000;
|
||||||
|
@ -2088,19 +2096,6 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
|
|
||||||
/* flush stdout in case MAVLink is about to take it over */
|
/* flush stdout in case MAVLink is about to take it over */
|
||||||
fflush(stdout);
|
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)
|
#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 */
|
/* now the instance is fully initialized and we can bump the instance count */
|
||||||
LL_APPEND(_mavlink_instances, this);
|
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)
|
#if defined(MAVLINK_UDP)
|
||||||
|
|
||||||
/* init socket if necessary */
|
/* init socket if necessary */
|
||||||
|
|
Loading…
Reference in New Issue