From a34e57a4cc8817186327436edc1aa14d2174be67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2020 23:05:34 +0100 Subject: [PATCH] Simulator: Increase stack, publication affinity This commit increases the send thread stack size and changes the thread affinity of the lockstep clocking topic. It also improves verbosity in case error states occur. --- src/modules/simulator/simulator_mavlink.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 949a93a199..9853194d5a 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -594,6 +594,10 @@ void Simulator::send() pthread_setname_np(pthread_self(), "sim_send"); #endif + // Subscribe to topics. + // Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS. + _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); + // Before starting, we ought to send a heartbeat to initiate the SITL // simulator to start sending sensor data which will set the time and // get everything rolling. @@ -631,6 +635,8 @@ void Simulator::send() send_controls(); } } + + orb_unsubscribe(_actuator_outputs_sub); } void Simulator::request_hil_state_quaternion() @@ -691,7 +697,7 @@ void Simulator::run() break; } else { - system_usleep(100); + system_usleep(500); } } @@ -722,7 +728,7 @@ void Simulator::run() } else { ::close(_fd); - system_usleep(100); + system_usleep(500); } } @@ -735,7 +741,7 @@ void Simulator::run() pthread_attr_t sender_thread_attr; pthread_attr_init(&sender_thread_attr); - pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(4000)); + pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(8000)); struct sched_param param; (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); @@ -769,10 +775,6 @@ void Simulator::run() #endif - // Subscribe to topics. - // Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS. - _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); - // got data from simulator, now activate the sending thread pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr); pthread_attr_destroy(&sender_thread_attr); @@ -789,6 +791,7 @@ void Simulator::run() if (pret == 0) { // Timed out. + PX4_ERR("poll timeout %d, %d", pret, errno); continue; } @@ -833,8 +836,6 @@ void Simulator::run() #endif } - - orb_unsubscribe(_actuator_outputs_sub); } #ifdef ENABLE_UART_RC_INPUT