Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: run the quad simulation at a much higher rate

Browse Source 
it now defaults to 1kHz
This commit is contained in:
Andrew Tridgell 2011-12-03 07:24:58 +11:00
parent e12fabebe9
commit 5be56a824d
1 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
Tools/autotest/pysim/sim_quad.py
View File

@ -55,18 +55,13 @@ def sim_send(m, a):
def sim_recv(m, a): def sim_recv(m, a):
'''receive control information from SITL''' '''receive control information from SITL'''
while True: try:
fd = sim_in.fileno() buf = sim_in.recv(22)
rin = [fd] except socket.error as e:
try: if not e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
(rin, win, xin) = select.select(rin, [], [], 1.0) raise
except select.error: return
util.check_parent()
continue
if fd in rin:
break
util.check_parent()
buf = sim_in.recv(22)
if len(buf) != 22: if len(buf) != 22:
return return
pwm = list(struct.unpack('<11H', buf)) 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("--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("--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("--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() (opts, args) = parser.parse_args()
@ -125,7 +120,6 @@ fdm = fgFDM.fgFDM()
# create the quadcopter model # create the quadcopter model
a = QuadCopter() a = QuadCopter()
a.update_frequency = opts.rate
# motors initially off # motors initially off
m = [0, 0, 0, 0] m = [0, 0, 0, 0]
@ -155,7 +149,11 @@ print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % (
a.home_altitude, a.home_altitude,
a.yaw)) a.yaw))
frame_time = 1.0/opts.rate
while True: while True:
frame_start = time.time()
sim_recv(m, a) sim_recv(m, a)
m2 = m[:] m2 = m[:]
@ -171,3 +169,6 @@ while True:
a.yaw, a.yaw_rate)) a.yaw, a.yaw_rate))
lastt = t lastt = t
frame_count = 0 frame_count = 0
frame_end = time.time()
if frame_end - frame_start < frame_time:
time.sleep(frame_time - (frame_end - frame_start))