From 3e6e1f5c2b042accca72346273fc1755dd7670fa Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 15 Oct 2018 15:00:29 +0200
Subject: [PATCH] POSIX: use lockstep_scheduler to fake time
This integrates the lockstep_scheduler, so that the system time is set
by the mavlink HIL_SENSOR message.
This means that the speed factor is removed and the speed is entirely
given by the simulator.
---
platforms/posix/src/px4_layer/drv_hrt.cpp | 78 ++++++++-------
src/drivers/drv_hrt.h | 7 +-
src/include/visibility.h | 1 +
src/lib/cdev/posix/cdev_platform.cpp | 8 +-
src/modules/simulator/simulator_mavlink.cpp | 105 ++++----------------
5 files changed, 74 insertions(+), 125 deletions(-)
diff --git a/platforms/posix/src/px4_layer/drv_hrt.cpp b/platforms/posix/src/px4_layer/drv_hrt.cpp
index 407c9a1673..cdf6c30a54 100644
--- a/platforms/posix/src/px4_layer/drv_hrt.cpp
+++ b/platforms/posix/src/px4_layer/drv_hrt.cpp
@@ -43,14 +43,13 @@
#include <px4_workqueue.h>
#include <px4_tasks.h>
#include <drivers/drv_hrt.h>
+#include <lockstep_scheduler/lockstep_scheduler.h>
#include <semaphore.h>
#include <time.h>
#include <string.h>
-#include <inttypes.h>
#include <errno.h>
#include "hrt_work.h"
-#define SPEED_FACTOR 1
static struct sq_queue_s callout_queue;
@@ -73,6 +72,8 @@ static hrt_abstime _delay_interval = 0;
static hrt_abstime max_time = 0;
pthread_mutex_t _hrt_mutex = PTHREAD_MUTEX_INITIALIZER;
+static LockstepScheduler lockstep_scheduler;
+
static void
hrt_call_invoke(void);
@@ -81,6 +82,12 @@ _hrt_absolute_time_internal(void);
__EXPORT hrt_abstime hrt_reset(void);
+
+hrt_abstime hrt_absolute_time_offset(void)
+{
+ return px4_timestart_monotonic;
+}
+
static void hrt_lock(void)
{
px4_sem_wait(&_hrt_lock);
@@ -191,7 +198,7 @@ hrt_abstime hrt_absolute_time(void)
return ret;
}
-__EXPORT hrt_abstime hrt_reset(void)
+hrt_abstime hrt_reset(void)
{
#ifndef __PX4_QURT
px4_timestart_monotonic = 0;
@@ -203,7 +210,7 @@ __EXPORT hrt_abstime hrt_reset(void)
/*
* Convert a timespec to absolute time.
*/
-hrt_abstime ts_to_abstime(struct timespec *ts)
+hrt_abstime ts_to_abstime(const struct timespec *ts)
{
hrt_abstime result;
@@ -594,61 +601,62 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
// Don't do any offseting on the Linux side on the Snapdragon.
return clock_gettime(clk_id, &tp);
#else
- struct timespec actual_tp;
- const int ret = clock_gettime(clk_id, &actual_tp);
-
- if (ret != 0) {
- return ret;
- }
-
- const uint64_t actual_usec = ts_to_abstime(&actual_tp);
-
if (clk_id == CLOCK_MONOTONIC) {
- if (px4_timestart_monotonic == 0) {
- px4_timestart_monotonic = actual_usec;
- }
- abstime_to_ts(tp, (actual_usec - px4_timestart_monotonic) * SPEED_FACTOR);
+ const uint64_t abstime = lockstep_scheduler.get_absolute_time();
+ abstime_to_ts(tp, abstime - px4_timestart_monotonic);
+ return 0;
} else {
- abstime_to_ts(tp, actual_usec);
+ return system_clock_gettime(clk_id, tp);
}
- //static unsigned counter = 0;
- //if (counter++ % 1000000 == 0) {
- // PX4_INFO("clk_id: %d, actual_tp.tv_sec: %llu, actual_tp.tv_nsec: %llu, px4_timestart: %llu: 0x%x",
- // clk_id, actual_tp.tv_sec, actual_tp.tv_nsec, px4_timestart[clk_id], &px4_timestart[clk_id]);
- //}
-
- return 0;
#endif
}
-int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
+int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)
{
- // not implemented
- return 0;
+ if (clk_id == CLOCK_REALTIME) {
+ 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);
+ return 0;
+ }
}
int px4_usleep(useconds_t usec)
{
- return system_usleep(usec / SPEED_FACTOR);
+ if (px4_timestart_monotonic == 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;
+
+ return lockstep_scheduler.usleep_until(time_finished);
}
unsigned int px4_sleep(unsigned int seconds)
{
useconds_t usec = seconds * 1000000;
- return system_usleep(usec / SPEED_FACTOR);
+ return px4_usleep(usec);
}
int px4_pthread_cond_timedwait(pthread_cond_t *cond,
pthread_mutex_t *mutex,
- const struct timespec *abstime)
+ const struct timespec *ts)
{
- struct timespec new_abstime;
- new_abstime.tv_sec = abstime->tv_sec / SPEED_FACTOR;
- new_abstime.tv_nsec = abstime->tv_nsec / SPEED_FACTOR;
- return pthread_cond_timedwait(cond, mutex, &new_abstime);
+ 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);
}
diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
index ac0694aba9..c50494afce 100644
--- a/src/drivers/drv_hrt.h
+++ b/src/drivers/drv_hrt.h
@@ -85,7 +85,7 @@ __EXPORT extern hrt_abstime hrt_absolute_time(void);
/**
* Convert a timespec to absolute time.
*/
-__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
+__EXPORT extern hrt_abstime ts_to_abstime(const struct timespec *ts);
/**
* Convert absolute time to a timespec.
@@ -191,6 +191,11 @@ __EXPORT extern void hrt_stop_delay(void);
*/
__EXPORT extern void hrt_stop_delay_delta(hrt_abstime delta);
+
+__EXPORT extern hrt_abstime hrt_reset(void);
+
+__EXPORT extern hrt_abstime hrt_absolute_time_offset(void);
+
#endif
__END_DECLS
diff --git a/src/include/visibility.h b/src/include/visibility.h
index 91445679dd..05b0ea5f83 100644
--- a/src/include/visibility.h
+++ b/src/include/visibility.h
@@ -97,6 +97,7 @@
// We don't poison usleep and sleep on NuttX because it is used in dependencies
// like uavcan and we don't need to fake time on the real system.
#include <unistd.h>
+#include <time.h>
#define system_usleep usleep
#define system_sleep sleep
diff --git a/src/lib/cdev/posix/cdev_platform.cpp b/src/lib/cdev/posix/cdev_platform.cpp
index 8418f6c4ca..5202dd2b50 100644
--- a/src/lib/cdev/posix/cdev_platform.cpp
+++ b/src/lib/cdev/posix/cdev_platform.cpp
@@ -384,19 +384,19 @@ extern "C" {
// Get the current time
struct timespec ts;
- // FIXME: check if QURT should probably be using CLOCK_MONOTONIC
- px4_clock_gettime(CLOCK_REALTIME, &ts);
+ // px4_sem_timedwait is implemented using CLOCK_MONOTONIC.
+ px4_clock_gettime(CLOCK_MONOTONIC, &ts);
// Calculate an absolute time in the future
const unsigned billion = (1000 * 1000 * 1000);
- unsigned tdiff = timeout;
- uint64_t nsecs = ts.tv_nsec + (tdiff * 1000 * 1000);
+ uint64_t nsecs = ts.tv_nsec + (timeout * 1000 * 1000);
ts.tv_sec += nsecs / billion;
nsecs -= (nsecs / billion) * billion;
ts.tv_nsec = nsecs;
// Execute a blocking wait for that time in the future
errno = 0;
+
ret = px4_sem_timedwait(&sem, &ts);
#ifndef __PX4_DARWIN
ret = errno;
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index d58430daf2..3dea43e879 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -49,8 +49,6 @@
#include <limits>
-extern "C" __EXPORT hrt_abstime hrt_reset(void);
-
#define SEND_INTERVAL 20
#define UDP_PORT 14560
@@ -79,7 +77,7 @@ using namespace simulator;
void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &msg, unsigned index)
{
- msg.time_usec = hrt_absolute_time();
+ msg.time_usec = hrt_absolute_time() + hrt_absolute_time_offset();
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
@@ -286,69 +284,27 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
- bool compensation_enabled = (imu.time_usec > 0);
-
// set temperature to a decent value
imu.temperature = 32.0f;
struct timespec ts;
- // clock_gettime(CLOCK_MONOTONIC, &ts);
- // uint64_t host_time = ts_to_abstime(&ts);
-
- hrt_abstime curr_sitl_time = hrt_absolute_time();
- hrt_abstime curr_sim_time = imu.time_usec;
-
- if (compensation_enabled && _initialized
- && _last_sim_timestamp > 0 && _last_sitl_timestamp > 0
- && _last_sitl_timestamp < curr_sitl_time
- && _last_sim_timestamp < curr_sim_time) {
-
- px4_clock_gettime(CLOCK_MONOTONIC, &ts);
- uint64_t timestamp = ts_to_abstime(&ts);
-
- perf_set_elapsed(_perf_sim_delay, timestamp - curr_sim_time);
- perf_count(_perf_sim_interval);
-
- int64_t dt_sitl = curr_sitl_time - _last_sitl_timestamp;
- int64_t dt_sim = curr_sim_time - _last_sim_timestamp;
-
- double curr_factor = ((double)dt_sim / (double)dt_sitl);
-
- if (curr_factor < 5.0) {
- _realtime_factor = _realtime_factor * 0.99 + 0.01 * curr_factor;
- }
-
- // calculate how much the system needs to be delayed
- int64_t sysdelay = dt_sitl - dt_sim;
-
- unsigned min_delay = 200;
-
- if (dt_sitl < 1e5
- && dt_sim < 1e5
- && sysdelay > min_delay + 100) {
+ abstime_to_ts(&ts, imu.time_usec);
+ px4_clock_settime(CLOCK_MONOTONIC, &ts);
- // the correct delay is exactly the scale between
- // the last two intervals
- px4_sim_start_delay();
- hrt_start_delay();
+ hrt_abstime now_us = hrt_absolute_time();
- unsigned exact_delay = sysdelay / _realtime_factor;
- unsigned usleep_delay = (sysdelay - min_delay) / _realtime_factor;
+#if 0
+ // This is just for to debug missing HIL_SENSOR messages.
+ static hrt_abstime last_time = 0;
+ hrt_abstime diff = now_us - last_time;
+ float step = diff / 4000.0f;
- // extend by the realtime factor to avoid drift
- px4_usleep(usleep_delay);
- hrt_stop_delay_delta(exact_delay);
- px4_sim_stop_delay();
- }
+ if (step > 1.1f || step < 0.9f) {
+ PX4_INFO("diff: %llu, step: %.2f", diff, step);
}
- hrt_abstime now = hrt_absolute_time();
-
- _last_sitl_timestamp = curr_sitl_time;
- _last_sim_timestamp = curr_sim_time;
-
- // correct timestamp
- imu.time_usec = now;
+ last_time = now_us;
+#endif
if (publish) {
publish_sensor_topics(&imu);
@@ -363,21 +319,23 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
- if (!armed || batt_sim_start == 0 || batt_sim_start > now) {
- batt_sim_start = now;
+ if (!armed || batt_sim_start == 0 || batt_sim_start > now_us) {
+ batt_sim_start = now_us;
}
float ibatt = -1.0f; // no current sensor in simulation
const float minimum_percentage = 0.5f; // change this value if you want to simulate low battery reaction
/* Simulate the voltage of a linearly draining battery but stop at the minimum percentage */
- float battery_percentage = (now - batt_sim_start) / discharge_interval_us;
+ float battery_percentage = 1.0f - (now_us - batt_sim_start) / discharge_interval_us;
+
battery_percentage = math::min(battery_percentage, minimum_percentage);
float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.full_cell_voltage(), _battery.empty_cell_voltage());
vbatt *= _battery.cell_count();
const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
- _battery.updateBatteryStatus(now, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status);
+ _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status);
+
// publish the battery voltage
int batt_multi;
@@ -734,17 +692,12 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..", udp_port);
fflush(stdout);
- uint64_t pstart_time = 0;
-
bool no_sim_data = true;
while (!px4_exit_requested() && no_sim_data) {
pret = ::poll(&fds[0], fd_count, 100);
if (fds[0].revents & POLLIN) {
- if (pstart_time == 0) {
- pstart_time = hrt_system_time();
- }
len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
// send hearbeat
@@ -764,7 +717,7 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
// have a message, handle it
handle_message(&msg, publish);
- if (msg.msgid != 0 && (hrt_system_time() - pstart_time > 1000000)) {
+ if (msg.msgid != 0) {
PX4_INFO("Got initial simulation data, running sim..");
no_sim_data = false;
}
@@ -778,9 +731,6 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
return;
}
- // reset system time
- (void)hrt_reset();
-
// subscribe to topics
for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
_actuator_outputs_sub[i] = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
@@ -794,8 +744,6 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
mavlink_status_t udp_status = {};
- bool sim_delay = false;
-
const unsigned max_wait_ms = 4;
//send MAV_CMD_SET_MESSAGE_INTERVAL for HIL_STATE_QUATERNION ground truth
@@ -817,23 +765,10 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
//timed out
if (pret == 0) {
- if (!sim_delay) {
- // we do not want to spam the console by default
- // PX4_WARN("mavlink sim timeout for %d ms", max_wait_ms);
- sim_delay = true;
- px4_sim_start_delay();
- hrt_start_delay();
- }
continue;
}
- if (sim_delay) {
- sim_delay = false;
- hrt_stop_delay();
- px4_sim_stop_delay();
- }
-
// this is undesirable but not much we can do
if (pret < 0) {
PX4_WARN("simulator mavlink: poll error %d, %d", pret, errno);
--
GitLab