mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
Peter Barker
Randy Mackay
parent
6d0c5da84a
commit
c5f52a8355
File diff suppressed because it is too large
Load Diff
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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'
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue