autotest: auto-adjust frame rate for crrcsim

Tools/autotest/pysim/aircraft.py

@@ -70,6 +70,12 @@ class Aircraft(object):
         self.last_wall_time = time.time()
         self.achieved_rate = rate
 
+    def adjust_frame_time(self, rate):
+        '''adjust frame_time calculation'''
+        self.rate = rate
+        self.frame_time = 1.0/rate
+        self.scaled_frame_time = self.frame_time/self.speedup
+
     def sync_frame_time(self):
         '''try to synchronise simulation time with wall clock time, taking
         into account desired speedup'''

Tools/autotest/pysim/crrcsim.py

@@ -19,6 +19,7 @@ class CRRCSim(Aircraft):
         self.sim.setblocking(0)
         self.accel_body = Vector3(0, 0, -self.gravity)
         self.frame = frame
+        self.last_timestamp = 0
         if self.frame.find("heli") != -1:
             self.decode_servos = self.decode_servos_heli
         else:
@@ -90,6 +91,12 @@ class CRRCSim(Aircraft):
         self.dcm.from_euler(roll, pitch, yaw)
         self.time_now = timestamp + self.time_base
 
+        # auto-adjust to crrcsim frame rate
+        deltat = timestamp - self.last_timestamp
+        if deltat < 0.01 and deltat > 0:
+            self.adjust_frame_time(1.0/deltat)
+        self.last_timestamp = timestamp
+
     def update(self, actuators):
         '''update model'''
         self.decode_servos(actuators)