autotest: run rover and copter with synthetic clock

Tools/autotest/apmrover2.py

@@ -83,8 +83,17 @@ def drive_APMrover2(viewerip=None, map=False):
     if map:
         options += ' --map'
 
+    sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=200 --nowait --home=%f,%f,%u,%u' % (
+        HOME.lat, HOME.lng, HOME.alt, HOME.heading)
+
     sil = util.start_SIL('APMrover2', wipe=True)
-    mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
+    mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options, synthetic_clock=True)
+
+    runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
+    runsim.delaybeforesend = 0
+    runsim.expect('Starting at lat')
+
+    print("WAITING FOR PARAMETERS")
     mavproxy.expect('Received [0-9]+ parameters')
 
     # setup test parameters
@@ -94,8 +103,15 @@ def drive_APMrover2(viewerip=None, map=False):
     # restart with new parms
     util.pexpect_close(mavproxy)
     util.pexpect_close(sil)
+    util.pexpect_close(runsim)
+
+    sil = util.start_SIL('APMrover2')
+    mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options, synthetic_clock=True)
+    mavproxy.expect('Logging to (\S+)')
+    logfile = mavproxy.match.group(1)
+    print("LOGFILE %s" % logfile)
 
-    sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=50 --home=%f,%f,%u,%u' % (
+    sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=200 --nowait --home=%f,%f,%u,%u' % (
         HOME.lat, HOME.lng, HOME.alt, HOME.heading)
 
     runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
@@ -103,12 +119,6 @@ def drive_APMrover2(viewerip=None, map=False):
     util.pexpect_autoclose(runsim)
     runsim.expect('Starting at lat')
 
-    sil = util.start_SIL('APMrover2')
-    mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
-    mavproxy.expect('Logging to (\S+)')
-    logfile = mavproxy.match.group(1)
-    print("LOGFILE %s" % logfile)
-
     buildlog = util.reltopdir("../buildlogs/APMrover2-test.tlog")
     print("buildlog=%s" % buildlog)
     if os.path.exists(buildlog):

Tools/autotest/arducopter.py

@@ -880,7 +880,7 @@ def fly_ArduCopter(viewerip=None, map=False):
     if TARGET != 'sitl':
         util.build_SIL('ArduCopter', target=TARGET)
 
-    sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
+    sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --nowait --nowait --rate=800 --home=%f,%f,%u,%u' % (
         FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
     sim_cmd += ' --wind=6,45,.3'
     if viewerip:
@@ -888,6 +888,9 @@ def fly_ArduCopter(viewerip=None, map=False):
 
     sil = util.start_SIL('ArduCopter', wipe=True)
     mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
+    sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
+    sim.delaybeforesend = 0
+    util.pexpect_autoclose(sim)
     mavproxy.expect('Received [0-9]+ parameters')
 
     # setup test parameters
@@ -897,11 +900,9 @@ def fly_ArduCopter(viewerip=None, map=False):
     # reboot with new parameters
     util.pexpect_close(mavproxy)
     util.pexpect_close(sil)
+    util.pexpect_close(sim)
 
     sil = util.start_SIL('ArduCopter', height=HOME.alt)
-    sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
-    sim.delaybeforesend = 0
-    util.pexpect_autoclose(sim)
     options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
     if viewerip:
         options += ' --out=%s:14550' % viewerip
@@ -912,6 +913,10 @@ def fly_ArduCopter(viewerip=None, map=False):
     logfile = mavproxy.match.group(1)
     print("LOGFILE %s" % logfile)
 
+    sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
+    sim.delaybeforesend = 0
+    util.pexpect_autoclose(sim)
+
     buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
     print("buildlog=%s" % buildlog)
     copyTLog = False
@@ -1247,13 +1252,16 @@ def fly_CopterAVC(viewerip=None, map=False):
     if TARGET != 'sitl':
         util.build_SIL('ArduCopter', target=TARGET)
 
-    sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
+    sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --nowait --home=%f,%f,%u,%u' % (
         FRAME, AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
     if viewerip:
         sim_cmd += ' --fgout=%s:5503' % viewerip
 
     sil = util.start_SIL('ArduCopter', wipe=True)
     mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
+    sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
+    sim.delaybeforesend = 0
+    util.pexpect_autoclose(sim)
     mavproxy.expect('Received [0-9]+ parameters')
 
     # setup test parameters
@@ -1263,11 +1271,9 @@ def fly_CopterAVC(viewerip=None, map=False):
     # reboot with new parameters
     util.pexpect_close(mavproxy)
     util.pexpect_close(sil)
+    util.pexpect_close(sim)
 
     sil = util.start_SIL('ArduCopter', height=HOME.alt)
-    sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
-    sim.delaybeforesend = 0
-    util.pexpect_autoclose(sim)
     options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
     if viewerip:
         options += ' --out=%s:14550' % viewerip
@@ -1278,6 +1284,10 @@ def fly_CopterAVC(viewerip=None, map=False):
     logfile = mavproxy.match.group(1)
     print("LOGFILE %s" % logfile)
 
+    sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
+    sim.delaybeforesend = 0
+    util.pexpect_autoclose(sim)
+
     buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
     print("buildlog=%s" % buildlog)
     if os.path.exists(buildlog):

Tools/autotest/pysim/aircraft.py

@@ -1,4 +1,4 @@
-import math, util, rotmat
+import math, util, rotmat, time
 from rotmat import Vector3, Matrix3
 
 class Aircraft(object):
@@ -27,6 +27,8 @@ class Aircraft(object):
         self.accelerometer = Vector3(0, 0, -self.gravity)
 
         self.wind = util.Wind('0,0,0')
+        self.time_base = time.time()
+        self.time_now = self.time_base + 100*1.0e-6
 
     def on_ground(self, position=None):
         '''return true if we are on the ground'''
@@ -55,3 +57,7 @@ class Aircraft(object):
         yaw = math.radians(yaw_degrees)
         self.dcm.from_euler(roll, pitch, yaw)
         
+    def time_advance(self, deltat):
+        '''advance time by deltat in seconds'''
+        self.time_now += deltat
+        

Tools/autotest/pysim/multicopter.py

@@ -103,7 +103,7 @@ class MultiCopter(Aircraft):
         # to hover against gravity when each motor is at hover_throttle
         self.thrust_scale = (self.mass * self.gravity) / (len(self.motors) * self.hover_throttle)
 
-        self.last_time = time.time()
+        self.last_time = self.time_now
 
     def update(self, servos):
         for i in range(0, len(self.motors)):
@@ -117,7 +117,7 @@ class MultiCopter(Aircraft):
         m = self.motor_speed
 
         # how much time has passed?
-        t = time.time()
+        t = self.time_now
         delta_time = t - self.last_time
         self.last_time = t
 

Tools/autotest/pysim/rover.py

@@ -26,7 +26,7 @@ class Rover(Aircraft):
         self.wheelbase = wheelbase
         self.wheeltrack = wheeltrack
         self.max_wheel_turn = max_wheel_turn
-        self.last_time = time.time()
+        self.last_time = self.time_now
         self.skid_steering = skid_steering
         self.skid_turn_rate = skid_turn_rate
         if self.skid_steering:
@@ -85,7 +85,7 @@ class Rover(Aircraft):
             throttle = state.throttle
 
         # how much time has passed?
-        t = time.time()
+        t = self.time_now
         delta_time = t - self.last_time
         self.last_time = t
 

Tools/autotest/pysim/sim_multicopter.py

@@ -38,7 +38,10 @@ def sim_send(m, a):
         if not e.errno in [ errno.ECONNREFUSED ]:
             raise
 
-    buf = struct.pack('<17dI',
+    timestamp = int((a.time_now - a.time_base) * 1e6)
+
+    buf = struct.pack('<Q17dI',
+                      timestamp,
                       a.latitude, a.longitude, a.altitude, degrees(yaw),
                       a.velocity.x, a.velocity.y, a.velocity.z,
                       a.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
@@ -61,7 +64,7 @@ def sim_recv(m):
         if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
             raise
         return
-        
+
     if len(buf) != 28:
         return
     control = list(struct.unpack('<14H', buf))
@@ -98,6 +101,7 @@ parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", def
 parser.add_option("--wind", dest="wind", help="Simulate wind (speed,direction,turbulance)", default='0,0,0')
 parser.add_option("--frame", dest="frame", help="frame type (+,X,octo)", default='+')
 parser.add_option("--gimbal", dest="gimbal", action='store_true', default=False, help="enable gimbal")
+parser.add_option("--nowait", action='store_true', help="don't pause between updates")
 
 (opts, args) = parser.parse_args()
 
@@ -120,7 +124,7 @@ fg_out.setblocking(0)
 # setup input from SITL
 sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 sim_in.bind(sim_in_address)
-sim_in.setblocking(0)
+sim_in.setblocking(1)
 
 # setup output to SITL
 sim_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -141,9 +145,6 @@ print("Simulating for frame %s" % opts.frame)
 # motors initially off
 m = [0.0] * 11
 
-lastt = time.time()
-frame_count = 0
-
 # parse home
 v = opts.home.split(',')
 if len(v) != 4:
@@ -168,7 +169,6 @@ print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % (
     a.yaw))
 
 frame_time = 1.0/opts.rate
-sleep_overhead = 0
 
 if opts.gimbal:
     from gimbal import Gimbal3Axis
@@ -177,30 +177,23 @@ if opts.gimbal:
 else:
     gimbal = None
 
+last_wall_time = time.time()
+
 while True:
-    frame_start = time.time()
+    frame_start = a.time_now
     sim_recv(m)
 
     m2 = m[:]
 
     a.update(m2)
+
     if gimbal is not None:
         gimbal.update()
     sim_send(m, a)
-    frame_count += 1
-    t = time.time()
-    if t - lastt > 1.0:
-#        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()
-    if frame_end - frame_start < frame_time:
-        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 - dt)
+
+    now = time.time()
+    if not opts.nowait and now < last_wall_time + frame_time:
+        time.sleep(last_wall_time+frame_time - now)
+    last_wall_time = time.time()
+
+    a.time_advance(frame_time)

Tools/autotest/pysim/sim_rover.py

@@ -15,7 +15,10 @@ def sim_send(a):
     earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
     (roll, pitch, yaw) = a.dcm.to_euler()
 
-    buf = struct.pack('<17dI',
+    timestamp = int((a.time_now - a.time_base) * 1e6)
+
+    buf = struct.pack('<Q17dI',
+                      timestamp,
                       a.latitude, a.longitude, a.altitude, degrees(yaw),
                       a.velocity.x, a.velocity.y, a.velocity.z,
                       a.accelerometer.x, a.accelerometer.y, a.accelerometer.z,
@@ -35,6 +38,7 @@ def sim_recv(state):
     try:
         buf = sim_in.recv(28)
     except socket.error as e:
+        print(e, e.error)
         if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
             raise
         return
@@ -49,7 +53,7 @@ def sim_recv(state):
     state.steering = (pwm[0]-1500)/500.0
     state.throttle = (pwm[2]-1500)/500.0
 
-#    print("steering=%f throttle=%f pwm=%s" % (state.steering, state.throttle, str(pwm)))
+    #print("steering=%f throttle=%f pwm=%s" % (state.steering, state.throttle, str(pwm)))
     
 
 
@@ -73,8 +77,9 @@ parser = OptionParser("sim_rover.py [options]")
 parser.add_option("--simin",  dest="simin",   help="SIM input (IP:port)",       default="127.0.0.1:5502")
 parser.add_option("--simout", dest="simout",  help="SIM output (IP:port)",      default="127.0.0.1:5501")
 parser.add_option("--home", dest="home",  type='string', default=None, help="home lat,lng,alt,hdg (required)")
-parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", default=100)
+parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", default=1000)
 parser.add_option("--skid-steering", action='store_true', default=False, help="Use skid steering")
+parser.add_option("--nowait", action='store_true', help="don't pause between updates")
 
 (opts, args) = parser.parse_args()
 
@@ -91,7 +96,7 @@ sim_in_address  = interpret_address(opts.simin)
 # setup input from SITL
 sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 sim_in.bind(sim_in_address)
-sim_in.setblocking(0)
+sim_in.setblocking(1)
 
 # setup output to SITL
 sim_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -127,18 +132,30 @@ print("Starting at lat=%f lon=%f alt=%f heading=%.1f" % (
     a.yaw))
 
 frame_time = 1.0/opts.rate
-sleep_overhead = 0
+last_wall_time = time.time()
+
+counter = 0
 
 while True:
-    frame_start = time.time()
+    frame_start = a.time_now
+
+    # receive control input from SITL
     sim_recv(state)
+
+    # update the model
     a.update(state)
+
+    # send model state to SITL
     sim_send(a)
-    t = time.time()
-    frame_end = time.time()
-    if frame_end - frame_start < frame_time:
-        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)
+
+    now = time.time()
+    if not opts.nowait and now < last_wall_time + frame_time:
+        time.sleep(last_wall_time+frame_time - now)
+    last_wall_time = time.time()
+
+    a.time_advance(frame_time)
+
+    counter += 1
+    if counter == 1000:
+        #print("t=%f %s %f" % ((a.time_now - a.time_base), time.ctime(), frame_time))
+        counter = 0

Tools/autotest/pysim/util.py

@@ -119,7 +119,7 @@ def pexpect_drain(p):
     except pexpect.TIMEOUT:
         pass
 
-def start_SIL(atype, valgrind=False, wipe=False, height=None):
+def start_SIL(atype, valgrind=False, wipe=False, height=None, synthetic_clock=False):
     '''launch a SIL instance'''
     import pexpect
     cmd=""
@@ -133,6 +133,8 @@ def start_SIL(atype, valgrind=False, wipe=False, height=None):
         cmd += ' -w'
     if height is not None:
         cmd += ' -H %u' % height
+    if synthetic_clock:
+        cmd += ' -S'
     ret = pexpect.spawn(cmd, logfile=sys.stdout, timeout=5)
     ret.delaybeforesend = 0
     pexpect_autoclose(ret)
@@ -140,7 +142,7 @@ def start_SIL(atype, valgrind=False, wipe=False, height=None):
     return ret
 
 def start_MAVProxy_SIL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:5760',
-                       options=None, logfile=sys.stdout):
+                       options=None, logfile=sys.stdout, synthetic_clock=False):
     '''launch mavproxy connected to a SIL instance'''
     import pexpect
     global close_list