Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: improved the speedup code 

more accurate sleeps using sleep debt system
This commit is contained in:
Andrew Tridgell 2020-06-04 19:00:40 +10:00
parent 750b220a5f
commit 5349113104
2 changed files with 27 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/SITL/SIM_Aircraft.cpp
View File

@ -231,21 +231,16 @@ void Aircraft::setup_frame_time(float new_rate, float new_speedup)
{
rate_hz = new_rate;
target_speedup = new_speedup;
frame_time_us = static_cast<uint64_t>(1.0e6f/rate_hz);
frame_time_us = uint64_t(1.0e6f/rate_hz);
scaled_frame_time_us = frame_time_us/target_speedup;
last_wall_time_us = get_wall_time_us();
achieved_rate_hz = rate_hz;
}
/* adjust frame_time calculation */
void Aircraft::adjust_frame_time(float new_rate)
{
if (!is_equal(rate_hz, new_rate)) {
rate_hz = new_rate;
frame_time_us = static_cast<uint64_t>(1.0e6f/rate_hz);
scaled_frame_time_us = frame_time_us/target_speedup;
}
frame_time_us = uint64_t(1.0e6f/new_rate);
rate_hz = new_rate;
}
/*
@ -258,31 +253,29 @@ void Aircraft::sync_frame_time(void)
{
frame_counter++;
uint64_t now = get_wall_time_us();
if (frame_counter >= 40 &&
now > last_wall_time_us) {
const float rate = frame_counter * 1.0e6f/(now - last_wall_time_us);
achieved_rate_hz = (0.99f*achieved_rate_hz) + (0.01f * rate);
if (achieved_rate_hz < rate_hz * target_speedup) {
scaled_frame_time_us *= 0.999f;
} else {
scaled_frame_time_us /= 0.999f;
}
#if 0
::printf("achieved_rate_hz=%.3f rate=%.2f rate_hz=%.3f sft=%.1f\n",
static_cast<double>(achieved_rate_hz),
static_cast<double>(rate),
static_cast<double>(rate_hz),
static_cast<double>(scaled_frame_time_us));
#endif
const uint32_t sleep_time = static_cast<uint32_t>(scaled_frame_time_us * frame_counter);
if (sleep_time > min_sleep_time) {
usleep(sleep_time);
}
last_wall_time_us = now;
frame_counter = 0;
uint64_t dt_us = now - last_wall_time_us;
const float target_dt_us = 1.0e6/(rate_hz*target_speedup);
// accumulate sleep debt if we're running too fast
sleep_debt_us += target_dt_us - dt_us;
if (sleep_debt_us < -1.0e5) {
// don't let a large negative debt build up
sleep_debt_us = -1.0e5;
}
uint32_t now_ms = now / 1000ULL;
if (now_ms - last_fps_report_ms > 2000) {
if (sleep_debt_us > min_sleep_time) {
// sleep if we have built up a debt of min_sleep_tim
usleep(sleep_debt_us);
sleep_debt_us -= (get_wall_time_us() - now);
}
last_wall_time_us = get_wall_time_us();
uint32_t now_ms = last_wall_time_us / 1000ULL;
float dt_wall = (now_ms - last_fps_report_ms) * 0.001;
if (dt_wall > 2.0) {
const float achieved_rate_hz = (frame_counter - last_frame_count) / dt_wall;
last_frame_count = frame_counter;
last_fps_report_ms = now_ms;
::printf("Rate: target:%.1f achieved:%.1f speedup %.1f/%.1f\n",
rate_hz*target_speedup, achieved_rate_hz,

4
libraries/SITL/SIM_Aircraft.h
View File

@ -188,12 +188,12 @@ protected:
const float gyro_noise;
const float accel_noise;
float rate_hz;
float achieved_rate_hz;
float target_speedup;
uint64_t frame_time_us;
float scaled_frame_time_us;
uint64_t last_wall_time_us;
uint32_t last_fps_report_ms;
int64_t sleep_debt_us;
uint32_t last_frame_count;
uint8_t instance;
const char *autotest_dir;
const char *frame;