From 0a23de087cdc6c85e1caa49c397001a2b0868759 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 5 Aug 2019 15:53:40 +1000 Subject: [PATCH] Tools: autotest: tests for new fence code Tools: autotest: move clear_mission up, use it before running each test Also have it understand MAV_MISSION_TYPE_ALL Tools: autotest: add tests for polyfence Tools: autotest: add tests for multiple inclusion polyfences Tools: autotest: ensure we can't arm inside a circle exclusion zone Tools: autotest: add test for arming outside an inclusion zone Tools: autotest: add tests for upload timeout Tools: autotest: add tests for not arming due to polygon zones Tools: autotest: add test for fence point reboot survivability Tools: autotest: add test for moving fence point Tools: autotest: add some tests via MAVProxy Tools: autotest: add test for avoiding polygonal exclusion zones Tools: autotest: add test for object avoidance Currently disabled as it doesn't pass where it seemingly should. Tools: autotest: skip MAVProxy tests on older MAVProxy versions Tools: autotest: adjust fence loading for lack of string back from MAVProxy Tools: autotest: create a do_RTL function Tools: autotest: cope with loading QGC-style fence files Tools: autotest: add test for object-avoidance in guided mode Tools: autotest: add OA test for auto mode Tools: autotest: add test for bendy ruler in guided mode Tools: autotest: adjust for new MAVProxy fence module not emitting old strings Tools: autotest: add tests for prearming due to bad parameter values --- Tools/autotest/apmrover2.py | 2867 ++++++++++++++++- Tools/autotest/arducopter.py | 93 +- Tools/autotest/common.py | 300 +- Tools/autotest/pysim/util.py | 16 +- .../autotest/rover-path-bendyruler-fence.txt | 11 + .../rover-path-bendyruler-mission-easier.txt | 4 + Tools/autotest/rover-path-planning-fence.txt | 25 + .../autotest/rover-path-planning-mission.txt | 4 + 8 files changed, 3231 insertions(+), 89 deletions(-) create mode 100644 Tools/autotest/rover-path-bendyruler-fence.txt create mode 100644 Tools/autotest/rover-path-bendyruler-mission-easier.txt create mode 100644 Tools/autotest/rover-path-planning-fence.txt create mode 100644 Tools/autotest/rover-path-planning-mission.txt diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index e1b219433c..a09b1265c9 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -3,10 +3,13 @@ # Drive APMrover2 in SITL from __future__ import print_function +import copy import os +import sys import time from common import AutoTest +from pysim import util from common import AutoTestTimeoutException from common import MsgRcvTimeoutException @@ -1042,42 +1045,964 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.wait_location(loc, accuracy=accuracy) self.disarm_vehicle() - def roundtrip_fencepoint_protocol(self, offset, count, lat, lng, target_system=1, target_component=1): + def check_mission_items_same(self, check_atts, want, got, epsilon=None,skip_first_item=False): + if epsilon is None: + epsilon = 1 + if len(want) != len(got): + raise NotAchievedException("Incorrect item count (want=%u got=%u)" % (len(want), len(got))) + for i in range(0, len(want)): + if skip_first_item and i == 0: + continue + item = want[i] + downloaded_item = got[i] + # note that we do not preserved frame for anything + check_atts = ['mission_type', 'command', 'x', 'y', 'seq', 'param1'] + # z is not preserved - self.progress("Sending fence point") + print("Comparing (%s) and (%s)" % (str(item), str(downloaded_item))) + + for att in check_atts: + item_val = getattr(item, att) + downloaded_item_val = getattr(downloaded_item, att) + if abs(item_val - downloaded_item_val) > epsilon: + raise NotAchievedException( + "Item %u (%s) has different %s after download want=%s got=%s (got-item=%s)" % + (i, str(item), att, str(item_val), str(downloaded_item_val), str(downloaded_item))) + + def check_fence_items_same(self, want, got): + check_atts = ['mission_type', 'command', 'x', 'y', 'seq', 'param1'] + return self.check_mission_items_same(check_atts, want, got) + + def check_mission_waypoint_items_same(self, want, got): + check_atts = ['mission_type', 'command', 'x', 'y', 'z', 'seq', 'param1'] + return self.check_mission_items_same(check_atts, want, got, skip_first_item=True) + + def check_fence_upload_download(self, items): + self.progress("check_fence_upload_download: upload %u items" % (len(items),)) + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + items) + self.progress("check_fence_upload_download: download items") + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.progress("Downloaded items: (%s)" % str(downloaded_items)) + if len(items) != len(downloaded_items): + raise NotAchievedException("Did not download same number of items as uploaded want=%u got=%u" % (len(items), len(downloaded_items))) + self.check_fence_items_same(items, downloaded_items) + + def fence_with_bad_frame(self, target_system=1, target_component=1): + return [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + + def fence_with_zero_vertex_count(self, target_system=1, target_component=1): + return [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + + def fence_with_wrong_vertex_count(self, target_system=1, target_component=1): + return [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 2, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + + def fence_with_multiple_return_points(self, target_system=1, target_component=1): + return [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + + def fence_with_invalid_latlon(self, target_system=1, target_component=1): + return [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(100 * 1e7), # bad latitude. bad. + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + + def fence_with_multiple_return_points_with_bad_sequence_numbers(self, target_system=1, target_component=1): + return [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0 * 1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(2.0 * 1e7), # latitude + int(2.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + + def fence_which_exceeds_storage_space(self, target_system=1, target_component=1): + ret = [] + for i in range(0, 60): + ret.append(self.mav.mav.mission_item_int_encode( + target_system, + target_component, + i, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + 0, # current + 0, # autocontinue + 10, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0 * 1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ) + return ret + + def fences_which_should_not_upload(self, target_system=1, target_component=1): + return [ ("Bad Frame", self.fence_with_bad_frame(target_system=target_system, target_component=target_component)), + ("Zero Vertex Count", self.fence_with_zero_vertex_count(target_system=target_system, target_component=target_component)), + ("Wrong Vertex Count", self.fence_with_wrong_vertex_count(target_system=target_system, target_component=target_component)), + ("Multiple return points", self.fence_with_multiple_return_points(target_system=target_system, target_component=target_component)), + ("Invalid lat/lon", self.fence_with_invalid_latlon(target_system=target_system, target_component=target_component)), + ("Multiple Return points with bad sequence numbers", self.fence_with_multiple_return_points_with_bad_sequence_numbers(target_system=target_system, target_component=target_component)), + ("Fence which exceeds storage space", self.fence_which_exceeds_storage_space(target_system=target_system, target_component=target_component)), + ] + + + def fence_with_single_return_point(self, target_system=1, target_component=1): + return [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + def fence_with_single_return_point_and_5_vertex_inclusion(self, target_system=1, target_component=1): + return [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0000 *1e7), # latitude + int(1.0000 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 2, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0001 *1e7), # latitude + int(1.0000 *1e7), # longitude + 32.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 3, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0001 *1e7), # latitude + int(1.0001 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 4, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0002 *1e7), # latitude + int(1.0002 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 5, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0002 *1e7), # latitude + int(1.0003 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + + def fence_with_many_exclusion_circles(self, count=50, target_system=1, target_component=1): + ret = [] + for i in range(0, count): + lat_deg = 1.0003 + count/10 + lng_deg = 1.0002 + count/10 + item = self.mav.mav.mission_item_int_encode( + target_system, + target_component, + i, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + 0, # current + 0, # autocontinue + count, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(lat_deg *1e7), # latitude + int(lng_deg *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + ret.append(item) + return ret + + def fence_with_many_exclusion_polyfences(self, target_system=1, target_component=1): + ret = [] + seq = 0 + for fencenum in range(0,4): + pointcount = fencenum + 6 + for p in range(0, pointcount): + lat_deg = 1.0003 + p/10 + fencenum/100 + lng_deg = 1.0002 + p/10 + fencenum/100 + item = self.mav.mav.mission_item_int_encode( + target_system, + target_component, + seq, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + 0, # current + 0, # autocontinue + pointcount, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(lat_deg *1e7), # latitude + int(lng_deg *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + ret.append(item) + seq += 1 + return ret + + def fences_which_should_upload(self, target_system=1, target_component=1): + return [ + ("Single Return Point", self.fence_with_single_return_point(target_system=target_system, target_component=target_component)), + ( "Return and 5-vertex-inclusion", self.fence_with_single_return_point_and_5_vertex_inclusion(target_system=target_system, target_component=target_component) ), + ( "Many exclusion circles", self.fence_with_many_exclusion_circles(target_system=target_system, target_component=target_component) ), + ( "Many exclusion polyfences", self.fence_with_many_exclusion_polyfences(target_system=target_system, target_component=target_component) ), + ( "Empty fence", [] ), + ] + + + def assert_fence_does_not_upload(self, fence, target_system=1, target_component=1): + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + # upload single item using mission item protocol: + upload_failed = False + try: + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + fence) + except NotAchievedException: + # TODO: make sure we failed for correct reason + upload_failed = True + if not upload_failed: + raise NotAchievedException("Uploaded fence when should not be possible") + self.progress("Fence rightfully bounced") + + def fencepoint_protocol_epsilon(self): + return 0.00002 + + def roundtrip_fencepoint_protocol(self, offset, count, lat, lng, target_system=1, target_component=1): + self.progress("Sending FENCE_POINT offs=%u count=%u" % (offset, count)) self.mav.mav.fence_point_send(target_system, target_component, offset, - 1, + count, lat, lng) self.progress("Requesting fence point") + m = self.get_fence_point(offset, target_system=target_system, target_component=target_component) + if abs(m.lat - lat) > self.fencepoint_protocol_epsilon(): + raise NotAchievedException("Did not get correct lat in fencepoint: got=%f want=%f" % (m.lat, lat)) + if abs(m.lng - lng) > self.fencepoint_protocol_epsilon(): + raise NotAchievedException("Did not get correct lng in fencepoint: got=%f want=%f" % (m.lng, lng)) + self.progress("Roundtrip OK") + + def roundtrip_fence_using_fencepoint_protocol(self, loc_list, target_system=1, target_component=1, ordering=None): + count = len(loc_list) + offset = 0 + self.set_parameter("FENCE_TOTAL", count) + if ordering is None: + ordering = range(count) + elif len(ordering) != len(loc_list): + raise ValueError("ordering list length mismatch") + + for offset in ordering: + loc = loc_list[offset] + self.roundtrip_fencepoint_protocol(offset, + count, + loc.lat, + loc.lng, + target_system, + target_component) + + self.progress("Validating uploaded fence") + returned_count = self.get_parameter("FENCE_TOTAL") + if returned_count != count: + raise NotAchievedException("Returned count mismatch (want=%u got=%u)" % + (count, returned_count)) + for i in range(count): + self.progress("Requesting fence point") + m = self.get_fence_point(offset, target_system=target_system, target_component=target_component) + if abs(m.lat-loc.lat) > self.fencepoint_protocol_epsilon(): + raise NotAchievedException("Returned lat mismatch (want=%f got=%f" % + (loc.lat, m.lat)) + if abs(m.lng-loc.lng) > self.fencepoint_protocol_epsilon(): + raise NotAchievedException("Returned lng mismatch (want=%f got=%f" % + (loc.lng, m.lng)) + if m.count != count: + raise NotAchievedException("Count mismatch (want=%u got=%u)" % + (count, m.count)) + + def assert_parameter_value(self, parameter, required): + got = self.get_parameter(parameter) + if got != required: + raise NotAchievedException("%s has unexpected value; want=%f got=%f" % + (parameter, required, got)) + + def send_fencepoint_expect_statustext(self, offset, count, lat, lng, statustext_fragment, target_system=1, target_component=1, timeout=10): + self.mav.mav.fence_point_send(target_system, + target_component, + offset, + count, + lat, + lng) + + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Did not get error message back") + m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1) + self.progress("statustext: %s (want='%s')" % + (str(m), statustext_fragment)) + if m is None: + continue + if statustext_fragment in m.text: + break + + def get_fence_point(self, idx, target_system=1, target_component=1): self.mav.mav.fence_fetch_point_send(target_system, target_component, - offset) + idx) m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=2) print("m: %s" % str(m)) if m is None: raise NotAchievedException("Did not get fence return point back") - if abs(m.lat - lat) > 0.000001: - raise NotAchievedException("Did not get correct lat in fencepoint: got=%f want=%f" % (m.lat, lat)) - if abs(m.lng - lng) > 0.000001: - raise NotAchievedException("Did not get correct lng in fencepoint: got=%f want=%f" % (m.lng, lng)) - self.progress("Roundtrip OK") + if m.idx != idx: + raise NotAchievedException("Invalid idx returned (want=%u got=%u)" % + (idx, m.seq)) + return m + + def test_gcs_fence_centroid(self, target_system=1, target_component=1): + self.start_subtest("Ensuring if we don't have a centroid it gets calculated") + items = self.test_gcs_fence_need_centroid( + target_system=target_system, + target_component=target_component) + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + items) + centroid = self.get_fence_point(0) + want_lat = 1.0001 + want_lng = 1.00005 + if abs(centroid.lat - want_lat) > 0.000001: + raise NotAchievedException("Centroid lat not as expected (want=%f got=%f)" % (want_lat, centroid.lat)) + if abs(centroid.lng - want_lng) > 0.000001: + raise NotAchievedException("Centroid lng not as expected (want=%f got=%f)" % (want_lng, centroid.lng)) + + + def test_gcs_fence_update_fencepoint(self, target_system=1, target_component=1): + self.start_subtest("Ensuring we can move a fencepoint") + items = self.test_gcs_fence_boring_triangle( + target_system=target_system, + target_component=target_component) + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + items) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + item_seq = 2 + item = items[item_seq] + print("item is (%s)" % str(item)) + self.progress("original x=%d" % item.x) + item.x += int(0.1 * 1e7) + self.progress("new x=%d" % item.x) + self.progress("try to overwrite item %u" % item_seq) + self.mav.mav.mission_write_partial_list_send( + target_system, + target_component, + item_seq, + item_seq, + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, item_seq) + item.pack(self.mav.mav) + self.mav.mav.send(item) + self.progress("Answered request for fence point %u" % item_seq) + + self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + downloaded_items2 = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + if downloaded_items2[item_seq].x != item.x: + raise NotAchievedException("Item did not update") + self.check_fence_items_same([items[0], items[1], item, items[3]], downloaded_items2) + + def test_gcs_fence_boring_triangle(self, target_system=1, target_component=1): + return copy.copy([ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 3, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0000 *1e7), # latitude + int(1.0000 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 3, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0001 *1e7), # latitude + int(1.0000 *1e7), # longitude + 32.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 2, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 3, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0001 *1e7), # latitude + int(1.0001 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 3, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.00015 *1e7), # latitude + int(1.00015 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ]) + + def test_gcs_fence_need_centroid(self, target_system=1, target_component=1): + return copy.copy([ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 4, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0000 *1e7), # latitude + int(1.0000 *1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 4, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0002 *1e7), # latitude + int(1.0000 *1e7), # longitude + 32.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 2, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 4, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0002 *1e7), # latitude + int(1.0001 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 3, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + 0, # current + 0, # autocontinue + 4, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0000 *1e7), # latitude + int(1.0001 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ]) + + def click_location_from_item(self, item): + self.mavproxy.send("click %f %f\n" % (item.x*1e-7, item.y*1e-7)) + + def test_gcs_fence_via_mavproxy(self, target_system=1, target_component=1): + self.start_subtest("Fence via MAVProxy") + if not self.mavproxy_can_do_mision_item_protocols(): + return + self.start_subsubtest("fence addcircle") + self.mavproxy.send("fence clear\n") + self.delay_sim_time(1) + radius = 20 + item = self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, + 0, # current + 0, # autocontinue + radius, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 0.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + print("item is (%s)" % str(item)) + self.click_location_from_item(item) + self.mavproxy.send("fence addcircle inc %u\n" % radius) + self.delay_sim_time(1) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + print("downloaded items: %s" % str(downloaded_items)) + self.check_fence_items_same([item], downloaded_items) + + radius_exc = 57.3 + item2 = self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + 0, # current + 0, # autocontinue + radius_exc, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0017 *1e7), # latitude + int(1.0017 *1e7), # longitude + 0.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.click_location_from_item(item2) + self.mavproxy.send("fence addcircle exc %f\n" % radius_exc) + self.delay_sim_time(1) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + print("downloaded items: %s" % str(downloaded_items)) + self.check_fence_items_same([item, item2], downloaded_items) + self.end_subsubtest("fence addcircle") + + self.start_subsubtest("fence addpoly") + self.mavproxy.send("fence clear\n") + self.delay_sim_time(1) + pointcount = 7 + self.mavproxy.send("fence addpoly inc 20 %u 37.2\n" % pointcount) # radius, pointcount, rotaiton + self.delay_sim_time(5) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + if len(downloaded_items) != pointcount: + raise NotAchievedException("Did not get expected number of points returned (want=%u got=%u)" % (pointcount, len(downloaded_items))) + self.end_subsubtest("fence addpoly") + + self.start_subsubtest("fence movepolypoint") + self.mavproxy.send("fence clear\n") + self.delay_sim_time(1) + triangle = self.test_gcs_fence_boring_triangle(target_system=target_system, + target_component=target_component) + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + triangle) + self.mavproxy.send("fence list\n") + self.delay_sim_time(1) + triangle[2].x += 500 + triangle[2].y += 700 + self.click_location_from_item(triangle[2]) + self.mavproxy.send("fence movepolypoint 0 2\n") + self.delay_sim_time(10) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.check_fence_items_same(triangle, downloaded_items) + self.end_subsubtest("fence movepolypoint") + + self.start_subsubtest("fence enable and disable") + self.mavproxy.send("fence enable\n") + self.mavproxy.expect("fence enabled") + self.mavproxy.send("fence disable\n") + self.mavproxy.expect("fence disabled") + self.end_subsubtest("fence enable and disable") + +# MANUAL> usage: fence def test_gcs_fence(self): target_system = 1 target_component = 1 self.progress("Testing FENCE_POINT protocol") - self.set_parameter("FENCE_TOTAL", 1) - self.roundtrip_fencepoint_protocol(0, 1, 1.2345, 5.4321, target_system=target_system, target_component=target_component) + self.start_subtest("FENCE_TOTAL manipulation") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE); + self.assert_parameter_value("FENCE_TOTAL", 0) + + self.set_parameter("FENCE_TOTAL", 5) + self.assert_parameter_value("FENCE_TOTAL", 5) + + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE); + self.assert_parameter_value("FENCE_TOTAL", 0) + + self.progress("sending out-of-range fencepoint") + self.send_fencepoint_expect_statustext(0, + 0, + 1.2345, + 5.4321, + "index past total", + target_system=target_component, + target_component=target_component) + + self.progress("sending another out-of-range fencepoint") + self.send_fencepoint_expect_statustext(0, + 1, + 1.2345, + 5.4321, + "bad count", + target_system=target_component, + target_component=target_component) + + self.set_parameter("FENCE_TOTAL", 1) + self.assert_parameter_value("FENCE_TOTAL", 1) + + self.send_fencepoint_expect_statustext(0, + 1, + 1.2345, + 5.4321, + "Invalid FENCE_TOTAL", + target_system=target_component, + target_component=target_component) + + self.set_parameter("FENCE_TOTAL", 5) + self.progress("Checking default points") + for i in range(5): + m = self.get_fence_point(i) + if m.count != 5: + raise NotAchievedException("Unexpected count in fence point (want=%u got=%u" % + (5, m.count)) + if m.lat != 0 or m.lng != 0: + raise NotAchievedException("Unexpected lat/lon in fencepoint") + + self.progress("Storing a return point") + self.roundtrip_fencepoint_protocol(0, 5, 1.2345, 5.4321, target_system=target_system, target_component=target_component) lat = 2.345 lng = 4.321 - self.roundtrip_fencepoint_protocol(0, 1, lat, lng, target_system=target_system, target_component=target_component) + self.roundtrip_fencepoint_protocol(0, 5, lat, lng, target_system=target_system, target_component=target_component) + + if not self.mavproxy_can_do_mision_item_protocols(): + self.progress("MAVProxy too old to do fence point protocols") + return + + self.progress("Download with new protocol") + items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + if len(items) != 1: + raise NotAchievedException("Unexpected fencepoint count (want=%u got=%u)" % (1, len(items))) + if items[0].command != mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT: + raise NotAchievedException("Fence return point not of correct type expected (%u) got %u" % (items[0].command, mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT)) + if items[0].frame != mavutil.mavlink.MAV_FRAME_GLOBAL: + raise NotAchievedException("Unexpected frame want=%s got=%s," % + (mavutil.mavlink.enums["MAV_FRAME"][mavutil.mavlink.MAV_FRAME_GLOBAL].name, + mavutil.mavlink.enums["MAV_FRAME"][items[0].frame].name,)) + got_lat = items[0].x + want_lat = lat * 1e7 + if abs(got_lat - want_lat) > 1: + raise NotAchievedException("Disagree in lat (got=%f want=%f)" % (got_lat, want_lat)) + if abs(items[0].y - lng * 1e7) > 1: + raise NotAchievedException("Disagree in lng") + if items[0].seq != 0: + raise NotAchievedException("Disagree in offset") + self.progress("Downloaded with new protocol OK") + + # upload using mission protocol: + items = self.test_gcs_fence_boring_triangle( + target_system=target_system, + target_component=target_component) + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + items) + + self.progress("Download with new protocol") + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + if len(downloaded_items) != len(items): + raise NotAchievedException("Did not download expected number of items (wanted=%u got=%u)" % + (len(items), len(downloaded_items))) + self.assert_parameter_value("FENCE_TOTAL", len(items) +1) # +1 for closing + self.progress("Ensuring fence items match what we sent up") + self.check_fence_items_same(items, downloaded_items) + + # now check centroid + self.progress("Requesting fence return point") + self.mav.mav.fence_fetch_point_send(target_system, + target_component, + 0) + m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=1) + print("m: %s" % str(m)) + + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + self.progress("Checking count post-nuke") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + self.assert_mission_count_on_link(self.mav, + 0, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + + self.start_subtest("Ensuring bad fences get bounced") + for fence in self.fences_which_should_not_upload(target_system=target_system, target_component=target_component): + (name, items) = fence + self.progress("Ensuring (%s) gets bounced" % (name,)) + self.assert_fence_does_not_upload(items) + + self.start_subtest("Ensuring good fences don't get bounced") + for fence in self.fences_which_should_upload(target_system=target_system, target_component=target_component): + (name, items) = fence + self.progress("Ensuring (%s) gets uploaded" % (name,)) + self.check_fence_upload_download(items) + self.progress("(%s) uploaded just fine" % (name,)) + + self.test_gcs_fence_update_fencepoint(target_system=target_system, + target_component=target_component) + + self.test_gcs_fence_centroid(target_system=target_system, + target_component=target_component) + + self.test_gcs_fence_via_mavproxy(target_system=target_system, + target_component=target_component) + + # explode the write_type_to_storage method + # FIXME: test converting invalid fences / minimally valid fences / normal fences + # FIXME: show that uploading smaller items take up less space + # FIXME: add test for consecutive breaches within the manual recovery period + # FIXME: ensure truncation does the right thing by fence_total + + # FIXME: test vehicle escape from outside inclusion zones to + # inside inclusion zones (and inside exclusion zones to outside + # exclusion zones) + # FIXME: add test that a fence with edges that cross can't be uploaded + # FIXME: add a test that fences enclose an area (e.g. all the points aren't the same value! def test_offboard(self, timeout=90): self.load_mission("rover-guided-mission.txt") @@ -1144,12 +2069,34 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) if not mav.mavlink20(): raise NotAchievedException("Not doing mavlink2") - tstart = self.get_sim_time() mav.mav.mission_request_list_send(target_system, target_component, mission_type) + self.assert_receive_mission_count_on_link(mav, + expected_count, + target_system, + target_component, + mission_type) + + def assert_receive_mission_count_on_link(self, + mav, + expected_count, + target_system, + target_component, + mission_type, + expected_target_system=None, + expected_target_component=None, + timeout=10): + if expected_target_system is None: + expected_target_system = mav.mav.srcSystem + if expected_target_component is None: + expected_target_component = mav.mav.srcComponent + self.progress("Waiting for mission count of (%u) from (%u:%u) to (%u:%u)" % + (expected_count, target_system, target_component, expected_target_system, expected_target_component)) + + tstart = self.get_sim_time_cached() while True: - if self.get_sim_time_cached() - tstart > 10: + if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Did not receive MISSION_COUNT on link") m = mav.recv_match(blocking=True, timeout=1) if m is None: @@ -1158,14 +2105,21 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.progress("Received (%s)" % str(m)) if m.get_type() == "MISSION_ACK": if m.type != mavutil.mavlink.MAV_MISSION_ACCEPTED: - raise NotAchievedException("Expected MISSION_COUNT, got (%s)" % m) + raise NotAchievedException("Expected MAV_MISSION_ACCEPTED, got (%s)" % m) if m.get_type() == "MISSION_COUNT": break + if m.target_system != expected_target_system: + raise NotAchievedException("Incorrect target system in MISSION_COUNT (want=%u got=%u)" % + (expected_target_system, m.target_system)) + if m.target_component != expected_target_component: + raise NotAchievedException("Incorrect target component in MISSION_COUNT") if m.mission_type != mission_type: raise NotAchievedException("Did not get expected mission type (want=%u got=%u)" % (mission_type, m.mission_type)) if m.count != expected_count: raise NotAchievedException("Bad count received (want=%u got=%u)" % (expected_count, m.count)) + self.progress("Asserted mission count (type=%u) is %u" % ( + (mission_type, m.count))) def get_mission_item_int_on_link(self, item, mav, target_system, target_component, mission_type): mav.mav.mission_request_int_send(target_system, @@ -1217,26 +2171,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) (m.target_component, mav.mav.srcComponent)) return m - def clear_mission(self, mission_type, target_system=1, target_component=1): - self.mav.mav.mission_count_send(target_system, - target_component, - 0, - mission_type) - m = self.mav.recv_match(type='MISSION_ACK', - blocking=True, - timeout=5) - if m is None: - raise NotAchievedException("Expected ACK for clearing mission") - if m.target_system != self.mav.mav.srcSystem: - raise NotAchievedException("ACK not targetted at correct system want=%u got=%u" % - (self.mav.mav.srcSystem, m.target_system)) - if m.target_component != self.mav.mav.srcComponent: - raise NotAchievedException("ACK not targetted at correct component want=%u got=%u" % - (self.mav.mav.srcComponent, m.target_component)) - if m.type != mavutil.mavlink.MAV_MISSION_ACCEPTED: - raise NotAchievedException("Expected MAV_MISSION_ACCEPTED got %s" % - (mavutil.mavlink.enums["MAV_MISSION_RESULT"][m.type].name,)) - def assert_receive_mission_item_request(self, mission_type, seq): self.progress("Expecting request for item %u" % seq) m = self.mav.recv_match(type='MISSION_REQUEST', @@ -1271,7 +2205,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) raise NotAchievedException("Expected mission ACK") if m.target_system != target_system: raise NotAchievedException("ACK not targetted at correct system want=%u got=%u" % - (mav.mav.srcSystem, m.target_system)) + (target_system, m.target_system)) if m.target_component != target_component: raise NotAchievedException("ACK not targetted at correct component") if m.mission_type != mission_type: @@ -1281,9 +2215,481 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) raise NotAchievedException("Expected ack type got %u got %u" % (want_type, m.type)) + def assert_filepath_content(self, filepath, want): + with open(filepath) as f: + got = f.read() + if want != got: + raise NotAchievedException("Did not get expected file content (want=%s) (got=%s)" % (want, got)) + + def mavproxy_can_do_mision_item_protocols(self): + mavproxy_version = self.mavproxy_version() + if not self.mavproxy_version_gt(1, 8, 12): + self.progress("MAVProxy is too old; skipping tests") + return False + return True + + def check_rally_items_same(self, want, got, epsilon=None): + check_atts = ['mission_type', 'command', 'x', 'y', 'z', 'seq', 'param1'] + return self.check_mission_items_same(check_atts, want, got, epsilon=epsilon) + + def click_three_in(self, target_system=1, target_component=1): + self.mavproxy.send('rally clear\n') + self.drain_mav_unparsed() + # there are race conditions in MAVProxy. Beware. + self.mavproxy.send("click 1.0 1.0\n") + self.mavproxy.send("rally add\n") + self.delay_sim_time(1) + self.mavproxy.send("click 2.0 2.0\n") + self.mavproxy.send("rally add\n") + self.delay_sim_time(1) + self.mavproxy.send("click 3.0 3.0\n") + self.mavproxy.send("rally add\n") + self.delay_sim_time(10) + self.assert_mission_count_on_link( + self.mav, + 3, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + ) + + def test_gcs_rally_via_mavproxy(self, target_system=1, target_component=1): + self.start_subtest("Testing mavproxy CLI for rally points") + if not self.mavproxy_can_do_mision_item_protocols(): + return + + self.start_subsubtest("rally add") + self.mavproxy.send('rally clear\n') + lat_s = "-5.6789" + lng_s = "98.2341" + lat = float(lat_s) + lng = float(lng_s) + self.mavproxy.send('click %s %s\n' % (lat_s, lng_s)) + self.drain_mav_unparsed() + self.mavproxy.send('rally add\n') + self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + target_system=255, + target_component=0) + self.delay_sim_time(5) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if len(downloaded_items) != 1: + raise NotAchievedException("Unexpected count (got=%u want=1)" % + (len(downloaded_items), )) + if (downloaded_items[0].x - int(lat *1e7)) > 1: + raise NotAchievedException("Bad rally lat. Want=%d got=%d" % + (int(lat*1e7), downloaded_items[0].x)) + if (downloaded_items[0].y - int(lng *1e7)) > 1: + raise NotAchievedException("Bad rally lng. Want=%d got=%d" % + (int(lng*1e7), downloaded_items[0].y)) + if (downloaded_items[0].z - int(90)) > 1: + raise NotAchievedException("Bad rally alt. Want=90 got=%d" % + (downloaded_items[0].y)) + self.end_subsubtest("rally add") + + self.start_subsubtest("rally list") + util.pexpect_drain(self.mavproxy) + self.mavproxy.send('rally list\n') + self.mavproxy.expect("Saved 1 rally items to ([^\s]*)\s") + filename = self.mavproxy.match.group(1) + self.assert_rally_filepath_content(filename, '''QGC WPL 110 +0 0 3 5100 0.000000 0.000000 0.000000 0.000000 -5.678900 98.234100 90.000000 0 +''') + self.end_subsubtest("rally list") + + self.start_subsubtest("rally save") + util.pexpect_drain(self.mavproxy) + save_tmppath = self.buildlogs_path("rally-testing-tmp.txt") + self.mavproxy.send('rally save %s\n' % save_tmppath) + self.mavproxy.expect("Saved 1 rally items to ([^\s]*)\s") + filename = self.mavproxy.match.group(1) + if filename != save_tmppath: + raise NotAchievedException("Bad save filepath; want=%s got=%s" % (save_tmppath, filename)) + self.assert_rally_filepath_content(filename, '''QGC WPL 110 +0 0 3 5100 0.000000 0.000000 0.000000 0.000000 -5.678900 98.234100 90.000000 0 +''') + self.end_subsubtest("rally save") + + + self.start_subsubtest("rally savecsv") + util.pexpect_drain(self.mavproxy) + csvpath = self.buildlogs_path("rally-testing-tmp.csv") + self.mavproxy.send('rally savecsv %s\n' % csvpath) + self.mavproxy.expect('"Seq","Frame"') + expected_content = '''"Seq","Frame","Cmd","P1","P2","P3","P4","X","Y","Z" +"0","Rel","NAV_RALLY_POINT","0.0","0.0","0.0","0.0","-5.67890024185","98.2341003418","90.0" +''' + if sys.version_info[0] >= 3: + # greater precision output by default + expected_content = '''"Seq","Frame","Cmd","P1","P2","P3","P4","X","Y","Z" +"0","Rel","NAV_RALLY_POINT","0.0","0.0","0.0","0.0","-5.678900241851807","98.23410034179688","90.0" +''' + self.assert_filepath_content(csvpath, expected_content) + + self.end_subsubtest("rally savecsv") + + self.start_subsubtest("rally load") + self.drain_mav() + self.mavproxy.send('rally clear\n') + self.assert_mission_count_on_link(self.mav, + 0, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + + # warning: uses file saved from previous test + self.start_subtest("Check rally load from filepath") + self.mavproxy.send('rally load %s\n' % save_tmppath) + self.mavproxy.expect("Loaded 1 rally items from ([^\s]*)\s") + self.mavproxy.expect("Sent all .* rally items") # notional race condition here + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if len(downloaded_items) != 1: + raise NotAchievedException("Unexpected item count (%u)" % len(downloaded_items)) + if abs(int(downloaded_items[0].x) - int(lat*1e7)) > 3: + raise NotAchievedException("Expected lat=%d got=%d" % (lat*1e7, downloaded_items[0].x)) + if abs(int(downloaded_items[0].y) - int(lng*1e7)) > 10: + raise NotAchievedException("Expected lng=%d got=%d" % (lng*1e7, downloaded_items[0].y)) + self.end_subsubtest("rally load") + + self.start_subsubtest("rally changealt") + self.mavproxy.send('rally clear\n') + self.mavproxy.send("click 1.0 1.0\n") + self.mavproxy.send("rally add\n") + self.mavproxy.send("click 2.0 2.0\n") + self.mavproxy.send("rally add\n") + self.delay_sim_time(10) + self.assert_mission_count_on_link( + self.mav, + 2, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + ) + self.drain_mav() + self.mavproxy.send("rally changealt 1 17.6\n") + self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + target_system=255, + target_component=0) + self.delay_sim_time(10) + self.mavproxy.send("rally changealt 2 19.1\n") + self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + target_system=255, + target_component=0) + self.delay_sim_time(10) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if len(downloaded_items) != 2: + raise NotAchievedException("Unexpected item count (%u)" % len(downloaded_items)) + if abs(int(downloaded_items[0].x) - int(1*1e7)) > 3: + raise NotAchievedException("Expected lat=%d got=%d" % (1*1e7, downloaded_items[0].x)) + if abs(int(downloaded_items[0].y) - int(1*1e7)) > 10: + raise NotAchievedException("Expected lng=%d got=%d" % (1*1e7, downloaded_items[0].y)) + # at some stage ArduPilot will stop rounding altitude. This + # will break then. + if abs(int(downloaded_items[0].z) - int(17.6)) > 0.0001: + raise NotAchievedException("Expected alt=%f got=%f" % (17.6, downloaded_items[0].z)) + + if abs(int(downloaded_items[1].x) - int(2*1e7)) > 3: + raise NotAchievedException("Expected lat=%d got=%d" % (2*1e7, downloaded_items[0].x)) + if abs(int(downloaded_items[1].y) - int(2*1e7)) > 10: + raise NotAchievedException("Expected lng=%d got=%d" % (2*1e7, downloaded_items[0].y)) + # at some stage ArduPilot will stop rounding altitude. This + # will break then. + if abs(int(downloaded_items[1].z) - int(19.1)) > 0.0001: + raise NotAchievedException("Expected alt=%f got=%f" % (19.1, downloaded_items[1].z)) + + self.progress("Now change two at once") + self.mavproxy.send("rally changealt 1 17.3 2\n") + self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + target_system=255, + target_component=0) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if len(downloaded_items) != 2: + raise NotAchievedException("Unexpected item count (%u)" % len(downloaded_items)) + + if abs(int(downloaded_items[0].x) - int(1*1e7)) > 3: + raise NotAchievedException("Expected lat=%d got=%d" % (1*1e7, downloaded_items[0].x)) + if abs(int(downloaded_items[0].y) - int(1*1e7)) > 10: + raise NotAchievedException("Expected lng=%d got=%d" % (1*1e7, downloaded_items[0].y)) + # at some stage ArduPilot will stop rounding altitude. This + # will break then. + if abs(int(downloaded_items[0].z) - int(17.3)) > 0.0001: + raise NotAchievedException("Expected alt=%f got=%f" % (17.3, downloaded_items[0].z)) + + if abs(int(downloaded_items[1].x) - int(2*1e7)) > 3: + raise NotAchievedException("Expected lat=%d got=%d" % (2*1e7, downloaded_items[0].x)) + if abs(int(downloaded_items[1].y) - int(2*1e7)) > 10: + raise NotAchievedException("Expected lng=%d got=%d" % (2*1e7, downloaded_items[0].y)) + # at some stage ArduPilot will stop rounding altitude. This + # will break then. + if abs(int(downloaded_items[1].z) - int(17.3)) > 0.0001: + raise NotAchievedException("Expected alt=%f got=%f" % (17.3, downloaded_items[0].z)) + + self.end_subsubtest("rally changealt") + + self.start_subsubtest("rally move") + self.mavproxy.send('rally clear\n') + self.mavproxy.send("click 1.0 1.0\n") + self.mavproxy.send("rally add\n") + self.mavproxy.send("click 2.0 2.0\n") + self.mavproxy.send("rally add\n") + self.delay_sim_time(5) + self.assert_mission_count_on_link( + self.mav, + 2, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + ) + self.mavproxy.send("click 3.0 3.0\n") + self.mavproxy.send("rally move 2\n") + self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + target_system=255, + target_component=0) + self.mavproxy.send("click 4.12345 4.987654\n") + self.mavproxy.send("rally move 1\n") + self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + target_system=255, + target_component=0) + + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if len(downloaded_items) != 2: + raise NotAchievedException("Unexpected item count (%u)" % len(downloaded_items)) + if downloaded_items[0].x != 41234500: + raise NotAchievedException("Bad latitude") + if downloaded_items[0].y != 49876540: + raise NotAchievedException("Bad longitude") + if downloaded_items[0].z != 90: + raise NotAchievedException("Bad altitude (want=%u got=%u)" % + (90, downloaded_items[0].z)) + + if downloaded_items[1].x != 30000000: + raise NotAchievedException("Bad latitude") + if downloaded_items[1].y != 30000000: + raise NotAchievedException("Bad longitude") + if downloaded_items[1].z != 90: + raise NotAchievedException("Bad altitude (want=%u got=%u)" % + (90, downloaded_items[1].z)) + self.end_subsubtest("rally move") + + self.start_subsubtest("rally movemulti") + self.drain_mav_unparsed() + self.mavproxy.send('rally clear\n') + self.drain_mav_unparsed() + # there are race conditions in MAVProxy. Beware. + self.mavproxy.send("click 1.0 1.0\n") + self.mavproxy.send("rally add\n") + self.mavproxy.send("click 2.0 2.0\n") + self.mavproxy.send("rally add\n") + self.mavproxy.send("click 3.0 3.0\n") + self.mavproxy.send("rally add\n") + self.delay_sim_time(10) + self.assert_mission_count_on_link( + self.mav, + 3, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + ) + click_lat = 2.0 + click_lon = 3.0 + unmoved_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if len(unmoved_items) != 3: + raise NotAchievedException("Unexpected item count") + self.mavproxy.send("click %f %f\n" % (click_lat, click_lon)) + self.mavproxy.send("rally movemulti 2 1 3\n") + # MAVProxy currently sends three separate items up. That's + # not great and I don't want to lock that behaviour in here. + self.delay_sim_time(10) + self.drain_mav_unparsed() + expected_moved_items = copy.copy(unmoved_items) + expected_moved_items[0].x = 1.0 * 1e7 + expected_moved_items[0].y = 2.0 * 1e7 + expected_moved_items[1].x = 2.0 * 1e7 + expected_moved_items[1].y = 3.0 * 1e7 + expected_moved_items[2].x = 3.0 * 1e7 + expected_moved_items[2].y = 4.0 * 1e7 + moved_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + # we're moving an entire degree in latitude; quite an epsilon required... + self.check_rally_items_same(expected_moved_items, moved_items, epsilon=10000) + + self.progress("now move back and rotate through 90 degrees") + self.mavproxy.send("click %f %f\n" % (2, 2)) + self.mavproxy.send("rally movemulti 2 1 3 90\n") + + # MAVProxy currently sends three separate items up. That's + # not great and I don't want to lock that behaviour in here. + self.delay_sim_time(10) + self.drain_mav_unparsed() + expected_moved_items = copy.copy(unmoved_items) + expected_moved_items[0].x = 3.0 * 1e7 + expected_moved_items[0].y = 1.0 * 1e7 + expected_moved_items[1].x = 2.0 * 1e7 + expected_moved_items[1].y = 2.0 * 1e7 + expected_moved_items[2].x = 1.0 * 1e7 + expected_moved_items[2].y = 3.0 * 1e7 + moved_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + # we're moving an entire degree in latitude; quite an epsilon required... + self.check_rally_items_same(expected_moved_items, moved_items, epsilon=12000) + self.end_subsubtest("rally movemulti") + + self.start_subsubtest("rally param") + self.mavproxy.send("rally param 3 2 5\n") + self.mavproxy.expect("Set param 2 for 3 to 5.000000") + self.end_subsubtest("rally param") + + self.start_subsubtest("rally remove") + self.click_three_in(target_system=target_system, target_component=target_component) + pure_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + self.progress("Removing last in list") + self.mavproxy.send("rally remove 3\n") + self.delay_sim_time(10) + self.assert_mission_count_on_link( + self.mav, + 2, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + ) + fewer_downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if len(fewer_downloaded_items) != 2: + raise NotAchievedException("Unexpected download list length") + shorter_items = copy.copy(pure_items) + shorter_items = shorter_items[0:2] + self.check_rally_items_same(shorter_items, fewer_downloaded_items) + + self.progress("Removing first in list") + self.mavproxy.send("rally remove 1\n") + self.delay_sim_time(5) + self.drain_mav_unparsed() + self.assert_mission_count_on_link( + self.mav, + 1, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + ) + fewer_downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if len(fewer_downloaded_items) != 1: + raise NotAchievedException("Unexpected download list length") + shorter_items = shorter_items[1:] + self.check_rally_items_same(shorter_items, fewer_downloaded_items) + + self.progress("Removing remaining item") + self.mavproxy.send("rally remove 1\n") + self.delay_sim_time(5) + self.drain_mav_unparsed() + self.assert_mission_count_on_link( + self.mav, + 0, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + ) + self.end_subsubtest("rally remove") + + self.start_subsubtest("rally show") + # what can we test here? + self.mavproxy.send("rally show %s\n" % save_tmppath) + self.end_subsubtest("rally show") + + # savelocal must be run immediately after show! + self.start_subsubtest("rally savelocal") + util.pexpect_drain(self.mavproxy) + savelocal_path = self.buildlogs_path("rally-testing-tmp-local.txt") + self.mavproxy.send('rally savelocal %s\n' % savelocal_path) + self.delay_sim_time(5) + self.assert_rally_filepath_content(savelocal_path, '''QGC WPL 110 +0 0 3 5100 0.000000 0.000000 0.000000 0.000000 -5.678900 98.234100 90.000000 0 +''') + self.end_subsubtest("rally savelocal") + + self.start_subsubtest("rally status") + self.click_three_in(target_system=target_system, target_component=target_component) + self.mavproxy.send("rally status\n") + self.mavproxy.expect("Have 3 of 3 rally items") + self.mavproxy.send("rally clear\n") + self.mavproxy.send("rally status\n") + self.mavproxy.expect("Have 0 of 0 rally items") + self.end_subsubtest("rally status") + + self.start_subsubtest("rally undo") + self.progress("Testing undo-remove") + self.click_three_in(target_system=target_system, target_component=target_component) + pure_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + self.progress("Removing first in list") + self.mavproxy.send("rally remove 1\n") + self.delay_sim_time(5) + self.drain_mav_unparsed() + self.assert_mission_count_on_link( + self.mav, + 2, + target_system, + target_component, + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + ) + self.mavproxy.send("rally undo\n") + self.delay_sim_time(5) + undone_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + self.check_rally_items_same(pure_items, undone_items) + + self.progress("Testing undo-move") + + pure_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + self.mavproxy.send("click 4.12345 4.987654\n") + self.mavproxy.send("rally move 1\n") + # move has already been tested, assume it works... + self.delay_sim_time(5) + self.drain_mav_unparsed() + self.mavproxy.send("rally undo\n") + self.delay_sim_time(5) + self.drain_mav_unparsed() + undone_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + self.check_rally_items_same(pure_items, undone_items) + + self.end_subsubtest("rally undo") + + self.start_subsubtest("rally update") + self.click_three_in(target_system=target_system, target_component=target_component) + pure_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + rally_update_tmpfilepath= self.buildlogs_path("rally-tmp-update.txt") + self.mavproxy.send("rally save %s\n" % rally_update_tmpfilepath) + self.delay_sim_time(5) + self.progress("Moving waypoint") + self.mavproxy.send("click 13.0 13.0\n") + self.mavproxy.send("rally move 1\n") + self.delay_sim_time(5) + self.progress("Reverting to original") + self.mavproxy.send("rally update %s\n" % rally_update_tmpfilepath) + self.delay_sim_time(5) + reverted_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + self.check_rally_items_same(pure_items, reverted_items) + + self.progress("Making sure specifying a waypoint to be updated works") + self.mavproxy.send("click 13.0 13.0\n") + self.mavproxy.send("rally move 1\n") + self.delay_sim_time(5) + self.mavproxy.send("click 17.0 17.0\n") + self.mavproxy.send("rally move 2\n") + self.delay_sim_time(5) + self.progress("Reverting to original item 2") + self.mavproxy.send("rally update %s 2\n" % rally_update_tmpfilepath) + self.delay_sim_time(5) + reverted_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + if reverted_items[0].x != 130000000: + raise NotAchievedException("Expected item1 x to stay changed (got=%u want=%u)" % (reverted_items[0].x, 130000000)) + if reverted_items[1].x == 170000000: + raise NotAchievedException("Expected item2 x to revert") + + self.end_subsubtest("rally update") + +# MANUAL> usage: rally + def test_gcs_rally(self): target_system = 1 target_component = 1 + + self.test_gcs_rally_via_mavproxy(target_system=target_system, + target_component=target_component) + self.mavproxy.send('rally clear\n') self.delay_sim_time(1) if self.get_parameter("RALLY_TOTAL") != 0: @@ -1296,6 +2702,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.mavproxy.expect("Unloaded module rally") self.mavproxy.send('module unload wp\n') self.mavproxy.expect("Unloaded module wp") + self.drain_mav() try: item1_lat = int(2.0000 *1e7) items = [ @@ -1424,7 +2831,11 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) want_type=mavutil.mavlink.MAV_MISSION_DENIED) # wait for the upload from sysid=1 to time out: - self.mavproxy.expect("upload timeout") + self.wait_text("upload timeout") + self.assert_receive_mission_ack( + mavutil.mavlink.MAV_MISSION_TYPE_RALLY, + want_type=mavutil.mavlink.MAV_MISSION_OPERATION_CANCELLED, + target_system=old_srcSystem) self.progress("Now trying to upload empty mission after timeout") self.mav.mav.mission_count_send(target_system, @@ -1764,9 +3175,150 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.mavproxy.send('module load wp\n') self.mavproxy.expect("Loaded module wp") - def wait_distance_to_home(self, distance, accuracy=5): + def test_gcs_mission(self): + target_system = 1 + target_component = 1 + self.mavproxy.send('wp clear\n') + self.delay_sim_time(1) + if self.get_parameter("MIS_TOTAL") != 0: + raise NotAchievedException("Failed to clear mission") + self.drain_mav_unparsed() + m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True, timeout=5) + if m is None: + raise NotAchievedException("Did not get expected MISSION_CURRENT") + if m.seq != 0: + raise NotAchievedException("Bad mission current") + self.load_mission("rover-gripper-mission.txt") + set_wp = 1 + self.mavproxy.send('wp set %u\n' % set_wp) + self.drain_mav() + m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True, timeout=5) + if m is None: + raise NotAchievedException("Did not get expected MISSION_CURRENT") + if m.seq != set_wp: + raise NotAchievedException("Bad mission current. want=%u got=%u" % + (set_wp, m.seq)) + + self.start_subsubtest("wp changealt") + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + changealt_item = 1 + oldalt = downloaded_items[changealt_item].z + want_newalt = 37.2 + self.mavproxy.send('wp changealt %u %f\n' % (changealt_item, want_newalt)) + self.delay_sim_time(5) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + if abs(downloaded_items[changealt_item].z - want_newalt) > 0.0001: + raise NotAchievedException( + "changealt didn't (want=%f got=%f)" % + (want_newalt, downloaded_items[changealt_item].z)) + self.end_subsubtest("wp changealt") + + self.start_subsubtest("wp sethome") + new_home_lat = 3.14159 + new_home_lng = 2.71828 + self.mavproxy.send('click %f %f\n' % (new_home_lat, new_home_lng)) + self.mavproxy.send('wp sethome\n') + self.delay_sim_time(5) + # any way to close the loop on this one? + # downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + # if abs(downloaded_items[0].x - new_home_lat) > 0.0001: + # raise NotAchievedException("wp sethome didn't work") + # if abs(downloaded_items[0].y - new_home_lng) > 0.0001: + # raise NotAchievedException("wp sethome didn't work") + self.end_subsubtest("wp sethome") + + self.start_subsubtest("wp slope") + self.mavproxy.send('wp slope\n') + self.mavproxy.expect("WP3: slope 0.1") + self.delay_sim_time(5) + self.end_subsubtest("wp slope") + + if not self.mavproxy_can_do_mision_item_protocols(): + # adding based on click location yet to be merged into MAVProxy + return + + self.start_subsubtest("wp split") + self.mavproxy.send("wp clear\n") + self.delay_sim_time(5) + self.mavproxy.send("wp list\n") + self.delay_sim_time(5) + items = [ + None, + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0 *1e7), # latitude + int(1.0 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_MISSION), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 2, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(2.0 *1e7), # latitude + int(2.0 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_MISSION), + ] + self.mavproxy.send("click 5 5\n") # space for home position + self.mavproxy.send("wp add\n") + self.delay_sim_time(5) + self.click_location_from_item(items[1]) + self.mavproxy.send("wp add\n") + self.delay_sim_time(5) + self.click_location_from_item(items[2]) + self.mavproxy.send("wp add\n") + self.delay_sim_time(5) + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + self.check_mission_waypoint_items_same(items, downloaded_items) + self.mavproxy.send("wp split 2\n") + self.delay_sim_time(5) + items_with_split_in = [ + items[0], + items[1], + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 2, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.5 *1e7), # latitude + int(1.5 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_MISSION), + items[2], + ] + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + self.check_mission_waypoint_items_same(items_with_split_in, + downloaded_items) + + # MANUAL> usage: wp + + def wait_distance_to_home(self, distance, accuracy=5, timeout=30): tstart = self.get_sim_time() - timeout = 30 while True: if self.get_sim_time_cached() - tstart > timeout: raise AutoTestTimeoutException("Failed to get home") @@ -1775,6 +3327,884 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) if abs(distance-self.distance_to_home(use_cached_home=True)) <= accuracy: break + def wait_location_sending_target(self, loc, target_system=1, target_component=1, timeout=60, max_delta=2): + tstart = self.get_sim_time() + last_sent = 0 + + type_mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) + + self.change_mode('GUIDED') + tstart = self.get_sim_time() + while True: + now = self.get_sim_time_cached() + if now - tstart > timeout: + raise AutoTestTimeoutException("Did not get to location") + if now - last_sent > 1: + last_sent = now + self.mav.mav.set_position_target_global_int_send( + 0, + target_system, + target_component, + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + type_mask, + int(loc.lat*1.0e7), + int(loc.lng*1.0e7), + 0, # alt + 0, # x-ve + 0, # y-vel + 0, # z-vel + 0, # afx + 0, # afy + 0, # afz + 0, # yaw, + 0, # yaw-rate + ) + m = self.mav.recv_match(blocking=True, + timeout=1) + if m is None: + continue + t = m.get_type() + if t == "POSITION_TARGET_GLOBAL_INT": + self.progress("Target: (%s)" % str(m)) + elif t == "GLOBAL_POSITION_INT": + self.progress("Position: (%s)" % str(m)) + delta = self.get_distance(mavutil.location(m.lat*1e-7, m.lon*1e-7, 0, 0), + loc) + self.progress("delta: %s" % str(delta)) + if delta < max_delta: + self.progress("Reached destination") + + def drive_somewhere_breach_boundary_and_rtl(self, loc, target_system=1, target_component=1, timeout=60): + tstart = self.get_sim_time() + last_sent = 0 + seen_fence_breach = False + + type_mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) + + self.change_mode('GUIDED') + while True: + now = self.get_sim_time_cached() + if now - tstart > timeout: + raise NotAchievedException("Did not breach boundary + RTL") + if now - last_sent > 1: + last_sent = now + self.mav.mav.set_position_target_global_int_send( + 0, + target_system, + target_component, + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + type_mask, + int(loc.lat*1.0e7), + int(loc.lng*1.0e7), + 0, # alt + 0, # x-ve + 0, # y-vel + 0, # z-vel + 0, # afx + 0, # afy + 0, # afz + 0, # yaw, + 0, # yaw-rate + ) + m = self.mav.recv_match(blocking=True, + timeout=1) + if m is None: + continue + t = m.get_type() + if t == "POSITION_TARGET_GLOBAL_INT": + print("Target: (%s)" % str(m)) + elif t == "GLOBAL_POSITION_INT": + print("Position: (%s)" % str(m)) + elif t == "FENCE_STATUS": + print("Fence: %s" % str(m)) + if m.breach_status != 0: + seen_fence_breach = True + self.progress("Fence breach detected!") + if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY: + raise NotAchievedException("Breach of unexpected type") + if self.mode_is("RTL", cached=True) and seen_fence_breach: + break + self.wait_distance_to_home(5, accuracy=2) + + def drive_somewhere_stop_at_boundary(self, + loc, + expected_stopping_point, + expected_distance_epsilon=1, + target_system=1, + target_component=1, + timeout=120): + tstart = self.get_sim_time() + last_sent = 0 + seen_fence_breach = False + + type_mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE + + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) + + self.change_mode('GUIDED') + at_stopping_point = False + while True: + now = self.get_sim_time_cached() + if now - tstart > timeout: + raise NotAchievedException("Did not arrive and stop at boundary") + if now - last_sent > 1: + last_sent = now + self.mav.mav.set_position_target_global_int_send( + 0, + target_system, + target_component, + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + type_mask, + int(loc.lat*1.0e7), + int(loc.lng*1.0e7), + 0, # alt + 0, # x-ve + 0, # y-vel + 0, # z-vel + 0, # afx + 0, # afy + 0, # afz + 0, # yaw, + 0, # yaw-rate + ) + m = self.mav.recv_match(blocking=True, + timeout=1) + if m is None: + continue + t = m.get_type() + if t == "POSITION_TARGET_GLOBAL_INT": + print("Target: (%s)" % str(m)) + elif t == "GLOBAL_POSITION_INT": + print("Position: (%s)" % str(m)) + delta = self.get_distance(mavutil.location(m.lat*1e-7, m.lon*1e-7, 0, 0), + mavutil.location(expected_stopping_point.lat, expected_stopping_point.lng, 0, 0)) + print("delta: %s want_delta<%f" % (str(delta), expected_distance_epsilon)) + at_stopping_point = delta < expected_distance_epsilon + + if at_stopping_point: + if t == "VFR_HUD": + print("groundspeed: %f" % m.groundspeed) + if m.groundspeed < 1: + self.progress("Seemed to have stopped at stopping point") + return + + def assert_fence_breached(self): + m = self.mav.recv_match(type='FENCE_STATUS', + blocking=True, + timeout=10) + if m is None: + raise NotAchievedException("Not receiving fence notifications?") + if m.breach_status != 1: + raise NotAchievedException("Expected to be breached") + + def wait_fence_not_breached(self, timeout=5): + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise AutoTestTimeoutException("Fence remains breached") + m = self.mav.recv_match(type='FENCE_STATUS', + blocking=True, + timeout=1) + if m is None: + self.progress("No FENCE_STATUS received") + continue + self.progress("STATUS: %s" % str(m)) + if m.breach_status == 0: + break + + def test_poly_fence_noarms(self, target_system=1, target_component=1): + '''various tests to ensure we can't arm when in breach of a polyfence''' + self.start_subtest("Ensure PolyFence arming checks work") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + self.delay_sim_time(5) # let breaches clear + # FIXME: should we allow this? + self.progress("Ensure we can arm with no poly in place") + self.change_mode("GUIDED") + self.wait_ready_to_arm() + self.arm_vehicle() + self.disarm_vehicle() + + self.test_poly_fence_noarms_exclusion_circle(target_system=target_system, + target_component=target_component) + self.test_poly_fence_noarms_inclusion_circle(target_system=target_system, + target_component=target_component) + self.test_poly_fence_noarms_exclusion_polyfence(target_system=target_system, + target_component=target_component) + self.test_poly_fence_noarms_inclusion_polyfence(target_system=target_system, + target_component=target_component) + + def test_poly_fence_noarms_exclusion_circle(self, target_system=1, target_component=1): + self.start_subtest("Ensure not armable when within an exclusion circle") + + here = self.mav.location() + + items = [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 - radius + 0, # p2 + 0, # p3 + 0, # p4 + int(here.lat*1e7), # latitude + int(here.lng*1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 - radius + 0, # p2 + 0, # p3 + 0, # p4 + int(self.offset_location_ne(here, 100, 100).lat*1e7), # latitude + int(here.lng*1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ] + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + items) + self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz + self.drain_mav() + self.assert_fence_breached() + if self.arm_motors_with_rc_input(): + raise NotAchievedException( + "Armed when within exclusion zone") + + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + []) + self.wait_fence_not_breached() + + def test_poly_fence_noarms_inclusion_circle(self, target_system=1, target_component=1): + self.start_subtest("Ensure not armable when outside an inclusion circle (but within another") + + here = self.mav.location() + + items = [ + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 - radius + 0, # p2 + 0, # p3 + 0, # p4 + int(here.lat*1e7), # latitude + int(here.lng*1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, + 0, # current + 0, # autocontinue + 5, # p1 - radius + 0, # p2 + 0, # p3 + 0, # p4 + int(self.offset_location_ne(here, 100, 100).lat*1e7), # latitude + int(here.lng*1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE), + ]; + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + items) + self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz + self.drain_mav() + self.assert_fence_breached() + if self.arm_motors_with_rc_input(): + raise NotAchievedException( + "Armed when outside an inclusion zone") + + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + []) + self.wait_fence_not_breached() + + def test_poly_fence_noarms_exclusion_polyfence(self, target_system=1, target_component=1): + self.start_subtest("Ensure not armable when inside an exclusion polyfence (but outside another") + + here = self.mav.location() + + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + [ + [ # east + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + ], [ # over the top of the vehicle + self.offset_location_ne(here, -50, -50), # bl + self.offset_location_ne(here, -50, 50), # br + self.offset_location_ne(here, 50, 50), # tr + self.offset_location_ne(here, 50, -50), # tl, + ] + ] + ) + self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz + self.drain_mav() + self.assert_fence_breached() + if self.arm_motors_with_rc_input(): + raise NotAchievedException( + "Armed when within polygon exclusion zone") + + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + []) + self.wait_fence_not_breached() + + def test_poly_fence_noarms_inclusion_polyfence(self, target_system=1, target_component=1): + self.start_subtest("Ensure not armable when outside an inclusion polyfence (but within another") + + here = self.mav.location() + + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + [ + [ # east + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + ], [ # over the top of the vehicle + self.offset_location_ne(here, -50, -50), # bl + self.offset_location_ne(here, -50, 50), # br + self.offset_location_ne(here, 50, 50), # tr + self.offset_location_ne(here, 50, -50), # tl, + ] + ] + ) + self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz + self.drain_mav() + self.assert_fence_breached() + if self.arm_motors_with_rc_input(): + raise NotAchievedException( + "Armed when outside polygon inclusion zone") + + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + []) + self.wait_fence_not_breached() + + def test_fence_upload_timeouts_1(self, target_system=1, target_component=1): + self.progress("Telling victim there will be one item coming") + self.mav.mav.mission_count_send(target_system, + target_component, + 1, + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK'], + blocking=True, + timeout=1) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Did not get ACK or mission request") + + if m.get_type() == "MISSION_ACK": + raise NotAchievedException("Expected MISSION_REQUEST") + + if m.seq != 0: + raise NotAchievedException("Expected request for seq=0") + + if m.target_system != self.mav.mav.srcSystem: + raise NotAchievedException("Incorrect target system in MISSION_REQUEST") + if m.target_component != self.mav.mav.srcComponent: + raise NotAchievedException("Incorrect target component in MISSION_REQUEST") + tstart = self.get_sim_time() + rerequest_count = 0 + while True: + m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK', 'STATUSTEXT'], + blocking=True, + timeout=1) + self.progress("Got (%s)" % str(m)) + if m is None: + self.progress("Did not receive any messages") + continue + if m.get_type() == "MISSION_REQUEST": + if m.seq != 0: + raise NotAchievedException("Received request for invalid seq") + if m.target_system != self.mav.mav.srcSystem: + raise NotAchievedException("Incorrect target system in MISSION_REQUEST") + if m.target_component != self.mav.mav.srcComponent: + raise NotAchievedException("Incorrect target component in MISSION_REQUEST") + rerequest_count += 1 + self.progress("Valid re-request received.") + continue + if m.get_type() == "MISSION_ACK": + raise NotAchievedException("Received unexpected MISSION_ACK") + if m.get_type() == "STATUSTEXT": + if "upload time" in m.text: + break + if rerequest_count < 3: + raise NotAchievedException("Expected several re-requests of mission item") + # timeouts come with an ack: + self.assert_receive_mission_ack( + mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + want_type=mavutil.mavlink.MAV_MISSION_OPERATION_CANCELLED, + ) + + def expect_request_for_item(self, item): + m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK'], + blocking=True, + timeout=1) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Did not get ACK or mission request") + + if m.get_type() == "MISSION_ACK": + raise NotAchievedException("Expected MISSION_REQUEST") + + if m.seq != item.seq: + raise NotAchievedException("Expected request for seq=%u" % item.seq) + + if m.target_system != self.mav.mav.srcSystem: + raise NotAchievedException("Incorrect target system in MISSION_REQUEST") + if m.target_component != self.mav.mav.srcComponent: + raise NotAchievedException("Incorrect target component in MISSION_REQUEST") + + + def test_fence_upload_timeouts_2(self, target_system=1, target_component=1): + self.progress("Telling victim there will be one item coming") + self.mav.mav.mission_count_send(target_system, + target_component, + 2, + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.progress("Sending item with seq=0") + item = self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + 0, # current + 0, # autocontinue + 1, # p1 radius + 0, # p2 + 0, # p3 + 0, # p4 + int(1.1 *1e7), # latitude + int(1.2 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.expect_request_for_item(item) + item.pack(self.mav.mav) + self.mav.mav.send(item) + + item = self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + 0, # current + 0, # autocontinue + 1, # p1 radius + 0, # p2 + 0, # p3 + 0, # p4 + int(1.1 *1e7), # latitude + int(1.2 *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + + self.expect_request_for_item(item) + + self.progress("Now waiting for a timeout") + tstart = self.get_sim_time() + rerequest_count = 0 + while True: + m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK', 'STATUSTEXT'], + blocking=True, + timeout=0.1) + self.progress("Got (%s)" % str(m)) + if m is None: + self.progress("Did not receive any messages") + continue + if m.get_type() == "MISSION_REQUEST": + if m.seq != 1: + raise NotAchievedException("Received request for invalid seq") + if m.target_system != self.mav.mav.srcSystem: + raise NotAchievedException("Incorrect target system in MISSION_REQUEST") + if m.target_component != self.mav.mav.srcComponent: + raise NotAchievedException("Incorrect target component in MISSION_REQUEST") + rerequest_count += 1 + self.progress("Valid re-request received.") + continue + if m.get_type() == "MISSION_ACK": + raise NotAchievedException("Received unexpected MISSION_ACK") + if m.get_type() == "STATUSTEXT": + if "upload time" in m.text: + break + if rerequest_count < 3: + raise NotAchievedException("Expected several re-requests of mission item") + # timeouts come with an ack: + self.assert_receive_mission_ack( + mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + want_type=mavutil.mavlink.MAV_MISSION_OPERATION_CANCELLED, + ) + + def test_fence_upload_timeouts(self, target_system=1, target_component=1): + self.test_fence_upload_timeouts_1(target_system=target_system, + target_component=target_component) + self.test_fence_upload_timeouts_2(target_system=target_system, + target_component=target_component) + + def test_poly_fence_compatability_ordering(self, target_system=1, target_component=1): + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + here = self.mav.location() + self.progress("try uploading return point last") + self.roundtrip_fence_using_fencepoint_protocol([ + self.offset_location_ne(here, 0, 0), # bl // return point + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + self.offset_location_ne(here, -50, 20), # closing point + ], ordering=[1, 2, 3, 4, 5, 0]) + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + + self.progress("try uploading return point in middle") + self.roundtrip_fence_using_fencepoint_protocol([ + self.offset_location_ne(here, 0, 0), # bl // return point + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + self.offset_location_ne(here, -50, 20), # closing point + ], ordering=[1, 2, 3, 0, 4, 5]) + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + + self.progress("try closing point in middle") + self.roundtrip_fence_using_fencepoint_protocol([ + self.offset_location_ne(here, 0, 0), # bl // return point + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + self.offset_location_ne(here, -50, 20), # closing point + ], ordering=[0, 1, 2, 5, 3, 4]) + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + + # this is expected to fail as we don't return the closing + # point correctly until the first is uploaded + self.progress("try closing point first") + failed = False + try: + self.roundtrip_fence_using_fencepoint_protocol([ + self.offset_location_ne(here, 0, 0), # bl // return point + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + self.offset_location_ne(here, -50, 20), # closing point + ], ordering=[5, 0, 1, 2, 3, 4]) + except NotAchievedException as e: + failed = "got=0.000000 want=" in str(e) + if not failed: + raise NotAchievedException("Expected failure, did not get it") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + + self.progress("try (almost) reverse order") + self.roundtrip_fence_using_fencepoint_protocol([ + self.offset_location_ne(here, 0, 0), # bl // return point + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + self.offset_location_ne(here, -50, 20), # closing point + ], ordering=[4, 3, 2, 1, 0, 5]) + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + + def test_poly_fence_compatability(self, target_system=1, target_component=1): + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + target_system=target_system, + target_component=target_component) + + self.test_poly_fence_compatability_ordering(target_system=target_system, target_component=target_component) + + here = self.mav.location() + + self.progress("Playing with changing point count") + self.roundtrip_fence_using_fencepoint_protocol( + [ + self.offset_location_ne(here, 0, 0), # bl // return point + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + self.offset_location_ne(here, -50, 20), # closing point + ]) + self.roundtrip_fence_using_fencepoint_protocol( + [ + self.offset_location_ne(here, 0, 0), # bl // return point + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, -50, 40), # tl, + self.offset_location_ne(here, -50, 20), # closing point + ]) + self.roundtrip_fence_using_fencepoint_protocol( + [ + self.offset_location_ne(here, 0, 0), # bl // return point + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + self.offset_location_ne(here, -50, 20), # closing point + ]) + + def test_poly_fence_reboot_survivability(self): + here = self.mav.location() + + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + [ + [ # east + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + ], [ # over the top of the vehicle + self.offset_location_ne(here, -50, -50), # bl + self.offset_location_ne(here, -50, 50), # br + self.offset_location_ne(here, 50, 50), # tr + self.offset_location_ne(here, 50, -50), # tl, + ] + ] + ) + self.reboot_sitl() + downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + downloaded_len = len(downloaded_items) + if downloaded_len != 8: + raise NotAchievedException("Items did not survive reboot (want=%u got=%u)" % + (8, downloaded_len)) + + def test_poly_fence(self): + '''test fence-related functions''' + target_system = 1 + target_component = 1 + + self.change_mode("LOITER") + self.wait_ready_to_arm() + here = self.mav.location() + self.progress("here: %f %f" % (here.lat, here.lng)) + self.set_parameter("FENCE_ENABLE", 1) + self.set_parameter("AVOID_ENABLE", 0) + +# self.set_parameter("SIM_SPEEDUP", 1) + + self.test_poly_fence_compatability() + + self.test_fence_upload_timeouts() + + self.test_poly_fence_noarms(target_system=target_system, target_component=target_component) + + self.arm_vehicle() + + self.test_poly_fence_inclusion(here, target_system=target_system, target_component=target_component) + self.test_poly_fence_exclusion(here, target_system=target_system, target_component=target_component) + + self.disarm_vehicle() + + self.test_poly_fence_reboot_survivability() + + + def test_poly_fence_inclusion_overlapping_inclusion_circles(self, here, target_system=1, target_component=1): + self.start_subtest("Overlapping circular inclusion") + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, + [ + { + "radius": 30, + "loc": self.offset_location_ne(here, -20, 0), + }, + { + "radius": 30, + "loc": self.offset_location_ne(here, 20, 0), + }, + ]) + self.mavproxy.send("fence list\n") + + self.delay_sim_time(5) + self.progress("Drive outside top circle") + fence_middle = self.offset_location_ne(here, -150, 0) + self.drive_somewhere_breach_boundary_and_rtl( + fence_middle, + target_system=target_system, + target_component=target_component) + + self.delay_sim_time(5) + self.progress("Drive outside bottom circle") + fence_middle = self.offset_location_ne(here, 150, 0) + self.drive_somewhere_breach_boundary_and_rtl( + fence_middle, + target_system=target_system, + target_component=target_component) + + def test_poly_fence_inclusion(self, here, target_system=1, target_component=1): + self.progress("Circle and Polygon inclusion") + self.test_poly_fence_inclusion_overlapping_inclusion_circles(here, target_system=target_system, target_component=target_component) + + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + [ + [ + self.offset_location_ne(here, -40, -20), # tl + self.offset_location_ne(here, 50, -20), # tr + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, -40, 20), # bl, + ], + { + "radius": 30, + "loc": self.offset_location_ne(here, -20, 0), + }, + ]) + + self.delay_sim_time(5) + self.mavproxy.send("fence list\n") + self.progress("Drive outside polygon") + fence_middle = self.offset_location_ne(here, -150, 0) + self.drive_somewhere_breach_boundary_and_rtl( + fence_middle, + target_system=target_system, + target_component=target_component) + + self.delay_sim_time(5) + self.progress("Drive outside circle") + fence_middle = self.offset_location_ne(here, 150, 0) + self.drive_somewhere_breach_boundary_and_rtl( + fence_middle, + target_system=target_system, + target_component=target_component) + + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + [ + [ + self.offset_location_ne(here, -20, -25), # tl + self.offset_location_ne(here, 50, -25), # tr + self.offset_location_ne(here, 50, 15), # br + self.offset_location_ne(here, -20, 15), # bl, + ], + [ + self.offset_location_ne(here, 20, -20), # tl + self.offset_location_ne(here, -50, -20), # tr + self.offset_location_ne(here, -50, 20), # br + self.offset_location_ne(here, 20, 20), # bl, + ], + ]) + + self.delay_sim_time(5) + self.mavproxy.send("fence list\n") + self.progress("Drive outside top polygon") + fence_middle = self.offset_location_ne(here, -150, 0) + self.drive_somewhere_breach_boundary_and_rtl( + fence_middle, + target_system=target_system, + target_component=target_component) + + self.delay_sim_time(5) + self.progress("Drive outside bottom polygon") + fence_middle = self.offset_location_ne(here, 150, 0) + self.drive_somewhere_breach_boundary_and_rtl( + fence_middle, + target_system=target_system, + target_component=target_component) + + def test_poly_fence_exclusion(self, here, target_system=1, target_component=1): + + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + [ + [ # east + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + ], [ # west + self.offset_location_ne(here, -50, -20), # tl + self.offset_location_ne(here, 50, -20), # tr + self.offset_location_ne(here, 50, -40), # br + self.offset_location_ne(here, -50, -40), # bl, + ], { + "radius": 30, + "loc": self.offset_location_ne(here, -60, 0), + }, + ]) + self.delay_sim_time(5) + self.mavproxy.send("fence list\n") + + self.progress("Breach eastern boundary") + fence_middle = self.offset_location_ne(here, 0, 30) + self.drive_somewhere_breach_boundary_and_rtl(fence_middle, + target_system=target_system, + target_component=target_component) + + self.progress("delaying - hack to work around manual recovery bug") + self.delay_sim_time(5) + + self.progress("Breach western boundary") + fence_middle = self.offset_location_ne(here, 0, -30) + self.drive_somewhere_breach_boundary_and_rtl(fence_middle, + target_system=target_system, + target_component=target_component) + + self.progress("delaying - hack to work around manual recovery bug") + self.delay_sim_time(5) + + self.progress("Breach southern circle") + fence_middle = self.offset_location_ne(here, -150, 0) + self.drive_somewhere_breach_boundary_and_rtl(fence_middle, + target_system=target_system, + target_component=target_component) + + def drive_smartrtl(self): self.change_mode("STEERING") self.wait_ready_to_arm() @@ -1824,6 +4254,336 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.wait_servo_channel_value(3, self.get_parameter("RC3_TRIM", 5), timeout=10) self.mav.motors_disarmed_wait() + def test_poly_fence_object_avoidance_guided(self, target_system=1, target_component=1): + if not self.mavproxy_can_do_mision_item_protocols(): + return + + self.test_poly_fence_object_avoidance_guided_pathfinding( + target_system=target_system, + target_component=target_component) + return + # twosquares is currently disabled because of the requirement to have an inclusion fence (which it doesn't have ATM) + self.test_poly_fence_object_avoidance_guided_two_squares( + target_system=target_system, + target_component=target_component) + + def test_poly_fence_object_avoidance_auto(self, target_system=1, target_component=1): + self.load_fence("rover-path-planning-fence.txt") + self.load_mission("rover-path-planning-mission.txt") + self.context_push() + ex = None + try: + self.set_parameter("AVOID_ENABLE", 3) + self.set_parameter("OA_TYPE", 2) + self.set_parameter("FENCE_MARGIN", 0) # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 + self.reboot_sitl() + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + self.mavproxy.send("fence list\n") + # target_loc is copied from the mission file + target_loc = mavutil.location(40.073799, -105.229156) + self.wait_location(target_loc, timeout=300) + # mission has RTL as last item + self.wait_distance_to_home(5, accuracy=2, timeout=300) + self.disarm_vehicle() + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.context_pop() + self.reboot_sitl() + if ex is not None: + raise ex + + def send_guided_mission_item(self, loc, target_system=1, target_component=1): + self.mav.mav.mission_item_send ( + target_system, + target_component, + 0, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 2, # current + 0, # autocontinue + 0, # param1 + 0, # param2 + 0, # param3 + 0, # param4 + loc.lat, # x + loc.lng, # y + 0 # z + ) + + def test_poly_fence_object_avoidance_guided_pathfinding(self, target_system=1, target_component=1): + self.load_fence("rover-path-planning-fence.txt") + self.context_push() + ex = None + try: + self.set_parameter("AVOID_ENABLE", 3) + self.set_parameter("OA_TYPE", 2) + self.set_parameter("FENCE_MARGIN", 0) # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + self.mavproxy.send("fence list\n") + target_loc = mavutil.location(40.073800, -105.229172) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + self.wait_location(target_loc, timeout=300) + self.do_RTL(timeout=300) + self.disarm_vehicle() + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.context_pop() + self.reboot_sitl() + if ex is not None: + raise ex + + def test_poly_fence_object_avoidance_guided_two_squares(self, target_system=1, target_component=1): + self.start_subtest("Ensure we can steer around obstacles in guided mode") + here = self.mav.location() + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + [ + [ # east + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 10), # tl + self.offset_location_ne(here, 50, 30), # tr + self.offset_location_ne(here, -50, 40), # br, + ], + [ # further east (and south + self.offset_location_ne(here, -60, 60), # bl + self.offset_location_ne(here, 40, 70), # tl + self.offset_location_ne(here, 40, 90), # tr + self.offset_location_ne(here, -60, 80), # br, + ], + ]) + self.mavproxy.send("fence list\n") + self.context_push() + ex = None + try: + self.set_parameter("AVOID_ENABLE", 3) + self.set_parameter("OA_TYPE", 2) + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.set_parameter("FENCE_ENABLE", 1) + self.mavproxy.send("fence list\n") + self.arm_vehicle() + + self.change_mode("GUIDED") + target = mavutil.location(40.071382, -105.228340, 0, 0) + self.send_guided_mission_item(target, + target_system=target_system, + target_component=target_component) + self.wait_location(target, timeout=300) + self.do_RTL() + self.disarm_vehicle() + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.context_pop() + self.reboot_sitl() + if ex is not None: + raise ex + + def test_poly_fence_avoidance_dont_breach_exclusion(self, target_system=1, target_component=1): + self.start_subtest("Ensure we stop before breaching an exclusion fence") + here = self.mav.location() + self.upload_fences_from_locations( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + [ + [ # east + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + self.offset_location_ne(here, -50, 40), # tl, + ], [ # west + self.offset_location_ne(here, -50, -20), # tl + self.offset_location_ne(here, 50, -20), # tr + self.offset_location_ne(here, 50, -40), # br + self.offset_location_ne(here, -50, -40), # bl, + ], { + "radius": 30, + "loc": self.offset_location_ne(here, -60, 0), + }, + ]) + self.mavproxy.send("fence list\n") + self.set_parameter("FENCE_ENABLE", 1) + self.set_parameter("AVOID_ENABLE", 3) + fence_middle = self.offset_location_ne(here, 0, 30) + # FIXME: this might be nowhere near "here"! + expected_stopping_point = mavutil.location(40.0713376, -105.2295738, 0, 0) + self.drive_somewhere_stop_at_boundary( + fence_middle, + expected_stopping_point, + target_system=target_system, + target_component=target_component) + self.set_parameter("AVOID_ENABLE", 0) + self.do_RTL() + + def do_RTL(self, timeout=60): + self.change_mode("RTL") + self.wait_distance_to_home(5, accuracy=2, timeout=timeout) + + def test_poly_fence_avoidance(self, target_system=1, target_component=1): + self.change_mode("LOITER") + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode("GUIDED") + + self.test_poly_fence_avoidance_dont_breach_exclusion(target_system=target_system, target_component=target_component) + + self.disarm_vehicle() + + def test_poly_fence_object_avoidance_guided_bendy_ruler(self, target_system=1, target_component=1): + if not self.mavproxy_can_do_mision_item_protocols(): + return + self.load_fence("rover-path-bendyruler-fence.txt") + self.context_push() + ex = None + try: + self.set_parameter("AVOID_ENABLE", 3) + self.set_parameter("OA_TYPE", 1) + self.set_parameter("OA_LOOKAHEAD", 50) + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + self.set_parameter("WP_RADIUS", 5) + self.mavproxy.send("fence list\n") + target_loc = mavutil.location(40.071060, -105.227734, 0, 0) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + # FIXME: we don't get within WP_RADIUS of our target?! + self.wait_location(target_loc, timeout=300, accuracy=15) + self.do_RTL(timeout=300) + self.disarm_vehicle() + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.context_pop() + self.disarm_vehicle() + self.reboot_sitl() + if ex is not None: + raise ex + + def test_poly_fence_object_avoidance_bendy_ruler_easier(self, target_system=1, target_component=1): + if not self.mavproxy_can_do_mision_item_protocols(): + return + self.test_poly_fence_object_avoidance_auto_bendy_ruler_easier(target_system=target_system, target_component=target_component) + self.test_poly_fence_object_avoidance_guided_bendy_ruler_easier(target_system=target_system, target_component=target_component) + + def test_poly_fence_object_avoidance_guided_bendy_ruler_easier(self, target_system=1, target_component=1): + '''finish-line issue means we can't complete the harder one. This + test can go away once we've nailed that one. The only + difference here is the target point. + ''' + if not self.mavproxy_can_do_mision_item_protocols(): + return + self.load_fence("rover-path-bendyruler-fence.txt") + self.context_push() + ex = None + try: + self.set_parameter("AVOID_ENABLE", 3) + self.set_parameter("OA_TYPE", 1) + self.set_parameter("OA_LOOKAHEAD", 50) + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + self.set_parameter("WP_RADIUS", 5) + self.mavproxy.send("fence list\n") + target_loc = mavutil.location(40.071260, -105.227000, 0, 0) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + # FIXME: we don't get within WP_RADIUS of our target?! + self.wait_location(target_loc, timeout=300, accuracy=15) + self.do_RTL(timeout=300) + self.disarm_vehicle() + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.context_pop() + self.disarm_vehicle() + self.reboot_sitl() + if ex is not None: + raise ex + + def test_poly_fence_object_avoidance_auto_bendy_ruler_easier(self, target_system=1, target_component=1): + '''finish-line issue means we can't complete the harder one. This + test can go away once we've nailed that one. The only + difference here is the target point. + ''' + if not self.mavproxy_can_do_mision_item_protocols(): + return + + self.load_fence("rover-path-bendyruler-fence.txt") + self.load_mission("rover-path-bendyruler-mission-easier.txt") + self.context_push() + ex = None + try: + self.set_parameter("AVOID_ENABLE", 3) + self.set_parameter("OA_TYPE", 1) + self.set_parameter("OA_LOOKAHEAD", 50) + self.reboot_sitl() + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + self.set_parameter("WP_RADIUS", 5) + self.mavproxy.send("fence list\n") + target_loc = mavutil.location(40.071260, -105.227000, 0, 0) + # target_loc is copied from the mission file + self.wait_location(target_loc, timeout=300) + # mission has RTL as last item + self.wait_distance_to_home(5, accuracy=2, timeout=300) + self.disarm_vehicle() + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.context_pop() + self.disarm_vehicle() + self.reboot_sitl() + if ex is not None: + raise ex + + def test_poly_fence_object_avoidance(self, target_system=1, target_component=1): + if not self.mavproxy_can_do_mision_item_protocols(): + return + + self.test_poly_fence_object_avoidance_auto( + target_system=target_system, + target_component=target_component) + self.test_poly_fence_object_avoidance_guided( + target_system=target_system, + target_component=target_component) + + def test_poly_fence_object_avoidance_bendy_ruler(self, target_system=1, target_component=1): + if not self.mavproxy_can_do_mision_item_protocols(): + return + # bendy Ruler isn't as flexible as Dijkstra for planning, so + # it gets a simpler test: + self.test_poly_fence_object_avoidance_guided_bendy_ruler( + target_system=target_system, + target_component=target_component, + ) + + def tests(self): '''return list of all tests''' ret = super(AutoTestRover, self).tests() @@ -1938,6 +4698,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) "Upload and download of rally", self.test_gcs_rally), + ("GCSMission", + "Upload and download of mission", + self.test_gcs_mission), + ("MotorTest", "Motor Test triggered via mavlink", self.test_motor_test), @@ -1950,6 +4714,26 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) "Test DataFlash SITL backend", self.test_dataflash_sitl), + ("PolyFence", + "PolyFence tests", + self.test_poly_fence), + + ("PolyFenceAvoidance", + "PolyFence avoidance tests", + self.test_poly_fence_avoidance), + + ("PolyFenceObjectAvoidance", + "PolyFence object avoidance tests", + self.test_poly_fence_object_avoidance), + + ("PolyFenceObjectAvoidanceBendyRuler", + "PolyFence object avoidance tests - bendy ruler", + self.test_poly_fence_object_avoidance_bendy_ruler), + + ("PolyFenceObjectAvoidanceBendyRulerEasier", + "PolyFence object avoidance tests - easier bendy ruler test", + self.test_poly_fence_object_avoidance_bendy_ruler_easier), + ("DownLoadLogs", "Download logs", lambda: self.log_download( self.buildlogs_path("APMrover2-log.bin"), @@ -1957,6 +4741,11 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) ]) return ret + def disabled_tests(self): + return { + "PolyFenceObjectAvoidanceBendyRuler": "currently broken", + } + def rc_defaults(self): ret = super(AutoTestRover, self).rc_defaults() ret[3] = 1500 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a579c99d8e..aa371066da 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -644,6 +644,43 @@ class AutoTestCopter(AutoTest): self.fly_fence_avoid_test_radius_check(avoid_behave=1, timeout=timeout) self.fly_fence_avoid_test_radius_check(avoid_behave=0, timeout=timeout) + def assert_prearm_failure(self, expected_statustext, timeout=5, ignore_prearm_failures=[]): + seen_statustext = False + seen_command_ack = False + + self.drain_mav() + tstart = self.get_sim_time_cached() + arm_last_send = 0 + while True: + if seen_command_ack and seen_statustext: + break + now = self.get_sim_time_cached() + if now - tstart > timeout: + raise NotAchievedException("Did not see failure-to-arm messages (statustext=%s command_ack=%s" % (seen_statustext, seen_command_ack)) + if now - arm_last_send > 1: + arm_last_send = now + self.send_mavlink_arm_command() + m = self.mav.recv_match(blocking=True, timeout=1) + if m is None: + continue + if m.get_type() == "STATUSTEXT": + if expected_statustext in m.text: + self.progress("Got: %s" % str(m)) + seen_statustext = True + elif "PreArm" in m.text and m.text[8:] not in ignore_prearm_failures: + self.progress("Got: %s" % str(m)) + raise NotAchievedException("Unexpected prearm failure (%s)" % m.text) + + if m.get_type() == "COMMAND_ACK": + print("Got: %s" % str(m)) + if m.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: + if m.result != 4: + raise NotAchievedException("command-ack says we didn't fail to arm") + self.progress("Got: %s" % str(m)) + seen_command_ack = True + if self.mav.motors_armed(): + raise NotAchievedException("Armed when we shouldn't have") + # fly_fence_test - fly east until you hit the horizontal circular fence def fly_fence_test(self, timeout=180): # enable fence, disable avoidance @@ -662,43 +699,29 @@ class AutoTestCopter(AutoTest): self.start_subtest("ensure we can't arm if ouside fence") self.load_fence_using_mavproxy("fence-in-middle-of-nowhere.txt") self.delay_sim_time(5) # let fence check run so it loads-from-eeprom - seen_statustext = False - seen_command_ack = False - - tstart = self.get_sim_time_cached() - while True: - self.send_mavlink_arm_command() - if seen_command_ack and seen_statustext: - break - if self.get_sim_time_cached() - tstart > 5: - raise NotAchievedException("Did not see failure-to-arm messages (statustext=%s command_ack=%s" % (seen_statustext, seen_command_ack)) - m = self.mav.recv_match(blocking=True, timeout=1) - if m is None: - continue - if m.get_type() == "STATUSTEXT": - if "vehicle outside fence" in m.text: - self.progress("Got: %s" % str(m)) - seen_statustext = True - elif "PreArm" in m.text: - self.progress("Got: %s" % str(m)) - raise NotAchievedException("Unexpected prearm failure (%s)" % m.text) - - if m.get_type() == "COMMAND_ACK": - print("Got: %s" % str(m)) - if m.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: - if m.result != 4: - raise NotAchievedException("command-ack says we didn't fail to arm") - self.progress("Got: %s" % str(m)) - seen_command_ack = True - if self.mav.motors_armed(): - raise NotAchievedException("Armed when we shouldn't have") - + self.assert_prearm_failure("vehicle outside fence") self.progress("Failed to arm outside fence (good!)") + self.clear_fence() + self.delay_sim_time(5) # let fence breach clear self.drain_mav() - self.mavproxy.send('fence clear\n') - self.delay_sim_time(2) - self.mavproxy.send('fence list\n') - self.mavproxy.expect("No geo-fence points") + self.end_subtest("ensure we can't arm if ouside fence") + + self.start_subtest("ensure we can't arm with bad radius") + self.context_push() + self.set_parameter("FENCE_RADIUS", -1) + self.assert_prearm_failure("Invalid FENCE_RADIUS value") + self.context_pop() + self.progress("Failed to arm with bad radius") + self.drain_mav() + self.end_subtest("ensure we can't arm with bad radius") + + self.start_subtest("ensure we can't arm with bad alt") + self.context_push() + self.set_parameter("FENCE_ALT_MAX", -1) + self.assert_prearm_failure("Invalid FENCE_ALT_MAX value") + self.context_pop() + self.progress("Failed to arm with bad altitude") + self.end_subtest("ensure we can't arm with bad radius") self.start_subtest("Check breach-fence behaviour") self.set_parameter("FENCE_TYPE", 2) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index ce9baeea85..98d1d31afb 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -15,6 +15,8 @@ import operator from MAVProxy.modules.lib import mp_util from pymavlink import mavwp, mavutil, DFReader +from pymavlink import mavextra + from pysim import util, vehicleinfo # a list of pexpect objects to read while waiting for @@ -222,6 +224,25 @@ class AutoTest(ABC): HOME.alt, HOME.heading) + def mavproxy_version(self): + '''return the current version of mavproxy as a tuple e.g. (1,8,8)''' + return util.MAVProxy_version() + + def mavproxy_version_gt(self, major, minor, point): + if os.getenv("AUTOTEST_FORCE_MAVPROXY_VERSION", None) is not None: + return True + (got_major, got_minor, got_point) = self.mavproxy_version() + print("Got: %s.%s.%s" % (got_major, got_minor, got_point)) + if got_major > major: + return True + elif got_major < major: + return False + if got_minor > minor: + return True + elif got_minor < minor: + return False + return got_point > point + def open_mavproxy_logfile(self): return MAVProxyLogFile() @@ -299,12 +320,29 @@ class AutoTest(ABC): def count_lines_in_filepath(self, filepath): return len([i for i in open(filepath)]) + def count_expected_fence_lines_in_filepath(self, filepath): + count = 0 + is_qgc = False + for i in open(filepath): + i = re.sub("#.*", "", i) # trim comments + if i.isspace(): + # skip empty lines + continue + if re.match("QGC", i): + # skip QGC header line + is_qgc = True + continue + count += 1 + if is_qgc: + count += 2 # file doesn't include return point + closing point + return count + def load_fence_using_mavproxy(self, filename): self.set_parameter("FENCE_TOTAL", 0) filepath = os.path.join(self.mission_directory(), filename) - count = self.count_lines_in_filepath(filepath) + count = self.count_expected_fence_lines_in_filepath(filepath) self.mavproxy.send('fence load %s\n' % filepath) - self.mavproxy.expect("Loaded %u geo-fence" % count) +# self.mavproxy.expect("Loaded %u (geo-)?fence" % count) tstart = self.get_sim_time_cached() while True: t2 = self.get_sim_time_cached() @@ -686,6 +724,97 @@ class AutoTest(ABC): raise ValueError("count %u not handled" % count) self.progress("Files same") + def assert_rally_files_same(self, file1, file2): + self.progress("Comparing (%s) and (%s)" % (file1, file2, )) + f1 = open(file1) + f2 = open(file2) + lines_f1 = f1.readlines() + lines_f2 = f2.readlines() + self.assert_rally_content_same(lines_f1, lines_f2) + + def assert_rally_filepath_content(self, file1, content): + f1 = open(file1) + lines_f1 = f1.readlines() + lines_content = content.split("\n") + print("lines content: %s" % str(lines_content)) + self.assert_rally_content_same(lines_f1, lines_content) + + def assert_rally_content_same(self, f1, f2): + '''check each line in f1 matches one-to-one with f2''' + for l1, l2 in zip(f1, f2): + print("l1: %s" % l1) + print("l2: %s" % l2) + l1 = l1.rstrip("\n") + l2 = l2.rstrip("\n") + l1 = l1.rstrip("\r") + l2 = l2.rstrip("\r") + if l1 == l2: + # e.g. the first "QGC WPL 110" line + continue + if re.match(r"0\s", l1): + # home changes... + continue + l1 = l1.rstrip() + l2 = l2.rstrip() + print("al1: %s" % str(l1)) + print("al2: %s" % str(l2)) + fields1 = re.split(r"\s+", l1) + fields2 = re.split(r"\s+", l2) + # line = int(fields1[0]) + 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, + # ]: + # # ardupilot doesn't remember frame on these commands + # if int(i1) == 3: + # i1 = 0 + # if int(i2) == 3: + # i2 = 0 + # if count == 6: # param 3 + # if t in [mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME]: + # # ardupilot canonicalises this to -1 for ccw or 1 for cw. + # if float(i1) == 0: + # i1 = 1.0 + # if float(i2) == 0: + # i2 = 1.0 + # if count == 7: # param 4 + # if t == mavutil.mavlink.MAV_CMD_NAV_LAND: + # # ardupilot canonicalises "0" to "1" param 4 (yaw) + # if int(float(i1)) == 0: + # i1 = 1 + # if int(float(i2)) == 0: + # i2 = 1 + if 0 <= count <= 3 or 11 <= count <= 11: + if int(i1) != int(i2): + raise ValueError("Rally points different: (%s vs %s) (%d vs %d) (count=%u)" % + (l1, l2, int(i1), int(i2), count)) # NOCI + continue + if 4 <= count <= 10: + f_i1 = float(i1) + f_i2 = float(i2) + delta = abs(f_i1 - f_i2) + max_allowed_delta = 0.000009 + if delta > max_allowed_delta: + raise ValueError( + ("Rally has different (float) content: " + + "(%s vs %s) " + + "(%f vs %f) " + + "(%.10f) " + + "(count=%u)") % + (l1, l2, + f_i1, f_i2, + delta, + count)) # NOCI + continue + raise ValueError("count %u not handled" % count) + self.progress("Rally content same") + def load_rally(self, filename): """Load rally points from a file to flight controller.""" self.progress("Loading rally points (%s)" % filename) @@ -938,6 +1067,7 @@ class AutoTest(ABC): def arm_vehicle(self, timeout=20): """Arm vehicle with mavlink arm message.""" self.progress("Arm motors with MAVLink cmd") + self.drain_mav() tstart = self.get_sim_time() self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 1, # ARM @@ -2192,11 +2322,20 @@ class AutoTest(ABC): continue if m.get_type() == 'MISSION_ACK': - raise NotAchievedException("Received unexpected mission ack %s" % str(m)) + if (m.target_system == 255 and + m.target_component == 0 and + m.type == 1 and + m.mission_type == 0): + # this is just MAVProxy trying to screw us up + continue + else: + raise NotAchievedException("Received unexpected mission ack %s" % str(m)) - self.progress("Handling request for item %u" % m.seq) + self.progress("Handling request for item %u/%u" % (m.seq, len(items)-1)) + self.progress("Item (%s)" % str(items[m.seq])) if m.seq in sent: - raise NotAchievedException("received duplicate request for item %u" % m.seq) + self.progress("received duplicate request for item %u" % m.seq) + continue if m.seq not in remaining_to_send: raise NotAchievedException("received request for unknown item %u" % m.seq) @@ -2211,10 +2350,9 @@ class AutoTest(ABC): if items[m.seq].target_component != target_component: raise NotAchievedException("supplied item not of correct target component") if items[m.seq].seq != m.seq: - raise NotAchievedException("requested item has incorrect sequence number") + raise NotAchievedException("supplied item has incorrect sequence number (%u vs %u)" % (items[m.seq].seq, m.seq)) items[m.seq].pack(self.mav.mav) - self.progress("Sending (%s)" % str(items[m.seq])) self.mav.mav.send(items[m.seq]) remaining_to_send.discard(m.seq) sent.add(m.seq) @@ -2230,22 +2368,34 @@ class AutoTest(ABC): (mavutil.mavlink.enums["MAV_MISSION_RESULT"][m.type].name),) self.progress("Upload of all %u items succeeded" % len(items)) - def download_using_mission_protocol(self, mission_type): + def download_using_mission_protocol(self, mission_type, verbose=False, timeout=10): '''mavlink2 required''' target_system = 1 target_component = 1 + self.drain_mav_unparsed() + self.progress("Sending mission_request_list") self.mav.mav.mission_request_list_send(target_system, target_component, mission_type) + tstart = self.get_sim_time_cached() while True: - m = self.mav.recv_match(type='MISSION_COUNT', - blocking=True, - timeout=5) - self.progress(str(m)) + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Did not get MISSION_COUNT packet") + m = self.mav.recv_match(blocking=True, timeout=0.1) + if verbose: + self.progress(str(m)) + if m.get_type() == 'MISSION_ACK': + if m.target_system == 255 and m.target_component == 0: + # this was for MAVProxy + continue + self.progress("Mission ACK: %s" % str(m)) + raise NotAchievedException("Received MISSION_ACK while waiting for MISSION_COUNT") + if m.get_type() != 'MISSION_COUNT': + continue if m is None: raise NotAchievedException("Did not get MISSION_COUNT response") - if m.target_component != 250: + if m.target_component != 250: # FIXME: constant?! continue if m.mission_type != mission_type: raise NotAchievedException("Mission count response of incorrect type") @@ -2324,6 +2474,17 @@ class AutoTest(ABC): m = self.poll_home_position() return mavutil.location(m.latitude*1.0e-7, m.longitude*1.0e-7, m.altitude*1.0e-3, 0) + def offset_location_ne(self, location, metres_north, metres_east): + '''return a new location offset from passed-in location''' + (target_lat, target_lng) = mavextra.gps_offset(location.lat, + location.lng, + metres_east, + metres_north) + return mavutil.location(target_lat, + target_lng, + location.alt, + location.heading) + def monitor_groundspeed(self, want, tolerance=0.5, timeout=5): tstart = self.get_sim_time() while True: @@ -2897,8 +3058,47 @@ class AutoTest(ABC): if m is None: raise NotAchievedException("Requested CAMERA_FEEDBACK did not arrive") - def clear_fence_using_mavproxy(self): + def clear_mission(self, mission_type, target_system=1, target_component=1): + '''clear mision_type from autopilot. Note that this does NOT actually + send a MISSION_CLEAR_ALL message + ''' + if mission_type == mavutil.mavlink.MAV_MISSION_TYPE_ALL: + # recurse + if not self.is_tracker() and not self.is_plane(): + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + if not self.is_sub() and not self.is_tracker(): + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) + return + + self.mav.mav.mission_count_send(target_system, + target_component, + 0, + mission_type) + m = self.mav.recv_match(type='MISSION_ACK', + blocking=True, + timeout=5) + if m is None: + raise NotAchievedException("Expected ACK for clearing mission") + if m.target_system != self.mav.mav.srcSystem: + raise NotAchievedException("ACK not targetted at correct system want=%u got=%u" % + (self.mav.mav.srcSystem, m.target_system)) + if m.target_component != self.mav.mav.srcComponent: + raise NotAchievedException("ACK not targetted at correct component want=%u got=%u" % + (self.mav.mav.srcComponent, m.target_component)) + if m.type != mavutil.mavlink.MAV_MISSION_ACCEPTED: + raise NotAchievedException("Expected MAV_MISSION_ACCEPTED got %s" % + (mavutil.mavlink.enums["MAV_MISSION_RESULT"][m.type].name,)) + + def clear_fence_using_mavproxy(self, timeout=10): self.mavproxy.send("fence clear\n") + tstart = self.get_sim_time_cached() + while True: + now = self.get_sim_time_cached() + if now - tstart > timeout: + raise AutoTestTimeoutException("FENCE_TOTAL did not go to zero") + if self.get_parameter("FENCE_TOTAL") == 0: + break def clear_fence(self): self.clear_fence_using_mavproxy() @@ -3070,6 +3270,63 @@ class AutoTest(ABC): '''return mode vehicle should start in with default RC inputs set''' return None + def upload_fences_from_locations(self, + vertex_type, + list_of_list_of_locs, + target_system=1, + target_component=1): + seq = 0 + items = [] + for locs in list_of_list_of_locs: + if type(locs) == dict: + # circular fence + if vertex_type == mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: + v = mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION + else: + v = mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION + item = self.mav.mav.mission_item_int_encode( + target_system, + target_component, + seq, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, + v, + 0, # current + 0, # autocontinue + locs["radius"], # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(locs["loc"].lat *1e7), # latitude + int(locs["loc"].lng *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + seq += 1 + items.append(item) + continue + count = len(locs) + for loc in locs: + item = self.mav.mav.mission_item_int_encode( + target_system, + target_component, + seq, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, + vertex_type, + 0, # current + 0, # autocontinue + count, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(loc.lat *1e7), # latitude + int(loc.lng *1e7), # longitude + 33.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + seq += 1 + items.append(item) + + self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + items) + def wait_for_initial_mode(self): '''wait until we get a heartbeat with an expected initial mode (the one specified in the vehicle constructor)''' @@ -3094,6 +3351,19 @@ switch value''' self.progress("---------- %s ----------" % description) self.progress("-") + def start_subsubtest(self, description): + self.progress(".") + self.progress(".......... %s .........." % description) + self.progress(".") + + def end_subtest(self, description): + '''TODO: sanity checks?''' + pass + + def end_subsubtest(self, description): + '''TODO: sanity checks?''' + pass + def test_skipped(self, test, reason): (name, desc, func) = test self.progress("##### %s is skipped: %s" % (name, reason)) @@ -3142,6 +3412,8 @@ switch value''' self.progress("Setting up RC parameters") self.set_rc_default() self.wait_for_mode_switch_poll() + if not self.is_tracker(): # FIXME - more to the point, fix Tracker's mission handling + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) for test in tests: (name, desc, func) = test diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 4d6d8ef171..a4721eeeee 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -353,12 +353,26 @@ def start_SITL(binary, return child +def mavproxy_cmd(): + '''return path to which mavproxy to use''' + return os.getenv('MAVPROXY_CMD', 'mavproxy.py') + +def MAVProxy_version(): + '''return the current version of mavproxy as a tuple e.g. (1,8,8)''' + command = "%s --version" % mavproxy_cmd() + output = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE).communicate()[0] + output = output.decode('ascii') + match = re.search("MAVProxy Version: ([0-9]+)[.]([0-9]+)[.]([0-9]+)", output) + if match is None: + raise ValueError("Unable to determine MAVProxy version from (%s)" % output) + return (int(match.group(1)), int(match.group(2)), int(match.group(3))) + def start_MAVProxy_SITL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:5760', options=[], logfile=sys.stdout): """Launch mavproxy connected to a SITL instance.""" import pexpect global close_list - MAVPROXY = os.getenv('MAVPROXY_CMD', 'mavproxy.py') + MAVPROXY = mavproxy_cmd() cmd = MAVPROXY + ' --master=%s --out=127.0.0.1:14550' % master if setup: cmd += ' --setup' diff --git a/Tools/autotest/rover-path-bendyruler-fence.txt b/Tools/autotest/rover-path-bendyruler-fence.txt new file mode 100644 index 0000000000..ace97c223c --- /dev/null +++ b/Tools/autotest/rover-path-bendyruler-fence.txt @@ -0,0 +1,11 @@ +QGC WPL 110 +0 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071766 -105.230202 0.000000 0 +1 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071014 -105.230247 0.000000 0 +2 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071014 -105.228821 0.000000 0 +3 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071609 -105.228867 0.000000 0 +4 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071602 -105.228172 0.000000 0 +5 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.070858 -105.227982 0.000000 0 +6 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.070789 -105.226219 0.000000 0 +7 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.072453 -105.226379 0.000000 0 +8 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.071609 -105.228172 0.000000 0 +9 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.071625 -105.227982 0.000000 0 diff --git a/Tools/autotest/rover-path-bendyruler-mission-easier.txt b/Tools/autotest/rover-path-bendyruler-mission-easier.txt new file mode 100644 index 0000000000..f15b78a66a --- /dev/null +++ b/Tools/autotest/rover-path-bendyruler-mission-easier.txt @@ -0,0 +1,4 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 40.071377 -105.229790 0.000000 1 +1 0 3 16 0.000000 0.000000 0.000000 0.000000 40.071260 -105.227000 100.000000 1 +2 0 0 20 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 diff --git a/Tools/autotest/rover-path-planning-fence.txt b/Tools/autotest/rover-path-planning-fence.txt new file mode 100644 index 0000000000..67feb4beb8 --- /dev/null +++ b/Tools/autotest/rover-path-planning-fence.txt @@ -0,0 +1,25 @@ +QGC WPL 110 +0 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.071613 -105.230118 0.000000 0 +1 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.071644 -105.229393 0.000000 0 +2 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.071106 -105.229393 0.000000 0 +3 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.071747 -105.228668 0.000000 0 +4 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.072220 -105.228775 0.000000 0 +5 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.072357 -105.229729 0.000000 0 +6 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.072777 -105.229988 0.000000 0 +7 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.073181 -105.228897 0.000000 0 +8 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.072914 -105.228073 0.000000 0 +9 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.073387 -105.228180 0.000000 0 +10 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.073635 -105.229431 0.000000 0 +11 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.073967 -105.229301 0.000000 0 +12 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.073891 -105.227478 0.000000 0 +13 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.072697 -105.227440 0.000000 0 +14 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.072430 -105.227898 0.000000 0 +15 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.071880 -105.227196 0.000000 0 +16 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.071407 -105.227730 0.000000 0 +17 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.070194 -105.227249 0.000000 0 +18 0 0 5001 19.000000 0.000000 0.000000 0.000000 40.070175 -105.230110 0.000000 0 +19 0 0 5002 4.000000 0.000000 0.000000 0.000000 40.071922 -105.228676 0.000000 0 +20 0 0 5002 4.000000 0.000000 0.000000 0.000000 40.071712 -105.228172 0.000000 0 +21 0 0 5002 4.000000 0.000000 0.000000 0.000000 40.071560 -105.228676 0.000000 0 +22 0 0 5002 4.000000 0.000000 0.000000 0.000000 40.071739 -105.228920 0.000000 0 +23 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.072430 -105.228004 0.000000 0 diff --git a/Tools/autotest/rover-path-planning-mission.txt b/Tools/autotest/rover-path-planning-mission.txt new file mode 100644 index 0000000000..46281973b7 --- /dev/null +++ b/Tools/autotest/rover-path-planning-mission.txt @@ -0,0 +1,4 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 40.071377 -105.229790 0.000000 1 +1 0 3 16 0.000000 0.000000 0.000000 0.000000 40.073799 -105.229156 100.000000 1 +2 0 0 20 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1