From c0394e9577eea45bb28fb2dacf6d67ddc4d06e52 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 16 Feb 2019 11:46:33 +1100 Subject: [PATCH] Tools: autotest: tidy handling of SITL start position --- Tools/autotest/apmrover2.py | 26 ++++++++++---------------- Tools/autotest/arducopter.py | 35 +++++++++++++---------------------- Tools/autotest/arduplane.py | 11 +++++------ Tools/autotest/ardusub.py | 13 ++++++------- Tools/autotest/common.py | 15 +++++++++++++++ 5 files changed, 49 insertions(+), 51 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index d8dba249ec..633ebfc8e0 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -21,11 +21,10 @@ 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, - -105.22978898137808, - 1583.702759, - 246) +SITL_START_LOCATION = mavutil.location(40.071374969556928, + -105.22978898137808, + 1583.702759, + 246) class AutoTestRover(AutoTest): @@ -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)) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 4defb1fb41..ee9140e139 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 5ec03debb6..6d38dbe044 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -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, diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 406a0f0af3..bcd24512c1 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -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, diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 45788798cc..aaeaa2aa8c 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -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: