SITL: improved the speedup code

more accurate sleeps using sleep debt system

libraries/SITL/SIM_Aircraft.cpp

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

libraries/SITL/SIM_Aircraft.h

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