From 3440f543f119b71d06e2c0a47d4eb99505591661 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 16 Sep 2022 14:10:14 -0400
Subject: [PATCH] simulator_ignition_bridge: wait for first clock set before
 returning successfully

---
 .../src/lockstep_scheduler.cpp                |  6 +++++
 .../SimulatorIgnitionBridge.cpp               | 25 +++++++++++++++++++
 .../SimulatorIgnitionBridge.hpp               |  2 ++
 3 files changed, 33 insertions(+)

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 f33dc7ad64..e9eddbf1b4 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
@@ -33,6 +33,8 @@
 
 #include <lockstep_scheduler/lockstep_scheduler.h>
 
+#include <px4_platform_common/log.h>
+
 LockstepScheduler::~LockstepScheduler()
 {
 	// cleanup the linked list
@@ -47,6 +49,10 @@ LockstepScheduler::~LockstepScheduler()
 
 void LockstepScheduler::set_absolute_time(uint64_t time_us)
 {
+	if (_time_us == 0 && time_us > 0) {
+		PX4_INFO("setting initial absolute time to %lu us", time_us);
+	}
+
 	_time_us = time_us;
 
 	{
diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
index dcdffb0d36..dfa5a06342 100644
--- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
+++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
@@ -221,7 +221,32 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
 		_task_id = task_id_is_work_queue;
 
 		if (instance->init() == PX4_OK) {
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+			// lockstep scheduler wait for initial clock set before returning
+			int sleep_count_limit = 1000;
+
+			while ((instance->world_time_us() == 0) && sleep_count_limit > 0) {
+				// wait for first clock message
+				system_usleep(1000);
+				sleep_count_limit--;
+			}
+
+			if (instance->world_time_us() == 0) {
+				PX4_ERR("timed out waiting for clock message");
+				instance->request_stop();
+				instance->ScheduleNow();
+
+			} else {
+				return PX4_OK;
+			}
+
+#else
 			return PX4_OK;
+
+#endif // ENABLE_LOCKSTEP_SCHEDULER
+
+			//return PX4_OK;
 		}
 
 	} else {
diff --git a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp
index 70efe2c392..69ca753641 100644
--- a/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp
+++ b/src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp
@@ -79,6 +79,8 @@ public:
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
+	uint64_t world_time_us() const { return _world_time_us.load(); }
+
 private:
 
 	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],