diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 98756d21b7..4caf8d6276 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -309,10 +309,15 @@ WorkQueueManagerRun(int, char **) PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam); } - // schedule policy FIFO — fall back to SCHED_OTHER on platforms - // where SCHED_FIFO is not available to unprivileged threads - // (winpthreads on MinGW is one such case). The work queues - // still work correctly at SCHED_OTHER priority. +#if defined(__PX4_WINDOWS) + // Windows-only: winpthreads on MinGW does not allow SCHED_FIFO + // for unprivileged threads, so fall back to SCHED_OTHER and + // clamp the priority into that policy's valid range. The Linux + // path keeps main's byte-exact behavior (try SCHED_FIFO and let + // pthread_create end up at the kernel default if it isn't + // privileged) — without that, every WQ on a regular Linux + // CI runner ends up at SCHED_OTHER priority 0, which subtly + // changes producer/consumer ordering in lockstep SITL. int sched_policy = SCHED_FIFO; int ret_setschedpolicy = pthread_attr_setschedpolicy(&attr, sched_policy); @@ -325,9 +330,6 @@ WorkQueueManagerRun(int, char **) PX4_ERR("failed to set sched policy (%i)", ret_setschedpolicy); } - // priority — clamp to the policy's valid range, otherwise - // pthread_attr_setschedparam rejects the value and the - // thread ends up at whatever default the library picks. const int max_prio = sched_get_priority_max(sched_policy); const int min_prio = sched_get_priority_min(sched_policy); int effective_prio = sched_priority; @@ -337,6 +339,17 @@ WorkQueueManagerRun(int, char **) if (effective_prio < min_prio) { effective_prio = min_prio; } param.sched_priority = effective_prio; +#else + // schedule policy FIFO + int ret_setschedpolicy = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); + + if (ret_setschedpolicy != 0) { + PX4_ERR("failed to set sched policy SCHED_FIFO (%i)", ret_setschedpolicy); + } + + // priority + param.sched_priority = sched_priority; +#endif int ret_setschedparam = pthread_attr_setschedparam(&attr, ¶m); if (ret_setschedparam != 0) { 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 259821dc77..40e0dc44f8 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 @@ -338,16 +338,23 @@ int LockstepScheduler::cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *loc int LockstepScheduler::usleep_until(uint64_t time_us) { - // Re-use a thread-local mutex/cond pair instead of allocating a new pair - // on every call. PTHREAD_MUTEX_INITIALIZER / PTHREAD_COND_INITIALIZER - // trigger lazy allocation of the underlying kernel object on winpthreads - // (and a heap allocation on glibc); without an explicit *_destroy() the - // auto-storage variants leak on every tick. usleep_until() is on the hot - // path of every PX4 work-queue tick, so a per-call leak compounds quickly - // at high simulation speed factors. A single thread_local pair is created - // once per thread and is destroyed when the thread exits. +#if defined(__PX4_WINDOWS) || defined(_WIN32) + // On winpthreads PTHREAD_*_INITIALIZER triggers lazy allocation of + // the underlying kernel object; without an explicit *_destroy() the + // auto-storage variants leak on every tick, which at high speed + // factors quickly accumulates. Keep a thread-local pair that is + // created once per thread and destroyed at thread exit. static thread_local pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER; static thread_local pthread_cond_t cond = PTHREAD_COND_INITIALIZER; +#else + // Linux glibc treats PTHREAD_*_INITIALIZER as a static struct + // initializer with no kernel-object allocation, so per-call + // auto-storage matches main and avoids the cross-call cond + // reuse semantics that interact badly with the LockstepScheduler + // linked list when a thread re-enters usleep_until(). + pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER; + pthread_cond_t cond = PTHREAD_COND_INITIALIZER; +#endif pthread_mutex_lock(&lock);