dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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:
Beat Küng 2019-12-05 15:06:11 +01:00 committed by Julian Oes
parent 08e36de020
commit 7ee74099ab
1 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/modules/mavlink/mavlink_main.cpp
View File

@ -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<actuator_armed_s> 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 */