mirror of https://github.com/ArduPilot/ardupilot
Tools: autotest: tidy handling of SITL start position
This commit is contained in:
parent
0f5041e873
commit
c0394e9577
|
@ -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))
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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:
|
||||
|
|
Loading…
Reference in New Issue