From eb64a6813f887ad1660e3bb8c4c244fd435d31ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:01:10 +0100 Subject: [PATCH] Sim MAVLink: Return correct type, remove very uninformed comment --- src/modules/simulator/simulator_mavlink.cpp | 22 +++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2d625a21ac..57b7d167f8 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -348,15 +348,23 @@ void Simulator::poll_topics() void *Simulator::sending_trampoline(void *) { _instance->send(); - return 0; // why do I have to put this??? + return nullptr; } void Simulator::send() { - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _actuator_outputs_sub; fds[0].events = POLLIN; + int rv; + // set the threads name + #ifdef __PX4_DARWIN + rv = pthread_setname_np("sim_send"); + #else + rv = pthread_setname_np(pthread_self(), "sim_send"); + #endif + int pret; while (true) { @@ -371,8 +379,6 @@ void Simulator::send() // this is undesirable but not much we can do if (pret < 0) { PX4_WARN("poll error %d, %d", pret, errno); - // sleep a bit before next try - usleep(100000); continue; } @@ -516,6 +522,14 @@ void Simulator::pollForMAVLinkMessages(bool publish) pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); + int rv; + // set the threads name + #ifdef __PX4_DARWIN + rv = pthread_setname_np("sim_rcv"); + #else + rv = pthread_setname_np(pthread_self(), "sim_rcv"); + #endif + // wait for new mavlink messages to arrive while (true) {