Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Peter Barker 2023-09-27 11:36:24 +10:00 committed by Andrew Tridgell
parent 36e385fb22
commit 1a04eadb63
2 changed files with 55 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Tools/autotest/ArduCopter_Tests/FarOrigin/copter_mission.txt Normal file
View File

@ -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

41
Tools/autotest/arducopter.py
View File

@ -2768,6 +2768,46 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.progress("Auto mission completed: passed!") 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): def fly_loaded_mission(self, num_wp):
'''fly mission loaded on vehicle. FIXME: get num_wp from vehicle''' '''fly mission loaded on vehicle. FIXME: get num_wp from vehicle'''
self.progress("test: Fly a mission from 1 to %u" % num_wp) 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.CompassMot,
self.AutoRTL, self.AutoRTL,
self.EK3_OGN_HGT_MASK, self.EK3_OGN_HGT_MASK,
self.FarOrigin,
]) ])
return ret return ret