From 9b3ce4a96afece92e79775b1d46eccd2579f242d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Dec 2011 07:38:51 +1100 Subject: [PATCH] autotest: estimate the cost of sleeping for more accurate frame rate this gets us quite close to 1kHz simulation --- Tools/autotest/pysim/sim_quad.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/pysim/sim_quad.py b/Tools/autotest/pysim/sim_quad.py index 7e5006dd53..46f2e181a0 100755 --- a/Tools/autotest/pysim/sim_quad.py +++ b/Tools/autotest/pysim/sim_quad.py @@ -150,7 +150,7 @@ print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % ( a.yaw)) frame_time = 1.0/opts.rate - +sleep_overhead = 0 while True: frame_start = time.time() @@ -171,4 +171,8 @@ while True: frame_count = 0 frame_end = time.time() if frame_end - frame_start < frame_time: - time.sleep(frame_time - (frame_end - frame_start)) + dt = frame_time - (frame_end - frame_start) + dt -= sleep_overhead + if dt > 0: + time.sleep(dt) + sleep_overhead = 0.99*sleep_overhead + 0.01*(time.time() - frame_end)