Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: load missions natively rather than using MAVProxy

Browse Source
This commit is contained in:
Peter Barker 2021-02-25 12:02:57 +11:00 committed by Peter Barker
parent 993d87469c
commit 10007e2e42
5 changed files with 97 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Tools/autotest/arducopter.py
View File

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

14
Tools/autotest/arduplane.py
View File

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

2
Tools/autotest/balancebot.py
View File

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

104
Tools/autotest/common.py
View File

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

8
Tools/autotest/rover.py
View File

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