autotest: use common frame time handling

Tools/autotest/pysim/aircraft.py

@@ -60,4 +60,30 @@ class Aircraft(object):
     def time_advance(self, deltat):
         '''advance time by deltat in seconds'''
         self.time_now += deltat
-        
+
+    def setup_frame_time(self, rate, speedup):
+        '''setup frame_time calculation'''
+        self.rate = rate
+        self.speedup = speedup
+        self.frame_time = 1.0/rate
+        self.scaled_frame_time = self.frame_time/speedup
+        self.last_wall_time = time.time()
+        self.achieved_rate = rate
+
+    def sync_frame_time(self):
+        '''try to synchronise simulation time with wall clock time, taking
+        into account desired speedup'''
+        now = time.time()
+        if now < self.last_wall_time + self.scaled_frame_time:
+            time.sleep(self.last_wall_time+self.scaled_frame_time - now)
+            now = time.time()
+
+        if now > self.last_wall_time and now - self.last_wall_time < 0.1:
+            rate = 1.0/(now - self.last_wall_time)
+            self.achieved_rate = (0.98*self.achieved_rate) + (0.02*rate)
+            if self.achieved_rate < self.rate*self.speedup:
+                self.scaled_frame_time *= 0.999
+            else:
+                self.scaled_frame_time *= 1.001
+
+        self.last_wall_time = now

Tools/autotest/pysim/sim_multicopter.py

@@ -171,9 +171,6 @@ print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % (
     a.home_altitude,
     a.yaw))
 
-frame_time = 1.0/opts.rate
-scaled_frame_time = frame_time/opts.speedup
-
 if opts.gimbal:
     from gimbal import Gimbal3Axis
     gimbal = Gimbal3Axis(a)
@@ -181,7 +178,8 @@ if opts.gimbal:
 else:
     gimbal = None
 
-last_wall_time = time.time()
+a.setup_frame_time(opts.rate, opts.speedup)
+counter = 0
 
 while True:
     frame_start = a.time_now
@@ -195,11 +193,13 @@ while True:
         gimbal.update()
     sim_send(m, a)
 
-    now = time.time()
-    if now < last_wall_time + scaled_frame_time:
-        time.sleep(last_wall_time+scaled_frame_time - now)
-    last_wall_time = time.time()
+    a.sync_frame_time()
 
     if frame_start == a.time_now:
         # time has not been advanced by a.update()
-        a.time_advance(frame_time)
+        a.time_advance(a.frame_time)
+
+    counter += 1
+    if counter == 1000:
+        print("t=%f speedup=%.1f" % ((a.time_now - a.time_base), a.achieved_rate/a.rate))
+        counter = 0

Tools/autotest/pysim/sim_rover.py

@@ -126,22 +126,18 @@ a.longitude = a.home_longitude
 
 a.set_yaw_degrees(a.yaw)
 
-print("Starting at lat=%f lon=%f alt=%f heading=%.1f" % (
+print("Starting at lat=%f lon=%f alt=%f heading=%.1f rate=%.1f" % (
     a.home_latitude,
     a.home_longitude,
     a.altitude,
-    a.yaw))
+    a.yaw,
+    opts.rate))
 
-frame_time = 1.0/opts.rate
-scaled_frame_time = frame_time/opts.speedup
-
-last_wall_time = time.time()
+a.setup_frame_time(opts.rate, opts.speedup)
 
 counter = 0
 
 while True:
-    frame_start = a.time_now
-
     # receive control input from SITL
     sim_recv(state)
 
@@ -151,14 +147,10 @@ while True:
     # send model state to SITL
     sim_send(a)
 
-    now = time.time()
-    if now < last_wall_time + scaled_frame_time:
-        time.sleep(last_wall_time+scaled_frame_time - now)
-    last_wall_time = time.time()
-
-    a.time_advance(frame_time)
+    a.sync_frame_time()
+    a.time_advance(a.frame_time)
 
     counter += 1
     if counter == 1000:
-        #print("t=%f %s %f" % ((a.time_now - a.time_base), time.ctime(), frame_time))
+        print("t=%f speedup=%.1f" % ((a.time_now - a.time_base), a.achieved_rate/a.rate))
         counter = 0