@ -127,34 +127,53 @@ Mavlink::Mavlink() :
@@ -127,34 +127,53 @@ Mavlink::Mavlink() :
}
_vehicle_command_sub . subscribe ( ) ;
if ( orb_exists ( ORB_ID ( event ) , 0 ) = = PX4_ERROR ) {
orb_advertise_queue ( ORB_ID ( event ) , nullptr , event_s : : ORB_QUEUE_LENGTH ) ;
}
_event_sub . subscribe ( ) ;
}
Mavlink : : ~ Mavlink ( )
{
if ( _task_running ) {
if ( running ( ) ) {
/* task wakes up every 10ms or so at the longest */
_task_should_exit = true ;
request_stop ( ) ;
/* wait for a second for the task to quit at our request */
unsigned i = 0 ;
do {
/* wait 20ms */
px4_usleep ( 2 0000) ;
/* wait at least 1 second (10ms * 10) */
px4_usleep ( 1 0000) ;
/* if we have given up, kill it */
if ( + + i > 5 0) {
//TODO store main task handle in Mavlink instance to allow killing task
//task_delete(_mavlink_task);
if ( + + i > 10 0) {
PX4_ERR ( " mavlink didn't stop, killing task %d " , _task_id ) ;
px4_task_delete ( _task_id ) ;
break ;
}
} while ( _task_ running) ;
} while ( running ( ) ) ;
}
if ( _instance_id > = 0 ) {
mavlink_module_instances [ _instance_id ] = nullptr ;
}
// if this instance was responsible for checking events then select a new mavlink instance
if ( check_events ( ) ) {
check_events_disable ( ) ;
// select next available instance
for ( Mavlink * inst : mavlink_module_instances ) {
if ( inst ) {
inst - > check_events_enable ( ) ;
break ;
}
}
}
perf_free ( _loop_perf ) ;
perf_free ( _loop_interval_perf ) ;
perf_free ( _send_byte_error_perf ) ;
@ -230,6 +249,20 @@ Mavlink::set_instance_id()
@@ -230,6 +249,20 @@ Mavlink::set_instance_id()
{
LockGuard lg { mavlink_module_mutex } ;
// instance count
size_t inst_count = 0 ;
for ( Mavlink * inst : mavlink_module_instances ) {
if ( inst ! = nullptr ) {
inst_count + + ;
}
}
// if this is the first instance use it to check events
if ( inst_count = = 0 ) {
check_events_enable ( ) ;
}
for ( int instance_id = 0 ; instance_id < MAVLINK_COMM_NUM_BUFFERS ; instance_id + + ) {
if ( mavlink_module_instances [ instance_id ] = = nullptr ) {
mavlink_module_instances [ instance_id ] = this ;
@ -312,9 +345,9 @@ Mavlink::destroy_all_instances()
@@ -312,9 +345,9 @@ Mavlink::destroy_all_instances()
for ( Mavlink * inst_to_del : mavlink_module_instances ) {
if ( inst_to_del ! = nullptr ) {
/* set flag to stop thread and wait for all threads to finish */
inst_to_del - > _task_should_exit = true ;
inst_to_del - > request_stop ( ) ;
while ( inst_to_del - > _task_ running) {
while ( inst_to_del - > running ( ) ) {
printf ( " . " ) ;
fflush ( stdout ) ;
px4_usleep ( 10000 ) ;
@ -527,44 +560,9 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
@@ -527,44 +560,9 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
return - EINVAL ;
}
/* back off 1800 ms to avoid running into the USB setup timing */
while ( _is_usb_uart & & hrt_absolute_time ( ) < 1800U * 1000U ) {
px4_usleep ( 50000 ) ;
}
/* open uart */
_uart_fd = : : open ( uart_name , O_RDWR | O_NOCTTY ) ;
/* if this is a config link, stay here and wait for it to open */
if ( _uart_fd < 0 & & _is_usb_uart ) {
uORB : : SubscriptionData < actuator_armed_s > armed_sub { ORB_ID ( actuator_armed ) } ;
/* get the system arming state and abort on arming */
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 ( ) ;
/* the system is now providing arming status feedback.
* instead of timing out , we resort to abort bringing
* up the terminal .
*/
if ( armed_sub . get ( ) . armed ) {
/* this is not an error, but we are done */
return - 1 ;
}
int errcode = errno ;
/* ENOTCONN means that the USB device is not yet connected */
px4_usleep ( errcode = = ENOTCONN ? 1000000 : 100000 ) ;
_uart_fd = : : open ( uart_name , O_RDWR | O_NOCTTY ) ;
}
}
/*
* Return here in the iridium mode since the iridium driver does not
* support the subsequent function calls .
@ -587,8 +585,6 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
@@ -587,8 +585,6 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config . c_oflag & = ~ ONLCR ;
if ( ! _is_usb_uart ) {
/* Set baud rate */
if ( cfsetispeed ( & uart_config , speed ) < 0 | | cfsetospeed ( & uart_config , speed ) < 0 ) {
PX4_ERR ( " ERR SET BAUD %s: %d \n " , uart_name , termios_state ) ;
@ -596,14 +592,6 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
@@ -596,14 +592,6 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
return - 1 ;
}
} else {
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000 ;
set_telemetry_status_type ( telemetry_status_s : : LINK_TYPE_USB ) ;
}
# if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
/* Put in raw mode */
cfmakeraw ( & uart_config ) ;
@ -626,12 +614,6 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
@@ -626,12 +614,6 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
int
Mavlink : : setup_flow_control ( enum FLOW_CONTROL_MODE mode )
{
// We can't do this on USB - skip
if ( _is_usb_uart ) {
_flow_control_mode = FLOW_CONTROL_OFF ;
return OK ;
}
struct termios uart_config ;
int ret = tcgetattr ( _uart_fd , & uart_config ) ;
@ -1251,7 +1233,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
@@ -1251,7 +1233,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
/* orb subscription must be done from the main thread,
* set _subscribe_to_stream and _subscribe_to_stream_rate fields
* which polled in mavlink main loop */
if ( ! _task_ should_exit) {
if ( ! should_exit ( ) ) {
/* wait for previous subscription completion */
while ( _subscribe_to_stream ! = nullptr ) {
px4_usleep ( MAIN_LOOP_DELAY / 2 ) ;
@ -2127,17 +2109,23 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2127,17 +2109,23 @@ Mavlink::task_main(int argc, char *argv[])
}
/* USB serial is indicated by /dev/ttyACMx */
if ( strcmp ( _device_name , " /dev/ttyACM0 " ) = = OK | | strcmp ( _device_name , " /dev/ttyACM1 " ) = = OK ) {
if ( strn cmp ( _device_name , " /dev/ttyACM " , 11 ) = = 0 ) {
if ( _datarate = = 0 ) {
_datarate = 800000 ;
}
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000 ;
if ( _mode = = MAVLINK_MODE_COUNT ) {
_mode = MAVLINK_MODE_CONFIG ;
}
_ftp_on = true ;
_is_usb_uart = true ;
_flow_control_mode = FLOW_CONTROL_OFF ;
set_telemetry_status_type ( telemetry_status_s : : LINK_TYPE_USB ) ;
}
if ( _mode = = MAVLINK_MODE_COUNT ) {
@ -2183,6 +2171,12 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2183,6 +2171,12 @@ Mavlink::task_main(int argc, char *argv[])
if ( ! set_instance_id ( ) ) {
PX4_ERR ( " no instances available " ) ;
return PX4_ERROR ;
} else {
// set thread name
char thread_name [ 13 ] ;
snprintf ( thread_name , sizeof ( thread_name ) , " mavlink_if%d " , get_instance_id ( ) ) ;
px4_prctl ( PR_SET_NAME , thread_name , px4_getpid ( ) ) ;
}
set_channel ( ) ;
@ -2249,13 +2243,9 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2249,13 +2243,9 @@ Mavlink::task_main(int argc, char *argv[])
if ( get_protocol ( ) = = Protocol : : SERIAL ) {
_uart_fd = mavlink_open_uart ( _baudrate , _device_name , _flow_control ) ;
if ( _uart_fd < 0 & & ! _is_usb_uart ) {
if ( _uart_fd < 0 ) {
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 ;
}
}
@ -2268,6 +2258,8 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2268,6 +2258,8 @@ Mavlink::task_main(int argc, char *argv[])
# endif // MAVLINK_UDP
_task_id = px4_getpid ( ) ;
/* if the protocol is serial, we send the system version blindly */
if ( get_protocol ( ) = = Protocol : : SERIAL ) {
send_autopilot_capabilities ( ) ;
@ -2275,17 +2267,11 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2275,17 +2267,11 @@ Mavlink::task_main(int argc, char *argv[])
_receiver . start ( ) ;
/* Events subscription: only the first MAVLink instance should check */
uORB : : Subscription event_sub { ORB_ID ( event ) } ;
const bool should_check_events = _instance_id = = 0 ;
uint16_t event_sequence_offset = 0 ; // offset to account for skipped events, not sent via MAVLink
// ensure topic exists, otherwise we might lose first queued events
orb_advertise_queue ( ORB_ID ( event ) , nullptr , event_s : : ORB_QUEUE_LENGTH ) ;
event_sub . subscribe ( ) ;
_mavlink_start_time = hrt_absolute_time ( ) ;
while ( ! _task_ should_exit) {
while ( ! should_exit ( ) ) {
/* main loop */
px4_usleep ( _main_loop_delay ) ;
@ -2504,10 +2490,13 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2504,10 +2490,13 @@ Mavlink::task_main(int argc, char *argv[])
}
/* handle new events */
if ( should_check_events ) {
if ( check_events ( ) ) {
if ( _event_sub . updated ( ) ) {
LockGuard lg { mavlink_module_mutex } ;
event_s orb_event ;
while ( event_sub . update ( & orb_event ) ) {
while ( _ event_sub. update ( & orb_event ) ) {
if ( events : : externalLogLevel ( orb_event . log_levels ) = = events : : LogLevel : : Disabled ) {
+ + event_sequence_offset ; // skip this event
@ -2524,6 +2513,7 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2524,6 +2513,7 @@ Mavlink::task_main(int argc, char *argv[])
}
}
}
}
_events . update ( t ) ;
@ -2608,7 +2598,7 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2608,7 +2598,7 @@ Mavlink::task_main(int argc, char *argv[])
/* delete streams */
_streams . clear ( ) ;
if ( _uart_fd > = 0 & & ! _is_usb_uart ) {
if ( _uart_fd > = 0 ) {
/* discard all pending data, as close() might block otherwise on NuttX with flow control enabled */
tcflush ( _uart_fd , TCIOFLUSH ) ;
/* close UART */
@ -2633,8 +2623,6 @@ Mavlink::task_main(int argc, char *argv[])
@@ -2633,8 +2623,6 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_destroy ( & _send_mutex ) ;
pthread_mutex_destroy ( & _radio_status_mutex ) ;
_task_running = false ;
PX4_INFO ( " exiting channel %i " , ( int ) _channel ) ;
return OK ;
@ -2791,16 +2779,15 @@ int Mavlink::start_helper(int argc, char *argv[])
@@ -2791,16 +2779,15 @@ int Mavlink::start_helper(int argc, char *argv[])
int res ;
if ( ! instance ) {
/* out of memory */
res = - ENOMEM ;
PX4_ERR ( " OUT OF MEM " ) ;
} else {
/* this will actually only return once MAVLink exits */
instance - > _task_running . store ( true ) ;
res = instance - > task_main ( argc , argv ) ;
instance - > _task_running = false ;
instance - > _task_running . store ( false ) ;
}
return res ;
@ -2838,14 +2825,12 @@ Mavlink::start(int argc, char *argv[])
@@ -2838,14 +2825,12 @@ Mavlink::start(int argc, char *argv[])
}
// Instantiate thread
char buf [ 24 ] ;
sprintf ( buf , " mavlink_if%d " , ic ) ;
// This is where the control flow splits
// between the starting task and the spawned
// task - start_helper() only returns
// when the started task exits.
px4_task_spawn_cmd ( buf ,
px4_task_spawn_cmd ( " mavlink_main " ,
SCHED_DEFAULT ,
SCHED_PRIORITY_DEFAULT ,
PX4_STACK_ADJUSTED ( 2896 ) + MAVLINK_NET_ADDED_STACK ,
@ -2907,7 +2892,7 @@ Mavlink::display_status()
@@ -2907,7 +2892,7 @@ Mavlink::display_status()
printf ( " \t rx errors: \t % " PRIu16 " \n " , _rstatus . rxerrors ) ;
printf ( " \t fixed: \t % " PRIu16 " \n " , _rstatus . fix ) ;
} else if ( _is_usb_uart ) {
} else if ( _tstatus . type = = telemetry_status_s : : LINK_TYPE_USB ) {
printf ( " USB CDC \n " ) ;
} else {
@ -3014,6 +2999,107 @@ Mavlink::display_status_streams()
@@ -3014,6 +2999,107 @@ Mavlink::display_status_streams()
}
}
int
Mavlink : : stop_command ( int argc , char * argv [ ] )
{
const char * device_name = nullptr ;
# if defined(MAVLINK_UDP)
char * eptr ;
int temp_int_arg ;
unsigned short network_port = 0 ;
# endif // MAVLINK_UDP
bool provided_device = false ;
bool provided_network_port = false ;
/*
* Called via main with original argv
* mavlink start
*
* Remove 2
*/
argc - = 2 ;
argv + = 2 ;
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false ;
int i = 0 ;
while ( i < argc ) {
if ( 0 = = strcmp ( argv [ i ] , " -d " ) & & i < argc - 1 ) {
provided_device = true ;
device_name = argv [ i + 1 ] ;
i + + ;
# if defined(MAVLINK_UDP)
} else if ( 0 = = strcmp ( argv [ i ] , " -u " ) & & i < argc - 1 ) {
provided_network_port = true ;
temp_int_arg = strtoul ( argv [ i + 1 ] , & eptr , 10 ) ;
if ( * eptr = = ' \0 ' ) {
network_port = temp_int_arg ;
} else {
err_flag = true ;
}
i + + ;
# endif // MAVLINK_UDP
} else {
err_flag = true ;
}
i + + ;
}
if ( ! err_flag ) {
Mavlink * inst = nullptr ;
if ( provided_device & & ! provided_network_port ) {
inst = get_instance_for_device ( device_name ) ;
# if defined(MAVLINK_UDP)
} else if ( provided_network_port & & ! provided_device ) {
inst = get_instance_for_network_port ( network_port ) ;
# endif // MAVLINK_UDP
} else if ( provided_device & & provided_network_port ) {
PX4_WARN ( " please provide either a device name or a network port " ) ;
return PX4_ERROR ;
}
if ( inst ! = nullptr ) {
/* set flag to stop thread and wait for all threads to finish */
if ( inst - > running ( ) & & ! inst - > should_exit ( ) ) {
inst - > request_stop ( ) ;
LockGuard lg { mavlink_module_mutex } ;
for ( int mavlink_instance = 0 ; mavlink_instance < MAVLINK_COMM_NUM_BUFFERS ; mavlink_instance + + ) {
if ( mavlink_module_instances [ mavlink_instance ] = = inst ) {
delete mavlink_module_instances [ mavlink_instance ] ;
mavlink_module_instances [ mavlink_instance ] = nullptr ;
return PX4_OK ;
}
}
}
return PX4_ERROR ;
}
} else {
usage ( ) ;
}
return PX4_ERROR ;
}
int
Mavlink : : stream_command ( int argc , char * argv [ ] )
{
@ -3215,6 +3301,13 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
@@ -3215,6 +3301,13 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
PRINT_MODULE_USAGE_COMMAND_DESCR ( " stop-all " , " Stop all instances " ) ;
PRINT_MODULE_USAGE_COMMAND_DESCR ( " stop " , " Stop a running instance " ) ;
# if defined(CONFIG_NET) || defined(__PX4_POSIX)
PRINT_MODULE_USAGE_PARAM_INT ( ' u ' , - 1 , 0 , 65536 , " Select Mavlink instance via local Network Port " , true ) ;
# endif
PRINT_MODULE_USAGE_PARAM_STRING ( ' d ' , nullptr , " <file:dev> " , " Select Mavlink instance via Serial Device " , true ) ;
PRINT_MODULE_USAGE_COMMAND_DESCR ( " status " , " Print status for all instances " ) ;
PRINT_MODULE_USAGE_ARG ( " streams " , " Print all enabled streams " , true ) ;
@ -3241,11 +3334,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
@@ -3241,11 +3334,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
if ( ! strcmp ( argv [ 1 ] , " start " ) ) {
return Mavlink : : start ( argc , argv ) ;
} else if ( ! strcmp ( argv [ 1 ] , " stop " ) ) {
PX4_WARN ( " mavlink stop is deprecated, use stop-all instead " ) ;
usage ( ) ;
return 1 ;
} else if ( ! strcmp ( argv [ 1 ] , " stop-all " ) ) {
return Mavlink : : destroy_all_instances ( ) ;
@ -3253,6 +3341,9 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
@@ -3253,6 +3341,9 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
bool show_streams_status = argc > 2 & & strcmp ( argv [ 2 ] , " streams " ) = = 0 ;
return Mavlink : : get_status_all_instances ( show_streams_status ) ;
} else if ( ! strcmp ( argv [ 1 ] , " stop " ) ) {
return Mavlink : : stop_command ( argc , argv ) ;
} else if ( ! strcmp ( argv [ 1 ] , " stream " ) ) {
return Mavlink : : stream_command ( argc , argv ) ;
Daniel Agar
3 years ago
committed by
GitHub