diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index c0f2c8e7ad..83c13dfc09 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -188,6 +188,7 @@ void SimMCast::multicast_open(void) strerror(errno)); exit(1); } + ::printf("multicast receiver initialised\n"); } /* @@ -247,7 +248,11 @@ void SimMCast::multicast_read(void) while (recvfrom(mc_fd, (void*)&state, sizeof(state), MSG_WAITALL, (sockaddr *)&in_addr, &len) != sizeof(state)) { // nop } + if (_sitl->state.timestamp_us == 0) { + printf("Got multicast state input\n"); + } if (state.timestamp_us < _sitl->state.timestamp_us) { + printf("multicast state time reset\n"); // main process has rebooted base_time_us += (_sitl->state.timestamp_us - state.timestamp_us); } diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 2a0aec986f..f132df7eb0 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -666,6 +666,7 @@ void SITL_State::multicast_state_open(void) fprintf(stderr, "udp servo connect failed\n"); exit(1); } + ::printf("multicast initialised\n"); } /*