posix lockstep remove HRT offset and use full sim time

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -37,6 +37,7 @@
  * High-resolution timer with callouts and timekeeping.
  */
 
+#include <px4_platform_common/atomic.h>
 #include <px4_platform_common/time.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/defines.h>
@@ -52,6 +53,7 @@
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 #include <lockstep_scheduler/lockstep_scheduler.h>
+static LockstepScheduler lockstep_scheduler {};
 #endif
 
 // Intervals in usec
@@ -77,22 +79,11 @@ __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
 static px4_sem_t 	_hrt_lock;
 static struct work_s	_hrt_work;
 
-static hrt_abstime px4_timestart_monotonic = 0;
-
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-static LockstepScheduler *lockstep_scheduler = new LockstepScheduler();
-#endif
-
 static void hrt_latency_update();
 
 static void hrt_call_reschedule();
 static void hrt_call_invoke();
 
-hrt_abstime hrt_absolute_time_offset()
-{
-	return px4_timestart_monotonic;
-}
-
 static void hrt_lock()
 {
 	// loop as the wait may be interrupted by a signal
@@ -136,8 +127,8 @@ hrt_abstime hrt_absolute_time()
 {
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	// optimized case (avoid ts_to_abstime) if lockstep scheduler is used
-	const uint64_t abstime = lockstep_scheduler->get_absolute_time();
+	return lockstep_scheduler.get_absolute_time();
-	return abstime - px4_timestart_monotonic;
+
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 	struct timespec ts;
 	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
@@ -476,8 +467,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 {
 	if (clk_id == CLOCK_MONOTONIC) {
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
-		const uint64_t abstime = lockstep_scheduler->get_absolute_time();
+		abstime_to_ts(tp, lockstep_scheduler.get_absolute_time());
-		abstime_to_ts(tp, abstime - px4_timestart_monotonic);
 		return 0;
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 #if defined(__PX4_DARWIN)
@@ -501,71 +491,60 @@ int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)
 		return system_clock_settime(clk_id, ts);
 
 	} else {
-		const uint64_t time_us = ts_to_abstime(ts);
+		lockstep_scheduler.set_absolute_time(ts_to_abstime(ts));
-
-		if (px4_timestart_monotonic == 0) {
-			px4_timestart_monotonic = time_us;
-		}
-
-		lockstep_scheduler->set_absolute_time(time_us);
 		return 0;
 	}
 }
 
-
 int px4_usleep(useconds_t usec)
 {
-	if (px4_timestart_monotonic == 0) {
+	if (lockstep_scheduler.get_absolute_time() == 0) {
-		// Until the time is set by the simulator, we fallback to the normal
+		// Until the time is set by the simulator, we fallback to the normal usleep
-		// usleep;
 		return system_usleep(usec);
 	}
 
-	const uint64_t time_finished = lockstep_scheduler->get_absolute_time() + usec;
+	const uint64_t time_finished = lockstep_scheduler.get_absolute_time() + usec;
 
-	return lockstep_scheduler->usleep_until(time_finished);
+	return lockstep_scheduler.usleep_until(time_finished);
 }
 
 unsigned int px4_sleep(unsigned int seconds)
 {
-	if (px4_timestart_monotonic == 0) {
+	if (lockstep_scheduler.get_absolute_time() == 0) {
-		// Until the time is set by the simulator, we fallback to the normal
+		// Until the time is set by the simulator, we fallback to the normal sleep
-		// sleep;
 		return system_sleep(seconds);
 	}
 
-	const uint64_t time_finished = lockstep_scheduler->get_absolute_time() +
+	const uint64_t time_finished = lockstep_scheduler.get_absolute_time() + ((uint64_t)seconds * 1000000);
-				       ((uint64_t)seconds * 1000000);
 
-	return lockstep_scheduler->usleep_until(time_finished);
+	return lockstep_scheduler.usleep_until(time_finished);
 }
 
 int px4_pthread_cond_timedwait(pthread_cond_t *cond,
 			       pthread_mutex_t *mutex,
 			       const struct timespec *ts)
 {
-	const uint64_t time_us = ts_to_abstime(ts);
+	const uint64_t scheduled = ts_to_abstime(ts);
-	const uint64_t scheduled = time_us + px4_timestart_monotonic;
+	return lockstep_scheduler.cond_timedwait(cond, mutex, scheduled);
-	return lockstep_scheduler->cond_timedwait(cond, mutex, scheduled);
 }
 
 int px4_lockstep_register_component()
 {
-	return lockstep_scheduler->components().register_component();
+	return lockstep_scheduler.components().register_component();
 }
 
 void px4_lockstep_unregister_component(int component)
 {
-	lockstep_scheduler->components().unregister_component(component);
+	lockstep_scheduler.components().unregister_component(component);
 }
 
 void px4_lockstep_progress(int component)
 {
-	lockstep_scheduler->components().lockstep_progress(component);
+	lockstep_scheduler.components().lockstep_progress(component);
 }
 
 void px4_lockstep_wait_for_components()
 {
-	lockstep_scheduler->components().wait_for_components();
+	lockstep_scheduler.components().wait_for_components();
 }
 #endif

src/drivers/drv_hrt.h

@@ -235,11 +235,6 @@ __EXPORT extern void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
  */
 __EXPORT extern void	hrt_init(void);
 
-#ifdef __PX4_POSIX
-
-__EXPORT extern hrt_abstime hrt_absolute_time_offset(void);
-
-#endif
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 
 __EXPORT extern int px4_lockstep_register_component(void);

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -119,7 +119,7 @@ void SimulatorMavlink::actuator_controls_from_outputs(mavlink_hil_actuator_contr
 {
 	memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t));
 
-	msg->time_usec = hrt_absolute_time() + hrt_absolute_time_offset();
+	msg->time_usec = hrt_absolute_time();
 
 	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);