fix(lockstep): stop SIH from passing the barrier on banked releases (#28802)

platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt

@@ -12,3 +12,4 @@ target_include_directories(lockstep_scheduler
 )
 
 px4_add_functional_gtest(SRC test/src/lockstep_scheduler_test.cpp LINKLIBS lockstep_scheduler)
+px4_add_functional_gtest(SRC test/src/lockstep_components_test.cpp LINKLIBS lockstep_scheduler)

platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h

@@ -69,6 +69,8 @@ public:
 	void wait_for_components();
 
 private:
+	void release_waiter();
+
 	const bool _no_cleanup_on_destroy;
 
 	px4_sem_t _components_sem;

platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp

@@ -91,7 +91,7 @@ void LockstepComponents::unregister_component(int component)
 
 	if (_components_progress_bitset == components_used_bitset) {
 		_components_progress_bitset = 0;
-		px4_sem_post(&_components_sem);
+		release_waiter();
 	}
 }
 
@@ -111,14 +111,19 @@ void LockstepComponents::lockstep_progress(int component)
 		// register_component and is fast enough it can land here as well, thus leading to 2 unlocks in a cycle.
 		// That is acceptable though.
 		_components_progress_bitset = 0;
+		release_waiter();
+	}
+}
 
-		// during startup it can happen that wait_for_components() is not called yet, so avoid increasing the
-		// semaphore counter more than necessary
-		int value;
+void LockstepComponents::release_waiter()
+{
+	// This also runs while nothing is waiting, e.g. before wait_for_components() is first called, or every time
+	// a work queue goes idle while no other component is registered. Each banked post would let the waiter
+	// through one barrier without waiting.
+	int value;
 
-		if (px4_sem_getvalue(&_components_sem, &value) == 0 && value < 1) {
-			px4_sem_post(&_components_sem);
-		}
+	if (px4_sem_getvalue(&_components_sem, &value) == 0 && value < 1) {
+		px4_sem_post(&_components_sem);
 	}
 }
 

platforms/posix/src/px4/common/lockstep_scheduler/test/src/lockstep_components_test.cpp

@@ -0,0 +1,66 @@
+#include <lockstep_scheduler/lockstep_components.h>
+#include <gtest/gtest.h>
+#include <atomic>
+#include <chrono>
+#include <thread>
+
+using namespace std::chrono_literals;
+
+namespace
+{
+
+class Waiter
+{
+public:
+	explicit Waiter(LockstepComponents &components)
+		: _thread([this, &components]() { components.wait_for_components(); _returned = true; })
+	{}
+
+	~Waiter() { _thread.join(); }
+
+	bool returned_within(std::chrono::milliseconds timeout)
+	{
+		const auto deadline = std::chrono::steady_clock::now() + timeout;
+
+		while (!_returned && std::chrono::steady_clock::now() < deadline) {
+			std::this_thread::sleep_for(1ms);
+		}
+
+		return _returned;
+	}
+
+private:
+	std::atomic<bool> _returned{false};
+	std::thread _thread;
+};
+
+} // namespace
+
+TEST(LockstepComponents, IdleComponentsBankAtMostOneRelease)
+{
+	LockstepComponents components;
+
+	// A work queue picking up and finishing work while nothing waits yet, as before the simulator
+	// engages lockstep.
+	for (int i = 0; i < 10; ++i) {
+		components.unregister_component(components.register_component());
+	}
+
+	const int busy = components.register_component();
+	int passed_while_busy = 0;
+
+	for (int i = 0; i < 3; ++i) {
+		Waiter waiter(components);
+
+		if (waiter.returned_within(100ms)) {
+			++passed_while_busy;
+
+		} else {
+			components.unregister_component(busy);
+			EXPECT_TRUE(waiter.returned_within(5s));
+			break;
+		}
+	}
+
+	EXPECT_LE(passed_while_busy, 1);
+}

src/modules/simulation/simulator_sih/sih.cpp

@@ -148,6 +148,13 @@ void Sih::lockstep_loop()
 			sleep_time = math::max(0, sim_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
 
 		} else {
+			// Holding a component of our own keeps the barrier closed until this step's data is out, so
+			// work queues going idle for unrelated reasons in between cannot release it.
+			if (_lockstep_component == -1) {
+				_lockstep_component = px4_lockstep_register_component();
+			}
+
+			px4_lockstep_progress(_lockstep_component);
 			px4_lockstep_wait_for_components();
 
 			// Wait for the control pipeline to produce new actuator outputs.
@@ -173,6 +180,8 @@ void Sih::lockstep_loop()
 					    current_wall_time_us - pre_compute_wall_time_us + sleep_time));
 		usleep(sleep_time);
 	}
+
+	px4_lockstep_unregister_component(_lockstep_component);
 }
 #endif
 

src/modules/simulation/simulator_sih/sih.hpp

@@ -214,6 +214,7 @@ private:
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	void lockstep_loop();
+	int _lockstep_component{-1};
 	uint64_t _current_simulation_time_us{0};
 	float _achieved_speedup{0.f};
 #endif