mirror of https://github.com/ArduPilot/ardupilot
autotest: create get_home_tuple_from_mission method
Useful if you want to start a mission where a saved waypoint file's home location is
This commit is contained in:
parent
0103776d83
commit
7647e38f3f
|
@ -3886,13 +3886,8 @@ class AutoTest(ABC):
|
|||
wp.z)
|
||||
return wp_int
|
||||
|
||||
def load_mission_from_filepath(self,
|
||||
filepath,
|
||||
filename,
|
||||
target_system=1,
|
||||
target_component=1,
|
||||
strict=True,
|
||||
reset_current_wp=True):
|
||||
def mission_from_filepath(self, filepath, filename, target_system=1, target_component=1):
|
||||
'''returns a list of mission-item-ints from filepath'''
|
||||
self.progress("Loading mission (%s)" % filename)
|
||||
path = os.path.join(testdir, filepath, filename)
|
||||
wploader = mavwp.MAVWPLoader(
|
||||
|
@ -3900,7 +3895,34 @@ class AutoTest(ABC):
|
|||
target_component=target_component
|
||||
)
|
||||
wploader.load(path)
|
||||
wpoints_int = [self.wp_to_mission_item_int(x) for x in wploader.wpoints]
|
||||
return [self.wp_to_mission_item_int(x) for x in wploader.wpoints]
|
||||
|
||||
def get_home_tuple_from_mission(self, filename):
|
||||
'''gets item 0 from the mission file, returns a tuple suitable for
|
||||
passing to customise_SITL_commandline as --home. Yaw will be
|
||||
0, so the caller may want to fill that in
|
||||
'''
|
||||
items = self.mission_from_filepath(
|
||||
self.current_test_name_directory,
|
||||
filename,
|
||||
)
|
||||
home_item = items[0]
|
||||
return (home_item.x * 1e-7, home_item.y * 1e-7, home_item.z, 0)
|
||||
|
||||
# TODO: rename the following to "upload_mission_from_filepath"
|
||||
def load_mission_from_filepath(self,
|
||||
filepath,
|
||||
filename,
|
||||
target_system=1,
|
||||
target_component=1,
|
||||
strict=True,
|
||||
reset_current_wp=True):
|
||||
wpoints_int = self.mission_from_filepath(
|
||||
filepath,
|
||||
filename,
|
||||
target_system=target_system,
|
||||
target_component=target_component
|
||||
)
|
||||
self.check_mission_upload_download(wpoints_int, strict=strict)
|
||||
if reset_current_wp:
|
||||
# ArduPilot doesn't reset the current waypoint by default
|
||||
|
|
Loading…
Reference in New Issue