dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

posix lockstep remove HRT offset and use full sim time

This commit is contained in:
Daniel Agar 2022-09-01 16:52:25 -04:00
parent 3da0293369
commit c52f2143d2
3 changed files with 21 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
platforms/posix/src/px4/common/drv_hrt.cpp
View File

@ -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 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

5
src/drivers/drv_hrt.h
View File

@ -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);

2
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
View File

@ -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);