Tools: autotest: tidy handling of SITL start position

This commit is contained in:
Peter Barker 2019-02-16 11:46:33 +11:00 committed by Peter Barker
parent 0f5041e873
commit c0394e9577
5 changed files with 49 additions and 51 deletions

View File

@ -21,8 +21,7 @@ from pymavlink import mavutil
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
# HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
HOME = mavutil.location(40.071374969556928,
SITL_START_LOCATION = mavutil.location(40.071374969556928,
-105.22978898137808,
1583.702759,
246)
@ -48,17 +47,15 @@ class AutoTestRover(AutoTest):
self.gdbserver = gdbserver
self.breakpoints = breakpoints
self.home = "%f,%f,%u,%u" % (HOME.lat,
HOME.lng,
HOME.alt,
HOME.heading)
self.homeloc = None
self.speedup = speedup
self.sitl = None
self.log_name = "APMrover2"
def sitl_start_location(self):
return SITL_START_LOCATION
def init(self):
if self.frame is None:
self.frame = 'rover'
@ -67,7 +64,7 @@ class AutoTestRover(AutoTest):
self.sitl = util.start_SITL(self.binary,
model=self.frame,
home=self.home,
home=self.sitl_home(),
speedup=self.speedup,
valgrind=self.valgrind,
gdb=self.gdb,
@ -474,8 +471,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.wait_mode('HOLD', timeout=600) # balancebot can take a long time!
pos = self.mav.location()
home_distance = self.get_distance(HOME, pos)
home_distance = self.distance_to_home()
home_distance_max = 5
if home_distance > home_distance_max:
raise NotAchievedException(
@ -492,9 +488,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
tstart = self.get_sim_time()
while self.get_sim_time() - tstart < timeout:
# m = self.mav.recv_match(type='VFR_HUD', blocking=True)
pos = self.mav.location()
home_distance = self.get_distance(HOME, pos)
if home_distance > distance:
if self.distance_to_home() > distance:
return
raise NotAchievedException("Failed to get %fm from home (now=%f)" %
(distance, home_distance))

View File

@ -19,8 +19,8 @@ from common import NotAchievedException, AutoTestTimeoutException, PreconditionF
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 270)
SITL_START_LOCATION_AVC = mavutil.location(40.072842, -105.230575, 1586, 0)
# Flight mode switch positions are set-up in arducopter.param to be
@ -51,17 +51,15 @@ class AutoTestCopter(AutoTest):
self.gdbserver = gdbserver
self.breakpoints = breakpoints
self.home = "%f,%f,%u,%u" % (HOME.lat,
HOME.lng,
HOME.alt,
HOME.heading)
self.homeloc = None
self.speedup = speedup
self.log_name = "ArduCopter"
self.sitl = None
def sitl_start_location(self):
return SITL_START_LOCATION
def mavproxy_options(self):
ret = super(AutoTestCopter, self).mavproxy_options()
if self.frame != 'heli':
@ -82,7 +80,7 @@ class AutoTestCopter(AutoTest):
self.sitl = util.start_SITL(self.binary,
model=self.frame,
home=self.home,
home=self.sitl_home(),
speedup=self.speedup,
valgrind=self.valgrind,
gdb=self.gdb,
@ -372,8 +370,7 @@ class AutoTestCopter(AutoTest):
while self.get_sim_time() < tstart + timeout:
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m
pos = self.mav.location() # requires a GPS fix to function
home_distance = self.get_distance(HOME, pos)
home_distance = self.distance_to_home()
home = ""
if alt <= 1 and home_distance < 10:
home = "HOME"
@ -604,8 +601,7 @@ class AutoTestCopter(AutoTest):
while self.get_sim_time() < tstart + timeout:
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m
pos = self.mav.location()
home_distance = self.get_distance(HOME, pos)
home_distance = self.distance_to_home()
self.progress("Alt: %.02f HomeDistance: %.02f" %
(alt, home_distance))
# recenter pitch sticks once we're home so we don't fly off again
@ -878,18 +874,14 @@ class AutoTestCopter(AutoTest):
# wait for arrival back home
self.mav.recv_match(type='VFR_HUD', blocking=True)
pos = self.mav.location()
dist_to_home = self.get_distance(HOME, pos)
while dist_to_home > 5:
while self.distance_to_home() > 5:
if self.get_sim_time() > (tstart + timeout):
raise AutoTestTimeoutException(
("GPS Glitch testing failed"
"- exceeded timeout %u seconds" % timeout))
self.mav.recv_match(type='VFR_HUD', blocking=True)
pos = self.mav.location()
dist_to_home = self.get_distance(HOME, pos)
self.progress("Dist from home: %.02f" % dist_to_home)
self.progress("Dist from home: %.02f" % self.distance_to_home())
# turn off simulator display of gps and actual position
if self.use_map:
@ -2830,12 +2822,11 @@ class AutoTestHeli(AutoTestCopter):
super(AutoTestHeli, self).__init__(*args, **kwargs)
self.log_name = "HeliCopter"
self.home = "%f,%f,%u,%u" % (AVCHOME.lat,
AVCHOME.lng,
AVCHOME.alt,
AVCHOME.heading)
self.frame = 'heli'
def sitl_start_location(self):
return SITL_START_LOCATION_AVC
def is_heli(self):
return True

View File

@ -18,7 +18,7 @@ from common import PreconditionFailedException
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
HOME = mavutil.location(-35.362938, 149.165085, 585, 354)
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 585, 354)
WIND = "0,180,0.2" # speed,direction,variance
@ -42,10 +42,6 @@ class AutoTestPlane(AutoTest):
self.gdbserver = gdbserver
self.breakpoints = breakpoints
self.home = "%f,%f,%u,%u" % (HOME.lat,
HOME.lng,
HOME.alt,
HOME.heading)
self.homeloc = None
self.speedup = speedup
@ -53,6 +49,9 @@ class AutoTestPlane(AutoTest):
self.log_name = "ArduPlane"
def sitl_start_location(self):
return SITL_START_LOCATION
def init(self):
if self.frame is None:
self.frame = 'plane-elevrev'
@ -64,7 +63,7 @@ class AutoTestPlane(AutoTest):
self.sitl = util.start_SITL(self.binary,
wipe=True,
model=self.frame,
home=self.home,
home=self.sitl_home(),
speedup=self.speedup,
defaults_file=defaults_file,
valgrind=self.valgrind,

View File

@ -14,7 +14,8 @@ from common import NotAchievedException
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
HOME = mavutil.location(33.810313, -118.393867, 0, 185)
SITL_START_LOCATION = mavutil.location(33.810313, -118.393867, 0, 185)
class AutoTestSub(AutoTest):
@ -37,11 +38,6 @@ class AutoTestSub(AutoTest):
self.gdbserver = gdbserver
self.breakpoints = breakpoints
self.home = "%f,%f,%u,%u" % (HOME.lat,
HOME.lng,
HOME.alt,
HOME.heading)
self.homeloc = None
self.speedup = speedup
self.sitl = None
@ -51,6 +47,9 @@ class AutoTestSub(AutoTest):
def default_mode(self):
return 'MANUAL'
def sitl_start_location(self):
return SITL_START_LOCATION
def init(self):
if self.frame is None:
self.frame = 'vectored'
@ -59,7 +58,7 @@ class AutoTestSub(AutoTest):
self.sitl = util.start_SITL(self.binary,
model=self.frame,
home=self.home,
home=self.sitl_home(),
speedup=self.speedup,
valgrind=self.valgrind,
gdb=self.gdb,

View File

@ -184,6 +184,13 @@ class AutoTest(ABC):
def buildlogs_dirpath():
return os.getenv("BUILDLOGS", util.reltopdir("../buildlogs"))
def sitl_home(self):
HOME = self.sitl_start_location()
return "%f,%f,%u,%u" % (HOME.lat,
HOME.lng,
HOME.alt,
HOME.heading)
def open_mavproxy_logfile(self):
return MAVProxyLogFile()
@ -1670,6 +1677,14 @@ class AutoTest(ABC):
raise NotAchievedException("home position not updated")
return m
def distance_to_home(self):
m = self.poll_home_position()
loc = mavutil.location(m.latitude * 1.0e-7,
m.longitude * 1.0e-7,
m.altitude * 1.0e-3,
0)
return self.get_distance(loc, self.mav.location())
def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
tstart = self.get_sim_time()
while True: