From 7647e38f3f2829dd07b2e23e9a947ff9cde95273 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 6 Jul 2022 12:40:59 +1000 Subject: [PATCH] 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 --- Tools/autotest/common.py | 38 ++++++++++++++++++++++++++++++-------- 1 file changed, 30 insertions(+), 8 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 9216df1875..7575c64715 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -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