From 641f40f4f62c7f88127410ea727c4d66a7f7ccab Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 13 Jul 2024 12:00:11 -0500 Subject: [PATCH] AP_HAL_SITL: improve periph simulation efficiency Removes busywait for simulation state packet, dramatically reducing CPU usage. The dominant wait time in AP_Periph is 1024 microseconds as this is the default value of HAL_PERIPH_LOOP_DELAY_US, so a 1ms wait is unlikely to be a problem. --- libraries/AP_HAL_SITL/SITL_Periph_State.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index b455fcafb749c..c90b40218b3e3 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -124,10 +124,9 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) { while (AP_HAL::micros64() < wait_time_usec) { struct sitl_input input {}; - sitl_model->update(input); + sitl_model->update(input); // delays up to 1 millisecond sim_update(); update_voltage_current(input, 0); - usleep(100); } } @@ -195,7 +194,7 @@ void SimMCast::multicast_read(void) printf("Waiting for multicast state\n"); } struct SITL::sitl_fdm state; - while (sock.recv((void*)&state, sizeof(state), 0) != sizeof(state)) { + while (sock.recv((void*)&state, sizeof(state), 1) != sizeof(state)) { // nop } if (_sitl->state.timestamp_us == 0) {