From 1c0dd8ba497d109fb9cd3a82f4526b9e466d6c2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Aug 2017 12:06:56 +0200 Subject: [PATCH] Simulator: Add scaling API to adjust for slow simulators The simulation engine had the ability to pause already and properly handled load spikes, however, it was not hardened against constant drift. This addition enables it to run at a constant slower-than-realtime rate successfully. --- src/drivers/drv_hrt.h | 5 ++ src/modules/simulator/simulator_mavlink.cpp | 60 +++++++++++++-------- src/platforms/posix/px4_layer/drv_hrt.c | 24 ++++++++- 3 files changed, 67 insertions(+), 22 deletions(-) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index befbfe799d..61bb5dfe1e 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -186,6 +186,11 @@ __EXPORT extern void hrt_start_delay(void); */ __EXPORT extern void hrt_stop_delay(void); +/** + * Stop to delay the HRT, but with an exact delta time. + */ +__EXPORT extern void hrt_stop_delay_delta(hrt_abstime delta); + #endif __END_DECLS diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index dd1602dee5..c74bc30025 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -284,50 +284,70 @@ 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; - uint64_t sim_timestamp = imu.time_usec; struct timespec ts; - px4_clock_gettime(CLOCK_MONOTONIC, &ts); - uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; - - perf_set_elapsed(_perf_sim_delay, timestamp - sim_timestamp); - perf_count(_perf_sim_interval); + // 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 (_last_sim_timestamp > 0 && _last_sitl_timestamp > 0 + if (compensation_enabled && _initialized + && _last_sim_timestamp > 0 && _last_sitl_timestamp > 0 && _last_sitl_timestamp < curr_sitl_time && _last_sim_timestamp < curr_sim_time) { - hrt_abstime dt_sitl = curr_sitl_time - _last_sitl_timestamp; - hrt_abstime dt_sim = curr_sim_time - _last_sim_timestamp; + px4_clock_gettime(CLOCK_MONOTONIC, &ts); + uint64_t timestamp = ts_to_abstime(&ts); - _realtime_factor = dt_sim / dt_sitl; + 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 - hrt_abstime sysdelay = dt_sitl - dt_sim; + int64_t sysdelay = dt_sitl - dt_sim; - if (_initialized - && _realtime_factor < 1.0f - && dt_sitl < 1e4 - && dt_sim < 1e4) { + 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(); - usleep(sysdelay); - hrt_stop_delay(); + + unsigned exact_delay = sysdelay / _realtime_factor; + unsigned usleep_delay = (sysdelay - min_delay) / _realtime_factor; + + // extend by the realtime factor to avoid drift + usleep(usleep_delay); + hrt_stop_delay_delta(exact_delay); px4_sim_stop_delay(); } } + hrt_abstime now = hrt_absolute_time(); + _last_sitl_timestamp = curr_sitl_time; _last_sim_timestamp = curr_sim_time; + // correct timestamp + imu.time_usec = now; + if (publish) { publish_sensor_topics(&imu); } @@ -335,8 +355,6 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) update_sensors(&imu); // battery simulation - hrt_abstime now = hrt_absolute_time(); - const float discharge_interval_us = 60 * 1000 * 1000; bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); @@ -787,7 +805,7 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) bool sim_delay = false; - const unsigned max_wait_ms = 3; + const unsigned max_wait_ms = 4; //send MAV_CMD_SET_MESSAGE_INTERVAL for HIL_STATE_QUATERNION ground truth mavlink_command_long_t cmd_long = {}; @@ -809,8 +827,8 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port) // we do not want to spam the console by default // PX4_WARN("mavlink sim timeout for %d ms", max_wait_ms); sim_delay = true; - hrt_start_delay(); px4_sim_start_delay(); + hrt_start_delay(); } continue; diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index fb5133d41f..3f61cd6735 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -332,6 +332,28 @@ void hrt_start_delay() pthread_mutex_unlock(&_hrt_mutex); } +void hrt_stop_delay_delta(hrt_abstime delta) +{ + pthread_mutex_lock(&_hrt_mutex); + + uint64_t delta_measured = _hrt_absolute_time_internal() - _start_delay_time; + + if (delta_measured < delta) { + //PX4_WARN("Slim slowdown inaccurate: (slowdown delay: %" PRIu64 " us, true: %" PRIu64 " us)", delta, delta_measured); + delta = delta_measured; + } + + _delay_interval += delta; + _start_delay_time = 0; + + if (delta > 100000) { + PX4_WARN("Computer load temporarily too high for real-time simulation. (slowdown delay: %" PRIu64 " us)", delta); + } + + pthread_mutex_unlock(&_hrt_mutex); + +} + void hrt_stop_delay() { pthread_mutex_lock(&_hrt_mutex); @@ -340,7 +362,7 @@ void hrt_stop_delay() _start_delay_time = 0; if (delta > 100000) { - PX4_INFO("Computer load temporarily too high for real-time simulation. (slowdown delay: %" PRIu64 " us)", delta); + PX4_WARN("Computer load temporarily too high for real-time simulation. (slowdown delay: %" PRIu64 " us)", delta); } pthread_mutex_unlock(&_hrt_mutex);