diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 5c78a3a7a1..ea51743039 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -675,7 +675,7 @@ class AutoTestCopter(AutoTest): self.start_subtest("Radio failsafe RTL with option to continue mission: FS_THR_ENABLE=1 & FS_OPTIONS=1") self.set_parameter('FS_OPTIONS', 1) self.progress("# Load copter_mission") - num_wp = self.load_mission("copter_mission.txt") + num_wp = self.load_mission("copter_mission.txt", strict=False) if not num_wp: raise NotAchievedException("load copter_mission failed") # self.takeoffAndMoveAway() @@ -840,7 +840,7 @@ class AutoTestCopter(AutoTest): # Trigger telemetry loss with failsafe enabled to test FS_OPTIONS settings self.start_subtest("GCS failsafe with option bit tests: FS_GCS_ENABLE=1 & FS_OPTIONS=64/2/16") - num_wp = self.load_mission("copter_mission.txt") + num_wp = self.load_mission("copter_mission.txt", strict=False) if not num_wp: raise NotAchievedException("load copter_mission failed") self.setGCSfailsafe(1) @@ -1466,7 +1466,7 @@ class AutoTestCopter(AutoTest): # Fly mission #1 self.progress("# Load copter_glitch_mission") # load the waypoint count - num_wp = self.load_mission("copter_glitch_mission.txt") + num_wp = self.load_mission("copter_glitch_mission.txt", strict=False) if not num_wp: raise NotAchievedException("load copter_glitch_mission failed") @@ -1993,7 +1993,7 @@ class AutoTestCopter(AutoTest): # Fly mission #1 self.progress("# Load copter_mission") # load the waypoint count - num_wp = self.load_mission("copter_mission.txt") + num_wp = self.load_mission("copter_mission.txt", strict=False) if not num_wp: raise NotAchievedException("load copter_mission failed") @@ -6847,7 +6847,7 @@ class AutoTestHeli(AutoTestCopter): # upload mission from file self.progress("# Load copter_AVC2013_mission") # load the waypoint count - num_wp = self.load_mission("copter_AVC2013_mission.txt") + num_wp = self.load_mission("copter_AVC2013_mission.txt", strict=False) if not num_wp: raise NotAchievedException("load copter_AVC2013_mission failed") diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index c1ec0481a8..5e18eb9791 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -521,10 +521,10 @@ class AutoTestPlane(AutoTest): return self.wait_level_flight() - def fly_mission(self, filename, mission_timeout=60.0): + def fly_mission(self, filename, mission_timeout=60.0, strict=True): """Fly a mission from a file.""" self.progress("Flying mission %s" % filename) - num_wp = self.load_mission(filename)-1 + num_wp = self.load_mission(filename, strict=strict)-1 self.change_mode('AUTO') self.wait_waypoint(1, num_wp, max_dist=60) self.wait_groundspeed(0, 0.5, timeout=mission_timeout) @@ -1328,7 +1328,7 @@ class AutoTestPlane(AutoTest): self.run_subtest("CIRCLE test", self.fly_CIRCLE) self.run_subtest("Mission test", - lambda: self.fly_mission("ap1.txt")) + lambda: self.fly_mission("ap1.txt", strict=False)) def airspeed_autocal(self): self.progress("Ensure no AIRSPEED_AUTOCAL on ground") @@ -1781,8 +1781,8 @@ class AutoTestPlane(AutoTest): self.plane_CPUFailsafe() def test_large_missions(self): - self.load_mission("Kingaroy-vlarge.txt") - self.load_mission("Kingaroy-vlarge2.txt") + self.load_mission("Kingaroy-vlarge.txt", strict=False) + self.load_mission("Kingaroy-vlarge2.txt", strict=False) def fly_soaring(self): @@ -1794,7 +1794,7 @@ class AutoTestPlane(AutoTest): defaults_filepath=self.model_defaults_filepath("ArduPlane", model), wipe=True) - self.load_mission('CMAC-soar.txt') + self.load_mission('CMAC-soar.txt', strict=False) self.set_current_waypoint(1) self.change_mode('AUTO') @@ -1899,7 +1899,7 @@ class AutoTestPlane(AutoTest): self.reboot_sitl() self.wait_ready_to_arm() self.arm_vehicle() - self.fly_mission("ap1.txt") + self.fly_mission("ap1.txt", strict=False) def fly_terrain_mission(self): diff --git a/Tools/autotest/balancebot.py b/Tools/autotest/balancebot.py index 688ba28438..c7548bc07e 100644 --- a/Tools/autotest/balancebot.py +++ b/Tools/autotest/balancebot.py @@ -115,7 +115,7 @@ inherit Rover's tests!''' ("DriveMission", "Drive Mission %s" % "balancebot1.txt", - lambda: self.drive_mission("balancebot1.txt")), + lambda: self.drive_mission("balancebot1.txt", strict=False)), ("TestWheelEncoder", "Test wheel encoders", diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index a81d991931..d82c46c5c0 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -2925,6 +2925,18 @@ class AutoTest(ABC): this_dir = os.path.dirname(__file__) return os.path.realpath(os.path.join(this_dir, "../..")) + def ardupilot_stores_frame_for_cmd(self, t): + # ardupilot doesn't remember frame on these commands + return t not in [ + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + mavutil.mavlink.MAV_CMD_CONDITION_YAW, + mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, + mavutil.mavlink.MAV_CMD_DO_JUMP, + mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + ] + def assert_mission_files_same(self, file1, file2, match_comments=False): self.progress("Comparing (%s) and (%s)" % (file1, file2, )) @@ -2961,16 +2973,8 @@ class AutoTest(ABC): t = int(fields1[3]) # mission item type for (count, (i1, i2)) in enumerate(zip(fields1, fields2)): if count == 2: # frame - if t in [mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - mavutil.mavlink.MAV_CMD_CONDITION_YAW, - mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, - mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, - mavutil.mavlink.MAV_CMD_DO_JUMP, - mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, - mavutil.mavlink.MAV_CMD_DO_SET_SERVO, - ]: - # ardupilot doesn't remember frame on these commands - if int(i1) in [3, 10]: # 3 is relative, 10 is AMSL + if not self.ardupilot_stores_frame_for_cmd(t): + if int(i1) in [3, 10]: # 3 is relative, 10 is terrain i1 = 0 if int(i2) in [3, 10]: i2 = 0 @@ -3122,10 +3126,54 @@ class AutoTest(ABC): def load_sample_mission(self): self.load_mission(self.sample_mission_filename()) - def load_mission(self, filename): - return self.load_mission_from_filepath(self.current_test_name_directory, filename) + def load_mission(self, filename, strict=True): + return self.load_mission_from_filepath( + self.current_test_name_directory, + filename, + strict=strict) - def load_mission_from_filepath(self, filepath, filename): + def wp_to_mission_item_int(self, wp): + '''convert a MISSION_ITEM to a MISSION_ITEM_INT. We always send as + MISSION_ITEM_INT to give cm level accuracy + Swiped from mavproxy_wp.py + ''' + if wp.get_type() == 'MISSION_ITEM_INT': + return wp + wp_int = mavutil.mavlink.MAVLink_mission_item_int_message( + wp.target_system, + wp.target_component, + wp.seq, + wp.frame, + wp.command, + wp.current, + wp.autocontinue, + wp.param1, + wp.param2, + wp.param3, + wp.param4, + int(wp.x*1.0e7), + int(wp.y*1.0e7), + wp.z) + return wp_int + + def load_mission_from_filepath(self, filepath, filename, target_system=1, target_component=1, strict=True): + self.progress("Loading mission (%s)" % filename) + path = os.path.join(testdir, filepath, filename) + wploader = mavwp.MAVWPLoader( + target_system=target_system, + target_component=target_component + ) + wploader.load(path) + wpoints_int = [self.wp_to_mission_item_int(x) for x in wploader.wpoints] + self.check_mission_upload_download(wpoints_int, strict=strict) + return len(wpoints_int) + + def load_mission_using_mavproxy(self, filename): + return self.load_mission_from_filepath_using_mavproxy( + self.current_test_name_directory, + filename) + + def load_mission_from_filepath_using_mavproxy(self, filepath, filename): """Load a mission from a file to flight controller.""" self.progress("Loading mission (%s)" % filename) path = os.path.join(testdir, filepath, filename) @@ -3217,7 +3265,8 @@ class AutoTest(ABC): want, got, epsilon=None, - skip_first_item=False): + skip_first_item=False, + strict=True): self.progress("Checking mission items same") if epsilon is None: epsilon = 1 @@ -3247,7 +3296,13 @@ class AutoTest(ABC): if got[0].mission_type == mavutil.mavlink.MAV_MISSION_TYPE_MISSION: item_val = getattr(item, 'frame') downloaded_item_val = getattr(downloaded_item, 'frame') - if not self.frames_equivalent(item_val, downloaded_item_val): + # if you are thinking of adding another, "don't annoy + # me, I know missions aren't troundtripped" non-strict + # thing here, DON'T do it without first checking "def + # assert_mission_files_same"; it makes the same checks + # as will be needed here eventually. + if ((strict or self.ardupilot_stores_frame_for_cmd(getattr(item, 'command'))) and + not self.frames_equivalent(item_val, downloaded_item_val)): raise NotAchievedException("Frame not same (got=%s want=%s)" % (self.string_for_frame(downloaded_item_val), self.string_for_frame(item_val))) @@ -3255,15 +3310,15 @@ class AutoTest(ABC): raise NotAchievedException("Z not preserved (got=%f want=%f)" % (item.z, downloaded_item.z)) - def check_fence_items_same(self, want, got): + def check_fence_items_same(self, want, got, strict=True): check_atts = ['mission_type', 'command', 'x', 'y', 'seq', 'param1'] - return self.check_mission_items_same(check_atts, want, got) + return self.check_mission_items_same(check_atts, want, got, strict=strict) - def check_mission_waypoint_items_same(self, want, got): + def check_mission_waypoint_items_same(self, want, got, strict=True): check_atts = ['mission_type', 'command', 'x', 'y', 'z', 'seq', 'param1'] - return self.check_mission_items_same(check_atts, want, got, skip_first_item=True) + return self.check_mission_items_same(check_atts, want, got, skip_first_item=True, strict=strict) - def check_mission_item_upload_download(self, items, itype, mission_type): + def check_mission_item_upload_download(self, items, itype, mission_type, strict=True): self.progress("check %s _upload/download: upload %u items" % (itype, len(items),)) self.upload_using_mission_protocol(mission_type, items) @@ -3274,9 +3329,9 @@ class AutoTest(ABC): raise NotAchievedException("Did not download same number of items as uploaded want=%u got=%u" % (len(items), len(downloaded_items))) if mission_type == mavutil.mavlink.MAV_MISSION_TYPE_FENCE: - self.check_fence_items_same(items, downloaded_items) + self.check_fence_items_same(items, downloaded_items, strict=strict) elif mission_type == mavutil.mavlink.MAV_MISSION_TYPE_MISSION: - self.check_mission_waypoint_items_same(items, downloaded_items) + self.check_mission_waypoint_items_same(items, downloaded_items, strict=strict) else: raise NotAchievedException("Unhandled") @@ -3286,11 +3341,12 @@ class AutoTest(ABC): "fence", mavutil.mavlink.MAV_MISSION_TYPE_FENCE) - def check_mission_upload_download(self, items): + def check_mission_upload_download(self, items, strict=True): self.check_mission_item_upload_download( items, "waypoints", - mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + mavutil.mavlink.MAV_MISSION_TYPE_MISSION, + strict=strict) def rc_defaults(self): return { diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 1fa043ff01..061a2abac9 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -387,10 +387,10 @@ class AutoTestRover(AutoTest): ################################################# # AUTOTEST ALL ################################################# - def drive_mission(self, filename): + def drive_mission(self, filename, strict=True): """Drive a mission from a file.""" self.progress("Driving mission %s" % filename) - self.load_mission(filename) + self.load_mission(filename, strict=strict) self.wait_ready_to_arm() self.arm_vehicle() self.change_mode('AUTO') @@ -3457,7 +3457,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) raise NotAchievedException("Did not get expected MISSION_CURRENT") if m.seq != 0: raise NotAchievedException("Bad mission current") - self.load_mission("rover-gripper-mission.txt") + self.load_mission_using_mavproxy("rover-gripper-mission.txt") set_wp = 1 self.mavproxy.send('wp set %u\n' % set_wp) self.drain_mav() @@ -5604,7 +5604,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) ("DriveMission", "Drive Mission %s" % "rover1.txt", - lambda: self.drive_mission("rover1.txt")), + lambda: self.drive_mission("rover1.txt", strict=False)), # disabled due to frequent failures in travis. This test needs re-writing # ("Drive Brake", self.drive_brake),