From c52f2143d2ff23cfd0a3865e99c3fe69d6eaaa87 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 1 Sep 2022 16:52:25 -0400 Subject: [PATCH] posix lockstep remove HRT offset and use full sim time --- platforms/posix/src/px4/common/drv_hrt.cpp | 61 ++++++------------- src/drivers/drv_hrt.h | 5 -- .../simulator_mavlink/SimulatorMavlink.cpp | 2 +- 3 files changed, 21 insertions(+), 47 deletions(-) diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index 6253e8171e..349986ce6d 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -37,6 +37,7 @@ * High-resolution timer with callouts and timekeeping. */ +#include #include #include #include @@ -52,6 +53,7 @@ #if defined(ENABLE_LOCKSTEP_SCHEDULER) #include +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 abstime - px4_timestart_monotonic; + return lockstep_scheduler.get_absolute_time(); + #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, abstime - px4_timestart_monotonic); + abstime_to_ts(tp, lockstep_scheduler.get_absolute_time()); 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); - - if (px4_timestart_monotonic == 0) { - px4_timestart_monotonic = time_us; - } - - lockstep_scheduler->set_absolute_time(time_us); + lockstep_scheduler.set_absolute_time(ts_to_abstime(ts)); return 0; } } - int px4_usleep(useconds_t usec) { - if (px4_timestart_monotonic == 0) { - // Until the time is set by the simulator, we fallback to the normal - // usleep; + if (lockstep_scheduler.get_absolute_time() == 0) { + // Until the time is set by the simulator, we fallback to the normal 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) { - // Until the time is set by the simulator, we fallback to the normal - // sleep; + if (lockstep_scheduler.get_absolute_time() == 0) { + // Until the time is set by the simulator, we fallback to the normal sleep return system_sleep(seconds); } - const uint64_t time_finished = lockstep_scheduler->get_absolute_time() + - ((uint64_t)seconds * 1000000); + const uint64_t time_finished = lockstep_scheduler.get_absolute_time() + ((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 = time_us + px4_timestart_monotonic; - return lockstep_scheduler->cond_timedwait(cond, mutex, scheduled); + const uint64_t scheduled = ts_to_abstime(ts); + 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 diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index fb1cc0722f..242d2a2ee8 100644 --- a/src/drivers/drv_hrt.h +++ b/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); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index d9157ae071..0069814279 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/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);