Skip to content

Commit

Permalink
AP_HAL_SITL: improve periph simulation efficiency
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
tpwrules authored and tridge committed Jul 16, 2024
1 parent 8dcd398 commit a3aa278
Showing 1 changed file with 2 additions and 3 deletions.
5 changes: 2 additions & 3 deletions libraries/AP_HAL_SITL/SITL_Periph_State.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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) {
Expand Down

0 comments on commit a3aa278

Please sign in to comment.