mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
parent
8dcd3986c7
commit
a3aa278589
|
@ -124,10 +124,9 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
|
||||||
{
|
{
|
||||||
while (AP_HAL::micros64() < wait_time_usec) {
|
while (AP_HAL::micros64() < wait_time_usec) {
|
||||||
struct sitl_input input {};
|
struct sitl_input input {};
|
||||||
sitl_model->update(input);
|
sitl_model->update(input); // delays up to 1 millisecond
|
||||||
sim_update();
|
sim_update();
|
||||||
update_voltage_current(input, 0);
|
update_voltage_current(input, 0);
|
||||||
usleep(100);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -195,7 +194,7 @@ void SimMCast::multicast_read(void)
|
||||||
printf("Waiting for multicast state\n");
|
printf("Waiting for multicast state\n");
|
||||||
}
|
}
|
||||||
struct SITL::sitl_fdm state;
|
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
|
// nop
|
||||||
}
|
}
|
||||||
if (_sitl->state.timestamp_us == 0) {
|
if (_sitl->state.timestamp_us == 0) {
|
||||||
|
|
Loading…
Reference in New Issue