AutoTest: revert change that breaks quadplane

This commit is contained in:
James O'Shannessy 2021-02-16 20:05:20 +11:00 committed by Peter Barker
parent 2353b55452
commit 01b27bdb61
1 changed files with 16 additions and 0 deletions

View File

@ -1514,6 +1514,22 @@ class AutoTest(ABC):
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,