diff options
author | Julian Oes <julian@oes.ch> | 2014-02-14 13:36:59 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-02-14 13:36:59 +0100 |
commit | 523637e0f1fb0247111818d0a88ce8c4574728ba (patch) | |
tree | 1b4b264642a18e61255485efbdcf646b9f8d94ba /src/modules/mavlink/mavlink_receiver.cpp | |
parent | ef46cd5e909115c8c208c409fcded0dd02037d5e (diff) | |
download | px4-firmware-523637e0f1fb0247111818d0a88ce8c4574728ba.tar.gz px4-firmware-523637e0f1fb0247111818d0a88ce8c4574728ba.tar.bz2 px4-firmware-523637e0f1fb0247111818d0a88ce8c4574728ba.zip |
Mavlink: Start multiple uart listeners, HIL working
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 16 |
1 files changed, 9 insertions, 7 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 44fff2416..371f945c4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -798,7 +798,10 @@ MavlinkReceiver::receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); + /* Set thread name */ + char thread_name[18]; + sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel()); + prctl(PR_SET_NAME, thread_name, getpid()); struct pollfd fds[1]; fds[0].fd = uart_fd; @@ -842,20 +845,19 @@ void MavlinkReceiver::print_status() void * MavlinkReceiver::start_helper(void *context) { - MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context)); + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context); return rcv->receive_thread(NULL); } pthread_t -MavlinkReceiver::receive_start(Mavlink *mavlink) +MavlinkReceiver::receive_start(Mavlink *parent) { - pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); // set to non-blocking read - int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0); - fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); + int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); + fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); @@ -865,7 +867,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent); pthread_attr_destroy(&receiveloop_attr); return thread; |