mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
autotest: run rover and copter with synthetic clock
This commit is contained in:
Andrew Tridgell
parent
9748cb1e3e
commit
45887a5d28
@ -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)
|
||||
|
||||
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=50 --home=%f,%f,%u,%u' % (
|
||||
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=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):
|
||||
|
||||
@ -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):
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user