autotest: add test for flying a mission far from EKF origin

Author: peterbarker

Tools/autotest/ArduCopter_Tests/FarOrigin/copter_mission.txt

@@ -0,0 +1,14 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	1015.559998	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.362881	149.165222	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364652	149.163501	20.000000	1
+3	0	0	115	640.000000	20.000000	1.000000	1.000000	0.000000	0.000000	0.000000	1
+4	0	3	19	5.000000	0.000000	1.000000	1.000000	0.000000	0.000000	20.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365361	149.163501	0.000000	1
+6	0	3	16	1.000000	0.000000	0.000000	0.000000	-35.365361	149.163995	40.000000	1
+7	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365361	149.164563	20.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364652	149.164531	20.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364652	149.163995	20.000000	1
+10	0	0	177	8.000000	1.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+11	0	3	16	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+12	0	0	20	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1

Tools/autotest/arducopter.py

@@ -2768,6 +2768,50 @@ def CopterMission(self):
 
         self.progress("Auto mission completed: passed!")
 
+#    def loc_from_received_GLOBAL_POSITION_INT(self):
+#        '''return a mavutil.location created from a received GLOBAL_POSITION_INT'''
+#        x = mavutil.location(m.lat/1e7, m.lon/1e7, m.alt/1e3, 0)
+
+    def set_origin(self, loc, timeout=60):
+        '''set the GPS global origin to loc'''
+        tstart = self.get_sim_time()
+        while True:
+            if self.get_sim_time_cached() - tstart > timeout:
+                raise AutoTestTimeoutException("Did not get non-zero lat")
+            target_system = 1
+            self.mav.mav.set_gps_global_origin_send(
+                target_system,
+                int(loc.lat * 1e7),
+                int(loc.lng * 1e7),
+                int(loc.alt * 1e3)
+            )
+            gpi = self.assert_receive_message('GLOBAL_POSITION_INT')
+            self.progress("gpi=%s" % str(gpi))
+            if gpi.lat != 0:
+                break
+
+    def FarOrigin(self):
+        '''fly a mission far from the vehicle origin'''
+        # Fly mission #1
+        self.set_parameters({
+            "SIM_GPS_DISABLE": 1,
+        })
+        self.reboot_sitl()
+        nz = mavutil.location(-43.730171, 169.983118, 1466.3, 270)
+        self.set_origin(nz)
+        self.set_parameters({
+            "SIM_GPS_DISABLE": 0,
+        })
+        self.progress("# Load copter_mission")
+        # load the waypoint count
+        num_wp = self.load_mission("copter_mission.txt", strict=False)
+        if not num_wp:
+            raise NotAchievedException("load copter_mission failed")
+
+        self.fly_loaded_mission(num_wp)
+
+        self.progress("Auto mission completed: passed!")
+
     def fly_loaded_mission(self, num_wp):
         '''fly mission loaded on vehicle.  FIXME: get num_wp from vehicle'''
         self.progress("test: Fly a mission from 1 to %u" % num_wp)
@@ -11388,6 +11432,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.CompassMot,
             self.AutoRTL,
             self.EK3_OGN_HGT_MASK,
+            self.FarOrigin,
         ])
         return ret