zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

autotest: don't use MAVProxy for uploading using fencepoint protocol

c415-sdk
Peter Barker 4 years ago committed by Peter Barker
parent
2b06ae9aa0
commit
d31f676c33
2 changed files with 101 additions and 70 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 102
      Tools/autotest/common.py
  2. 69
      Tools/autotest/rover.py

102
Tools/autotest/common.py
Unescape View File

@ -1358,8 +1358,108 @@ class AutoTest(ABC): @@ -1358,8 +1358,108 @@ class AutoTest(ABC):
break
self.delay_sim_time(1)
def get_fence_point(self, idx, target_system=1, target_component=1):
self.mav.mav.fence_fetch_point_send(target_system,
target_component,
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 m.idx != idx:
raise NotAchievedException("Invalid idx returned (want=%u got=%u)" %
(idx, m.seq))
return m
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,
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 load_fence(self, filename):
self.load_fence_using_mavproxy(filename)
filepath = os.path.join(testdir, self.current_test_name_directory, filename)
self.progress("Loading fence from (%s)" % str(filepath))
locs = []
for line in open(filepath,'rb'):
if len(line) == 0:
continue
m = re.match("([-\d.]+)\s+([-\d.]+)\s*", line.decode('ascii'))
if m is None:
raise ValueError("Did not match (%s)" % line)
locs.append(mavutil.location(float(m.group(1)), float(m.group(2)), 0, 0))
if self.is_plane():
# create return point as the centroid:
total_lat = 0
total_lng = 0
total_cnt = 0
for loc in locs:
total_lat += loc.lat
total_lng += loc.lng
total_cnt += 1
locs2 = [mavutil.location(total_lat/total_cnt,
total_lng/total_cnt,
0,
0)] # return point
locs2.extend(locs)
locs2.append(copy.copy(locs2[1]))
return self.roundtrip_fence_using_fencepoint_protocol(locs2)
self.upload_fences_from_locations(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
locs
])
def fetch_parameters(self):
self.mavproxy.send("param fetch\n")

69
Tools/autotest/rover.py
Unescape View File

@ -1677,62 +1677,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) @@ -1677,62 +1677,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
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,
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 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,
@ -1753,19 +1697,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) @@ -1753,19 +1697,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
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,
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 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(

Write Preview
Loading…
Cancel
Save