diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp index 25d5479da2..259821dc77 100644 --- a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp +++ b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp @@ -306,42 +306,7 @@ int LockstepScheduler::cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *loc pthread_mutex_lock(lock); #else - - while (!timed_wait.timeout) { - struct timespec poll_deadline; - clock_gettime(CLOCK_REALTIME, &poll_deadline); - long total_ns = poll_deadline.tv_nsec + 500L * 1000L * 1000L; - poll_deadline.tv_sec += total_ns / 1000000000L; - poll_deadline.tv_nsec = total_ns % 1000000000L; - - result = pthread_cond_timedwait(cond, lock, &poll_deadline); - - if (timed_wait.timeout) { - break; - } - - if (result == ETIMEDOUT) { - // Watchdog fired; re-check _time_us in case we lost the - // broadcast. _timed_waits_mutex is taken here only to - // synchronise with set_absolute_time. - _timed_waits_mutex.lock(); - - if (time_us <= _time_us) { - timed_wait.timeout = true; - } - - _timed_waits_mutex.unlock(); - continue; - } - - if (result != 0) { - break; - } - - // Spurious wakeup or real signal not reflected in `timeout` - // (shouldn't happen normally). Loop to re-check. - } - + result = pthread_cond_wait(cond, lock); #endif const bool timeout = timed_wait.timeout; @@ -350,13 +315,6 @@ int LockstepScheduler::cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *loc result = ETIMEDOUT; } - if (result == ETIMEDOUT && !timeout) { - // pthread_cond_timedwait returned ETIMEDOUT but broadcast also - // flipped `timeout` true between our check and this moment — we - // re-enter the loop above so this branch is effectively dead. - result = 0; - } - timed_wait.done = true; if (!timeout && _setting_time) {