forked from Archive/PX4-Autopilot
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.
This commit is contained in:
Julian Oes
parent
f0ce300744
commit
3e6e1f5c2b
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
abstime_to_ts(&ts, imu.time_usec);
|
||||
px4_clock_settime(CLOCK_MONOTONIC, &ts);
|
||||
|
||||
hrt_abstime curr_sitl_time = hrt_absolute_time();
|
||||
hrt_abstime curr_sim_time = imu.time_usec;
|
||||
hrt_abstime now_us = hrt_absolute_time();
|
||||
|
||||
if (compensation_enabled && _initialized
|
||||
&& _last_sim_timestamp > 0 && _last_sitl_timestamp > 0
|
||||
&& _last_sitl_timestamp < curr_sitl_time
|
||||
&& _last_sim_timestamp < curr_sim_time) {
|
||||
#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;
|
||||
|
||||
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) {
|
||||
|
||||
// the correct delay is exactly the scale between
|
||||
// the last two intervals
|
||||
px4_sim_start_delay();
|
||||
hrt_start_delay();
|
||||
|
||||
unsigned exact_delay = sysdelay / _realtime_factor;
|
||||
unsigned usleep_delay = (sysdelay - min_delay) / _realtime_factor;
|
||||
|
||||
// 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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue