diff --git a/Tools/autotest/ArduCopter_Tests/FarOrigin/copter_mission.txt b/Tools/autotest/ArduCopter_Tests/FarOrigin/copter_mission.txt new file mode 100644 index 0000000000..56049591de --- /dev/null +++ b/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 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 56ece1f7ed..3c4323cb39 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2768,6 +2768,46 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.progress("Auto mission completed: passed!") + 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 +11428,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.CompassMot, self.AutoRTL, self.EK3_OGN_HGT_MASK, + self.FarOrigin, ]) return ret