autotest: fix Beacon test

Tools/autotest/arducopter.py

@@ -4749,12 +4749,24 @@ class AutoTestCopter(AutoTest):
                 break
         self.do_RTL()
 
+    def get_global_position_int(self, timeout=30):
+        tstart = self.get_sim_time()
+        while True:
+            if self.get_sim_time_cached() - tstart > timeout:
+                raise NotAchievedException("Did not get good global_position_int")
+            m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            self.progress("GPI: %s" % str(m))
+            if m is None:
+                continue
+            if m.lat != 0 or m.lon != 0:
+                return m
+
     def fly_beacon_position(self):
         self.reboot_sitl()
 
         self.wait_ready_to_arm(require_absolute=True)
 
-        old_pos = self.mav.location()
+        old_pos = self.get_global_position_int()
         print("old_pos=%s" % str(old_pos))
 
         self.context_push()
@@ -4770,8 +4782,17 @@ class AutoTestCopter(AutoTest):
             self.set_parameter("EK3_ENABLE", 1)
             self.set_parameter("EK3_GPS_TYPE", 3) # NOGPS
             self.set_parameter("EK2_ENABLE", 0)
+            self.reboot_sitl()
             self.set_parameter("AHRS_EKF_TYPE", 3)
 
+            # turn off GPS arming checks.  This may be considered a
+            # bug that we need to do this.
+            old_arming_check = int(self.get_parameter("ARMING_CHECK"))
+            if old_arming_check == 1:
+                old_arming_check = 1^25-1
+            new_arming_check = int(old_arming_check) & ~(1<<3)
+            self.set_parameter("ARMING_CHECK", new_arming_check)
+
             self.reboot_sitl()
 
             # require_absolute=True infers a GPS is present
@@ -4783,23 +4804,35 @@ class AutoTestCopter(AutoTest):
                 if self.get_sim_time_cached() - tstart > timeout:
                     raise NotAchievedException("Did not get new position like old position")
                 self.progress("Fetching location")
-                new_pos = self.mav.location()
+                new_pos = self.get_global_position_int()
-                pos_delta = self.get_distance(old_pos, new_pos)
+                pos_delta = self.get_distance_int(old_pos, new_pos)
                 max_delta = 1
                 self.progress("delta=%u want <= %u" % (pos_delta, max_delta))
                 if pos_delta <= max_delta:
                     break
 
             self.progress("Moving to ensure location is tracked")
-            self.takeoff(10, mode="LOITER")
+            self.takeoff(10, mode="STABILIZE")
-            self.set_rc(2, 1400)
+            self.change_mode("CIRCLE")
-            self.delay_sim_time(5)
+
-            self.set_rc(2, 1500)
+            tstart = self.get_sim_time()
-            self.wait_groundspeed(0, 0.1)
+            max_delta = 0
-            pos_delta = self.get_distance(self.sim_location(), self.mav.location)
+            max_allowed_delta = 10
+            while True:
+                if self.get_sim_time_cached() - tstart > timeout:
+                    break
+
+                pos_delta = self.get_distance_int(self.sim_location_int(), self.get_global_position_int())
+                self.progress("pos_delta=%f max_delta=%f max_allowed_delta=%f" % (pos_delta, max_delta, max_allowed_delta))
                 if pos_delta > max_delta:
-                raise NotAchievedException("Vehicle location not tracking simulated location (%u > %u)" %
+                    max_delta = pos_delta
-                                           (pos_delta, max_delta))
+                if pos_delta > max_allowed_delta:
+                    raise NotAchievedException("Vehicle location not tracking simulated location (%f > %f)" %
+                                               (pos_delta, max_allowed_delta))
+            self.progress("Tracked location just fine (max_delta=%f)" % max_delta)
+            self.change_mode("LOITER")
+            self.wait_groundspeed(0, 0.3, timeout=120)
+            self.land_and_disarm()
 
         except Exception as e:
             self.progress("Caught exception: %s" %
@@ -5469,7 +5502,6 @@ class AutoTestCopter(AutoTest):
         return {
             "Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702",
             "HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525",
-            "BeaconPosition": "See https://github.com/ArduPilot/ardupilot/issues/11689",
         }
 
 class AutoTestHeli(AutoTestCopter):

Tools/autotest/common.py

@@ -753,6 +753,13 @@ class FRSkyPassThrough(FRSkySPort):
     def progress_tag(self):
         return "FRSkyPassthrough"
 
+class LocationInt(object):
+    def __init__(self, lat, lon, alt, yaw):
+        self.lat = lat
+        self.lon = lon
+        self.alt = alt
+        self.yaw = yaw
+
 class AutoTest(ABC):
     """Base abstract class.
     It implements the common function for all vehicle types.
@@ -1914,6 +1921,14 @@ class AutoTest(ABC):
                                 0,
                                 math.degrees(m.yaw))
 
+    def sim_location_int(self):
+        """Return current simulator location."""
+        m = self.mav.recv_match(type='SIMSTATE', blocking=True)
+        return mavutil.location(m.lat,
+                                m.lng,
+                                0,
+                                math.degrees(m.yaw))
+
     def save_wp(self, ch=7):
         """Trigger RC Aux to save waypoint."""
         self.set_rc(ch, 1000)