From 5be56a824d7c733cb7236802a59bdbc12d8df14e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Dec 2011 07:24:58 +1100 Subject: [PATCH] autotest: run the quad simulation at a much higher rate it now defaults to 1kHz --- Tools/autotest/pysim/sim_quad.py | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/Tools/autotest/pysim/sim_quad.py b/Tools/autotest/pysim/sim_quad.py index 641f1fd559..7e5006dd53 100755 --- a/Tools/autotest/pysim/sim_quad.py +++ b/Tools/autotest/pysim/sim_quad.py @@ -55,18 +55,13 @@ def sim_send(m, a): def sim_recv(m, a): '''receive control information from SITL''' - while True: - fd = sim_in.fileno() - rin = [fd] - try: - (rin, win, xin) = select.select(rin, [], [], 1.0) - except select.error: - util.check_parent() - continue - if fd in rin: - break - util.check_parent() - buf = sim_in.recv(22) + try: + buf = sim_in.recv(22) + except socket.error as e: + if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]: + raise + return + if len(buf) != 22: return pwm = list(struct.unpack('<11H', buf)) @@ -88,7 +83,7 @@ parser.add_option("--fgout", dest="fgout", help="flightgear output (IP:port)", 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=50) +parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", default=1000) (opts, args) = parser.parse_args() @@ -125,7 +120,6 @@ fdm = fgFDM.fgFDM() # create the quadcopter model a = QuadCopter() -a.update_frequency = opts.rate # motors initially off m = [0, 0, 0, 0] @@ -155,7 +149,11 @@ print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % ( a.home_altitude, a.yaw)) +frame_time = 1.0/opts.rate + + while True: + frame_start = time.time() sim_recv(m, a) m2 = m[:] @@ -171,3 +169,6 @@ while True: a.yaw, a.yaw_rate)) lastt = t frame_count = 0 + frame_end = time.time() + if frame_end - frame_start < frame_time: + time.sleep(frame_time - (frame_end - frame_start))