From 5824bdb5fb3fe538f1f54bdd07aef97a441657d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 30 Jan 2026 13:25:08 +1300 Subject: [PATCH] simulator_sih: implement lockstep by waiting for outputs Without this, we see test failures in CI as soon as CPU load is higher. --- src/modules/simulation/simulator_sih/sih.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 465ada5d1f..3ed6b96a59 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -148,6 +148,22 @@ void Sih::lockstep_loop() } else { px4_lockstep_wait_for_components(); + + // Wait for the control pipeline to produce new actuator outputs. + // Without this, under CPU load the controllers may not run between + // SIH iterations, causing stale actuator data and sluggish response. + uint64_t wait_start_us = micros(); + constexpr uint64_t actuator_wait_timeout_us = 10'000'000; // 10s wall time + + while (!_actuator_out_sub.updated() && !should_exit()) { + if (micros() - wait_start_us > actuator_wait_timeout_us) { + PX4_WARN("SIH lockstep: timed out waiting for actuator_outputs_sim"); + break; + } + + usleep(100); + } + current_wall_time_us = micros(); sleep_time = math::max(0, rt_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us)); }