Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tools: autotest: add tests for upload/download rally using mission protocol 

Tools: autotest: add tests for mission clearing

Tools: autotest: add tests for out-of-range mission counts

Tools: autotest: add test for Plane DO_CHANGE_SPEED
This commit is contained in:
Peter Barker 2019-03-29 16:17:21 +11:00 committed by Peter Barker
parent a7c5f5a6c8
commit 8d90b09829
2 changed files with 658 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

654
Tools/autotest/apmrover2.py
View File

@ -1025,6 +1025,117 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.wait_location(loc, accuracy=accuracy)
self.disarm_vehicle()
def upload_using_mission_protocol(self, mission_type, items):
'''mavlink2 required'''
target_system = 1
target_component = 1
self.mav.mav.mission_count_send(target_system,
target_component,
len(items),
mission_type)
tstart = self.get_sim_time_cached()
remaining_to_send = set(range(0, len(items)))
sent = set()
while True:
if self.get_sim_time_cached() - tstart > 10:
raise NotAchievedException("timeout uploading %s" % str(mission_type))
if len(remaining_to_send) == 0:
self.progress("All sent")
break
m = self.mav.recv_match(type=['MISSION_REQUEST', 'MISSION_ACK'],
blocking=True,
timeout=1)
if m is None:
continue
if m.get_type() == 'MISSION_ACK':
raise NotAchievedException("Received unexpected mission ack %s" % str(m))
self.progress("Handling request for item %u" % m.seq)
if m.seq in sent:
raise NotAchievedException("received duplicate request for item %u" % m.seq)
if m.seq not in remaining_to_send:
raise NotAchievedException("received request for unknown item %u" % m.seq)
if m.mission_type != mission_type:
raise NotAchievedException("received request for item from wrong mission type")
if items[m.seq].mission_type != mission_type:
raise NotAchievedException("supplied item not of correct mission type")
if items[m.seq].target_system != target_system:
raise NotAchievedException("supplied item not of correct target system")
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")
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)
m = self.mav.recv_match(type='MISSION_ACK',
blocking=True,
timeout=1)
if m is None:
raise NotAchievedException("Did not receive MISSION_ACK")
if m.mission_type != mission_type:
raise NotAchievedException("Mission ack not of expected mission type")
if m.type != mavutil.mavlink.MAV_MISSION_ACCEPTED:
raise NotAchievedException("Mission upload failed (%u)" % m.type)
self.progress("Upload succeeded")
def download_using_mission_protocol(self, mission_type):
'''mavlink2 required'''
target_system = 1
target_component = 1
self.mav.mav.mission_request_list_send(target_system,
target_component,
mission_type)
while True:
m = self.mav.recv_match(type='MISSION_COUNT',
blocking=True,
timeout=1)
self.progress(str(m))
if m is None:
raise NotAchievedException("Did not get MISSION_COUNT response")
if m.target_component != 250:
continue
if m.mission_type != mission_type:
raise NotAchievedException("Mission count response of incorrect type")
break
items = []
tstart = self.get_sim_time_cached()
remaining_to_receive = set(range(0, m.count))
next_to_request = 0
while True:
if self.get_sim_time_cached() - tstart > 10:
raise NotAchievedException("timeout downloading %s" % str(mission_type))
if len(remaining_to_receive) == 0:
self.progress("All received")
return items
self.progress("Requesting item %u" % next_to_request)
self.mav.mav.mission_request_int_send(target_system,
target_component,
next_to_request,
mission_type)
m = self.mav.recv_match(type='MISSION_ITEM_INT',
blocking=True,
timeout=1)
if m is None:
raise NotAchievedException("Did not receive MISSION_ITEM_INT")
if m.mission_type != mission_type:
raise NotAchievedException("Received waypoint of wrong type")
if m.seq != next_to_request:
raise NotAchievedException("Received waypoint is out of sequence")
self.progress("Got item %u" % m.seq)
items.append(m)
next_to_request += 1
remaining_to_receive.discard(m.seq)
def test_gcs_fence(self):
self.progress("Testing FENCE_POINT protocol")
self.set_parameter("FENCE_TOTAL", 1)
@ -1129,6 +1240,545 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
raise NotAchievedException("Did not get sptgi message")
print("spgti: %s" % str(sptgi))
def assert_mission_count_on_link(self, mav, expected_count, target_system, target_component, mission_type):
mav.mav.mission_request_list_send(target_system,
target_component,
mission_type)
m = mav.recv_match(type="MISSION_COUNT", blocking=True, timeout=1)
if m is None:
raise NotAchievedException("Did not receive MISSION_COUNT on link")
if m.count != expected_count:
raise NotAchievedException("Bad count received (want=%u got=%u)" %
(expected_count, m.count))
def get_mission_item_on_link(self, item, mav, target_system, target_component, mission_type):
mav.mav.mission_request_int_send(target_system,
target_component,
item,
mission_type)
m = mav.recv_match(type='MISSION_ITEM_INT',
blocking=True,
timeout=1)
if m is None:
raise NotAchievedException("Did not receive mission item int")
if m.target_system != mav.mav.srcSystem:
raise NotAchievedException("Unexpected target system %u want=%u" %
(m.target_system, mav.mav.srcSystem))
if m.target_component != mav.mav.srcComponent:
raise NotAchievedException("Unexpected target component %u want=%u" %
(m.target_component, mav.mav.srcComponent))
return m
def clear_mission(self, mission_type, target_system, target_component):
self.mav.mav.mission_count_send(target_system,
target_component,
0,
mission_type)
m = self.mav.recv_match(type='MISSION_ACK',
blocking=True,
timeout=1)
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 %u" %
(m.type))
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',
blocking=True,
timeout=1)
if m is None:
raise NotAchievedException("Did not get item request")
if m.mission_type != mission_type:
raise NotAchievedException("Incorrect mission type (wanted=%u got=%u)" %
(mission_type, m.mission_type))
if m.seq != seq:
raise NotAchievedException("Unexpected sequence number (want=%u got=%u)" % (seq, m.seq))
self.progress("Received item request OK")
def assert_receive_mission_ack(self, mission_type,
want_type=mavutil.mavlink.MAV_MISSION_ACCEPTED,
target_system=None,
target_component=None):
if target_system is None:
target_system = self.mav.mav.srcSystem
if target_component is None:
target_component = self.mav.mav.srcComponent
self.progress("Expecting mission ack")
m = self.mav.recv_match(type='MISSION_ACK',
blocking=True,
timeout=1)
if m is None:
raise NotAchievedException("Expected mission ACK")
if m.target_system != target_system:
raise NotAchievedException("ACK not targetted at correct system want=%u got=%u" %
(self.mav.mav.srcSystem, m.target_system))
if m.target_component != target_component:
raise NotAchievedException("ACK not targetted at correct component")
if m.mission_type != mission_type:
raise NotAchievedException("Unexpected mission type %u want=%u" %
(m.mission_type, mission_type))
if m.type != want_type:
raise NotAchievedException("Expected ack type got %u got %u" %
(want_type, m.type))
def test_gcs_rally(self):
target_system = 1
target_component = 1
self.mavproxy.send('rally clear\n')
self.delay_sim_time(1)
if self.get_parameter("RALLY_TOTAL") != 0:
raise NotAchievedException("Failed to clear rally points")
old_srcSystem = self.mav.mav.srcSystem
# stop MAVProxy poking the autopilot:
self.mavproxy.send('module unload rally\n')
self.mavproxy.expect("Unloaded module rally")
self.mavproxy.send('module unload wp\n')
self.mavproxy.expect("Unloaded module wp")
try:
item1_lat = int(2.0000 *1e7)
items = [
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_WAYPOINT,
0, # current
0, # autocontinue
0, # 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_RALLY),
self.mav.mav.mission_item_int_encode(
target_system,
target_component,
1, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
item1_lat, # latitude
int(2.0000 *1e7), # longitude
32.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
self.mav.mav.mission_item_int_encode(
target_system,
target_component,
2, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(3.0000 *1e7), # latitude
int(3.0000 *1e7), # longitude
33.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
]
self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
items)
downloaded = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
print("Got items (%s)" % str(items))
if len(downloaded) != len(items):
raise NotAchievedException("Did not download correct number of items want=%u got=%u" % (len(downloaded), len(items)))
rally_total = self.get_parameter("RALLY_TOTAL")
if rally_total != len(downloaded):
raise NotAchievedException("Unexpected rally point count: want=%u got=%u" % (len(items), rally_total))
self.progress("Pruning count by setting parameter (urgh)")
self.set_parameter("RALLY_TOTAL", 2)
downloaded = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
if len(downloaded) != 2:
raise NotAchievedException("Failed to prune rally points by setting parameter. want=%u got=%u" % (2, len(downloaded)))
self.progress("Uploading a third item using old protocol")
new_item2_lat = int(6.0 *1e7)
self.set_parameter("RALLY_TOTAL", 3)
self.mav.mav.rally_point_send(target_system,
target_component,
2, # sequence number
3, # total count
new_item2_lat,
int(7.0 *1e7),
15,
0, # "break" alt?!
0, # "land dir"
0) # flags
downloaded = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
if len(downloaded) != 3:
raise NotAchievedException("resetting rally point count didn't change items returned")
if downloaded[2].x != new_item2_lat:
raise NotAchievedException("Bad lattitude in downloaded item: want=%u got=%u" % (new_item2_lat, downloaded[2].x))
self.progress("Grabbing original item 1 using original protocol")
self.mav.mav.rally_fetch_point_send(target_system,
target_component,
1)
m = self.mav.recv_match(type="RALLY_POINT", blocking=True, timeout=1)
if m.target_system != 255:
raise NotAchievedException("Bad target_system on received rally point (want=%u got=%u)" % (255, m.target_system))
if m.target_component != 250: # autotest's component ID
raise NotAchievedException("Bad target_component on received rally point")
if m.lat != item1_lat:
raise NotAchievedException("Bad latitude on received rally point")
self.start_subtest("Test upload lockout and timeout")
self.progress("Starting upload from normal sysid")
self.mav.mav.mission_count_send(target_system,
target_component,
len(items),
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.drain_mav() # throw away requests for items
self.mav.mav.srcSystem = 243
self.progress("Attempting upload from sysid=%u" %
(self.mav.mav.srcSystem,))
self.mav.mav.mission_count_send(target_system,
target_component,
len(items),
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
want_type=mavutil.mavlink.MAV_MISSION_DENIED)
self.progress("Attempting download from sysid=%u" %
(self.mav.mav.srcSystem,))
self.mav.mav.mission_request_list_send(target_system,
target_component,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
want_type=mavutil.mavlink.MAV_MISSION_DENIED)
# wait for the upload from sysid=1 to time out:
self.mavproxy.expect("upload timeout")
self.progress("Now trying to upload empty mission after timeout")
self.mav.mav.mission_count_send(target_system,
target_component,
0,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.drain_mav()
self.start_subtest("Check rally upload/download across separate links")
self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
items)
mav2 = mavutil.mavlink_connection("tcp:localhost:5763",
robust_parsing=True,
source_system = 7,
source_component=7)
expected_count = 3
self.progress("Assert mision count on new link")
self.assert_mission_count_on_link(mav2, expected_count, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.progress("Assert mission count on original link")
self.assert_mission_count_on_link(self.mav, expected_count, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.progress("Get first item on new link")
m2 = self.get_mission_item_on_link(2, mav2, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.progress("Get first item on original link")
m = self.get_mission_item_on_link(2, self.mav, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
if m2.x != m.x:
raise NotAchievedException("mission items do not match (%d vs %d)" % (m2.x, m.x))
self.start_subtest("Should enforce items come from correct GCS")
self.mav.mav.mission_count_send(target_system,
target_component,
1,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 0)
self.progress("Attempting to upload from bad sysid")
old_sysid = self.mav.mav.srcSystem
self.mav.mav.srcSystem = 17
items[0].pack(self.mav.mav)
self.mav.mav.send(items[0])
self.mav.mav.srcSystem = old_sysid
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
want_type=mavutil.mavlink.MAV_MISSION_DENIED,
target_system=17)
self.progress("Sending from correct sysid")
items[0].pack(self.mav.mav)
self.mav.mav.send(items[0])
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.drain_mav()
self.drain_all_pexpects()
self.start_subtest("Attempt to send item on different link to that which we are sending requests on")
self.progress("Sending count")
self.mav.mav.mission_count_send(target_system,
target_component,
2,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 0)
old_mav2_system = mav2.mav.srcSystem
old_mav2_component = mav2.mav.srcComponent
mav2.mav.srcSystem = self.mav.mav.srcSystem
mav2.mav.srcComponent = self.mav.mav.srcComponent
self.progress("Sending item on second link")
# note that the routing table in ArduPilot now will say
# this sysid/compid is on both links which may cause
# weirdness...
items[0].pack(mav2.mav)
mav2.mav.send(items[0])
mav2.mav.srcSystem = old_mav2_system
mav2.mav.srcComponent = old_mav2_component
# we continue to receive requests on the original link:
m = self.mav.recv_match(type='MISSION_REQUEST',
blocking=True,
timeout=1)
if m is None:
raise NotAchievedException("Did not get mission request")
if m.seq != 1:
raise NotAchievedException("Unexpected sequence number (expected=%u got=%u)" % (1, m.seq))
items[1].pack(self.mav.mav)
self.mav.mav.send(items[1])
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.drain_mav()
self.drain_all_pexpects()
self.start_subtest("Upload mission and rally points at same time")
self.progress("Sending rally count")
self.mav.mav.mission_count_send(target_system,
target_component,
3,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 0)
self.progress("Sending wp count")
self.mav.mav.mission_count_send(target_system,
target_component,
3,
mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 0)
self.progress("Answering request for mission item 0")
wp = self.mav.mav.mission_item_int_send(
target_system,
target_component,
0, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(1.1000 *1e7), # latitude
int(1.2000 *1e7), # longitude
321.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 1)
self.progress("Answering request for rally point 0")
items[0].pack(self.mav.mav)
self.mav.mav.send(items[0])
self.progress("Expecting request for rally item 1")
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 1)
self.progress("Answering request for rally point 1")
items[1].pack(self.mav.mav)
self.mav.mav.send(items[1])
self.progress("Expecting request for rally item 2")
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 2)
self.progress("Answering request for rally point 2")
items[2].pack(self.mav.mav)
self.mav.mav.send(items[2])
self.progress("Expecting mission ack for rally")
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.progress("Answering request for waypoints item 1")
wp = self.mav.mav.mission_item_int_send(
target_system,
target_component,
1, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(1.1000 *1e7), # latitude
int(1.2000 *1e7), # longitude
321.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, 2)
self.progress("Answering request for waypoints item 2")
self.mav.mav.mission_item_int_send(
target_system,
target_component,
2, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(1.1000 *1e7), # latitude
int(1.2000 *1e7), # longitude
321.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_MISSION),
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
self.start_subtest("Test write-partial-list")
self.progress("Clearing rally points using count-send")
self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
target_system,
target_component)
self.progress("Should not be able to set items completely past the waypoint count")
self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
items)
self.mav.mav.mission_write_partial_list_send(
target_system,
target_component,
17,
20,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
want_type=mavutil.mavlink.MAV_MISSION_ERROR)
self.progress("Should not be able to set items overlapping the waypoint count")
self.mav.mav.mission_write_partial_list_send(
target_system,
target_component,
0,
20,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
want_type=mavutil.mavlink.MAV_MISSION_ERROR)
self.progress("try to overwrite items 1 and 2")
self.mav.mav.mission_write_partial_list_send(
target_system,
target_component,
1,
2,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 1)
self.progress("Try shoving up an incorrectly sequenced item")
self.mav.mav.mission_item_int_send(
target_system,
target_component,
0, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(1.1000 *1e7), # latitude
int(1.2000 *1e7), # longitude
321.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
want_type=mavutil.mavlink.MAV_MISSION_INVALID_SEQUENCE)
self.progress("Try shoving up an incorrectly sequenced item (but within band)")
self.mav.mav.mission_item_int_send(
target_system,
target_component,
2, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
int(1.1000 *1e7), # latitude
int(1.2000 *1e7), # longitude
321.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
want_type=mavutil.mavlink.MAV_MISSION_INVALID_SEQUENCE)
self.progress("Now provide correct item")
item1_latitude = int(1.2345*1e7)
self.mav.mav.mission_item_int_send(
target_system,
target_component,
1, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, # current
0, # autocontinue
0, # p1
0, # p2
0, # p3
0, # p4
item1_latitude, # latitude
int(1.2000 *1e7), # longitude
321.0000, # altitude
mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
self.assert_receive_mission_item_request(mavutil.mavlink.MAV_MISSION_TYPE_RALLY, 2)
self.progress("Answering request for rally point 2")
items[2].pack(self.mav.mav)
self.mav.mav.send(items[2])
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.progress("TODO: ensure partial mission write was good")
self.start_subtest("clear mission types")
self.assert_mission_count_on_link(self.mav, 3, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_mission_count_on_link(self.mav, 3, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
self.mav.mav.mission_clear_all_send(target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_mission_count_on_link(self.mav, 0, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_mission_count_on_link(self.mav, 3, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
self.mav.mav.mission_clear_all_send(target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
self.assert_mission_count_on_link(self.mav, 0, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_mission_count_on_link(self.mav, 0, target_system, target_component, mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
self.start_subtest("try sending out-of-range counts")
self.mav.mav.mission_count_send(target_system,
target_component,
1,
230)
self.assert_receive_mission_ack(230,
want_type=mavutil.mavlink.MAV_MISSION_UNSUPPORTED)
self.mav.mav.mission_count_send(target_system,
target_component,
16000,
mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
self.assert_receive_mission_ack(mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
want_type=mavutil.mavlink.MAV_MISSION_NO_SPACE)
except Exception as e:
self.progress("Received exception (%s)" % self.get_exception_stacktrace(e))
self.mav.mav.srcSystem = old_srcSystem
raise e
self.mavproxy.send('module load rally\n')
self.mavproxy.expect("Loaded module rally")
self.mavproxy.send('module load wp\n')
self.mavproxy.expect("Loaded module wp")
def tests(self):
'''return list of all tests'''
ret = super(AutoTestRover, self).tests()
@ -1235,6 +1885,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
"Upload and download of fence",
self.test_gcs_fence),
("GCSRally",
"Upload and download of rally",
self.test_gcs_rally),
("DataFlashOverMAVLink",
"Test DataFlash over MAVLink",
self.test_dataflash_over_mavlink),

6
Tools/autotest/common.py
View File

@ -442,10 +442,12 @@ class AutoTest(ABC):
continue
util.pexpect_drain(p)
def drain_mav(self):
def drain_mav(self, mav=None):
if mav is None:
mav = self.mav
count = 0
tstart = time.time()
while self.mav.recv_match(blocking=False) is not None:
while mav.recv_match(blocking=False) is not None:
count += 1
tdelta = time.time() - tstart
if tdelta == 0: