Tools: autotest: tidy handling of SITL start position

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)
+SITL_START_LOCATION = mavutil.location(40.071374969556928,
-HOME = mavutil.location(40.071374969556928,
+                                       -105.22978898137808,
-                        -105.22978898137808,
+                                       1583.702759,
-                        1583.702759,
+                                       246)
-                        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.distance_to_home()
-        home_distance = self.get_distance(HOME, pos)
         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()
+            if self.distance_to_home() > distance:
-            home_distance = self.get_distance(HOME, pos)
-            if home_distance > distance:
                 return
         raise NotAchievedException("Failed to get %fm from home (now=%f)" %
                                    (distance, home_distance))

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)
+SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 270)
-AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
+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.distance_to_home()
-            home_distance = self.get_distance(HOME, pos)
             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.distance_to_home()
-            home_distance = self.get_distance(HOME, pos)
             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()
+        while self.distance_to_home() > 5:
-        dist_to_home = self.get_distance(HOME, pos)
-        while dist_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()
+            self.progress("Dist from home: %.02f" % self.distance_to_home())
-            dist_to_home = self.get_distance(HOME, pos)
-            self.progress("Dist from home: %.02f" % dist_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
 

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,

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,

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: