autotest: fixed sleep_overhead calculation in sim_multicopter.py

Tools/autotest/pysim/sim_multicopter.py

@@ -176,10 +176,11 @@ while True:
     frame_count += 1
     t = time.time()
     if t - lastt > 1.0:
-        #print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % (
-         #   frame_count/(t-lastt),
-          #  a.velocity.z, a.accel.z, a.position.z, a.altitude,
-           # a.yaw, a.yaw_rate))
+#        print("%.2f fps sleepOverhead=%f zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f" % (
+#            frame_count/(t-lastt),
+#            sleep_overhead,
+#            a.velocity.z, a.accelerometer.z, a.position.z, a.altitude,
+#            a.yaw))
         lastt = t
         frame_count = 0
     frame_end = time.time()
@@ -188,4 +189,4 @@ while True:
         dt -= sleep_overhead
         if dt > 0:
             time.sleep(dt)
-        sleep_overhead = 0.99*sleep_overhead + 0.01*(time.time() - frame_end)
+        sleep_overhead = 0.99*sleep_overhead + 0.01*(time.time() - frame_end - dt)