Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: block in MC read for packet in AP_Periph

Browse Source 
can't advance time without a new multicast packet
This commit is contained in:
Andrew Tridgell 2023-08-19 16:37:16 +10:00 committed by Peter Barker
parent 032c5cc6fe
commit 4a07f7a4eb
1 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_HAL_SITL/SITL_Periph_State.cpp
View File

@ -244,19 +244,20 @@ void SimMCast::multicast_read(void)
}
struct SITL::sitl_fdm state;
socklen_t len = sizeof(in_addr);
if (recvfrom(mc_fd, (void*)&state, sizeof(state), MSG_WAITALL, (sockaddr *)&in_addr, &len) == sizeof(state)) {
if (state.timestamp_us < _sitl->state.timestamp_us) {
// main process has rebooted
base_time_us += (_sitl->state.timestamp_us - state.timestamp_us);
}
_sitl->state = state;
hal.scheduler->stop_clock(_sitl->state.timestamp_us + base_time_us);
HALSITL::Scheduler::timer_event();
if (servo_fd == -1) {
servo_fd_open();
} else {
servo_send();
}
while (recvfrom(mc_fd, (void*)&state, sizeof(state), MSG_WAITALL, (sockaddr *)&in_addr, &len) != sizeof(state)) {
// nop
}
if (state.timestamp_us < _sitl->state.timestamp_us) {
// main process has rebooted
base_time_us += (_sitl->state.timestamp_us - state.timestamp_us);
}
_sitl->state = state;
hal.scheduler->stop_clock(_sitl->state.timestamp_us + base_time_us);
HALSITL::Scheduler::timer_event();
if (servo_fd == -1) {
servo_fd_open();
} else {
servo_send();
}
}