From 5be56a824d7c733cb7236802a59bdbc12d8df14e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
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))