Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

autotest: correct uploading of exclusion circles

Browse Source
This commit is contained in:
Peter Barker 2024-07-16 17:31:13 +10:00 committed by Peter Barker
parent 1de60a31b0
commit 8dcd3986c7
3 changed files with 215 additions and 247 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
Tools/autotest/arduplane.py
View File

@ -1608,18 +1608,14 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
# Test arming outside inclusion zone # Test arming outside inclusion zone
self.progress("Test arming while vehicle outside of inclusion zone") self.progress("Test arming while vehicle outside of inclusion zone")
self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types
locs = [ self.upload_fences_from_locations([(
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
mavutil.location(1.000, 1.000, 0, 0), mavutil.location(1.000, 1.000, 0, 0),
mavutil.location(1.000, 1.001, 0, 0), mavutil.location(1.000, 1.001, 0, 0),
mavutil.location(1.001, 1.001, 0, 0), mavutil.location(1.001, 1.001, 0, 0),
mavutil.location(1.001, 1.000, 0, 0) mavutil.location(1.001, 1.000, 0, 0)
] ]
self.upload_fences_from_locations( )])
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
[
locs
]
)
self.delay_sim_time(10) # let fence check run so it loads-from-eeprom self.delay_sim_time(10) # let fence check run so it loads-from-eeprom
self.do_fence_enable() self.do_fence_enable()
self.assert_fence_enabled() self.assert_fence_enabled()
@ -1637,12 +1633,9 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0), mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0),
mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0),
] ]
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, locs),
[ ])
locs
]
)
self.delay_sim_time(10) # let fence check run so it loads-from-eeprom self.delay_sim_time(10) # let fence check run so it loads-from-eeprom
self.do_fence_enable() self.do_fence_enable()
self.assert_fence_enabled() self.assert_fence_enabled()
@ -3720,12 +3713,9 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0), mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0),
mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0),
] ]
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs),
[ ])
locs
]
)
self.delay_sim_time(1) self.delay_sim_time(1)
self.wait_ready_to_arm() self.wait_ready_to_arm()
self.takeoff(alt=50) self.takeoff(alt=50)
@ -3783,12 +3773,9 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.003, 0, 0), mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.003, 0, 0),
mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0),
] ]
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs),
[ ])
locs
]
)
self.delay_sim_time(1) self.delay_sim_time(1)
self.wait_ready_to_arm() self.wait_ready_to_arm()
self.takeoff(alt=50) self.takeoff(alt=50)

194
Tools/autotest/rover.py
View File

@ -4195,22 +4195,22 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
here = self.mav.location() here = self.mav.location()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
[ # east
[ # east
self.offset_location_ne(here, -50, 20), # bl self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl, self.offset_location_ne(here, -50, 40), # tl,
], [ # over the top of the vehicle ]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl, self.offset_location_ne(here, 50, -50), # tl,
] ]),
] ])
)
self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz
self.drain_mav() self.drain_mav()
self.assert_fence_breached() self.assert_fence_breached()
@ -4231,22 +4231,22 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
here = self.mav.location() here = self.mav.location()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
[ # east
[ # east
self.offset_location_ne(here, -50, 20), # bl self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl, self.offset_location_ne(here, -50, 40), # tl,
], [ # over the top of the vehicle ]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl, self.offset_location_ne(here, 50, -50), # tl,
] ]),
] ])
)
self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz self.delay_sim_time(5) # ArduPilot only checks for breaches @1Hz
self.drain_mav() self.drain_mav()
self.assert_fence_breached() self.assert_fence_breached()
@ -4605,22 +4605,22 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_reboot_survivability(self): def test_poly_fence_reboot_survivability(self):
here = self.mav.location() here = self.mav.location()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
[ # east
[ # east
self.offset_location_ne(here, -50, 20), # bl self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl, self.offset_location_ne(here, -50, 40), # tl,
], [ # over the top of the vehicle ]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl, self.offset_location_ne(here, 50, -50), # tl,
] ]),
] ])
)
self.reboot_sitl() self.reboot_sitl()
downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE)
downloaded_len = len(downloaded_items) downloaded_len = len(downloaded_items)
@ -4663,17 +4663,15 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_inclusion_overlapping_inclusion_circles(self, here, target_system=1, target_component=1): def test_poly_fence_inclusion_overlapping_inclusion_circles(self, here, target_system=1, target_component=1):
self.start_subtest("Overlapping circular inclusion") self.start_subtest("Overlapping circular inclusion")
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, {
[
{
"radius": 30, "radius": 30,
"loc": self.offset_location_ne(here, -20, 0), "loc": self.offset_location_ne(here, -20, 0),
}, }),
{ (mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, {
"radius": 30, "radius": 30,
"loc": self.offset_location_ne(here, 20, 0), "loc": self.offset_location_ne(here, 20, 0),
}, }),
]) ])
if self.mavproxy is not None: if self.mavproxy is not None:
# handy for getting pretty pictures # handy for getting pretty pictures
@ -4702,19 +4700,17 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
target_system=target_system, target_system=target_system,
target_component=target_component) target_component=target_component)
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
[
[
self.offset_location_ne(here, -40, -20), # tl self.offset_location_ne(here, -40, -20), # tl
self.offset_location_ne(here, 50, -20), # tr self.offset_location_ne(here, 50, -20), # tr
self.offset_location_ne(here, 50, 20), # br self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, -40, 20), # bl, self.offset_location_ne(here, -40, 20), # bl,
], ]),
{ (mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, {
"radius": 30, "radius": 30,
"loc": self.offset_location_ne(here, -20, 0), "loc": self.offset_location_ne(here, -20, 0),
}, }),
]) ])
self.delay_sim_time(5) self.delay_sim_time(5)
@ -4735,21 +4731,19 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
target_system=target_system, target_system=target_system,
target_component=target_component) target_component=target_component)
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
[
[
self.offset_location_ne(here, -20, -25), # tl self.offset_location_ne(here, -20, -25), # tl
self.offset_location_ne(here, 50, -25), # tr self.offset_location_ne(here, 50, -25), # tr
self.offset_location_ne(here, 50, 15), # br self.offset_location_ne(here, 50, 15), # br
self.offset_location_ne(here, -20, 15), # bl, self.offset_location_ne(here, -20, 15), # bl,
], ]),
[ (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
self.offset_location_ne(here, 20, -20), # tl self.offset_location_ne(here, 20, -20), # tl
self.offset_location_ne(here, -50, -20), # tr self.offset_location_ne(here, -50, -20), # tr
self.offset_location_ne(here, -50, 20), # br self.offset_location_ne(here, -50, 20), # br
self.offset_location_ne(here, 20, 20), # bl, self.offset_location_ne(here, 20, 20), # bl,
], ]),
]) ])
self.delay_sim_time(5) self.delay_sim_time(5)
@ -4772,23 +4766,25 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_exclusion(self, here, target_system=1, target_component=1): def test_poly_fence_exclusion(self, here, target_system=1, target_component=1):
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
[ # east
[ # east
self.offset_location_ne(here, -50, 20), # bl self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl, self.offset_location_ne(here, -50, 40), # tl,
], [ # west ]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# west
self.offset_location_ne(here, -50, -20), # tl self.offset_location_ne(here, -50, -20), # tl
self.offset_location_ne(here, 50, -20), # tr self.offset_location_ne(here, 50, -20), # tr
self.offset_location_ne(here, 50, -40), # br self.offset_location_ne(here, 50, -40), # br
self.offset_location_ne(here, -50, -40), # bl, self.offset_location_ne(here, -50, -40), # bl,
], { ]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, {
"radius": 30, "radius": 30,
"loc": self.offset_location_ne(here, -60, 0), "loc": self.offset_location_ne(here, -60, 0),
}, }),
]) ])
self.delay_sim_time(5) self.delay_sim_time(5)
if self.mavproxy is not None: if self.mavproxy is not None:
@ -5009,21 +5005,21 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_object_avoidance_guided_two_squares(self, target_system=1, target_component=1): 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") self.start_subtest("Ensure we can steer around obstacles in guided mode")
here = self.mav.location() here = self.mav.location()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
[ # east
[ # east
self.offset_location_ne(here, -50, 20), # bl self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 10), # tl self.offset_location_ne(here, 50, 10), # tl
self.offset_location_ne(here, 50, 30), # tr self.offset_location_ne(here, 50, 30), # tr
self.offset_location_ne(here, -50, 40), # br, self.offset_location_ne(here, -50, 40), # br,
], ]),
[ # further east (and south (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# further east (and south
self.offset_location_ne(here, -60, 60), # bl self.offset_location_ne(here, -60, 60), # bl
self.offset_location_ne(here, 40, 70), # tl self.offset_location_ne(here, 40, 70), # tl
self.offset_location_ne(here, 40, 90), # tr self.offset_location_ne(here, 40, 90), # tr
self.offset_location_ne(here, -60, 80), # br, self.offset_location_ne(here, -60, 80), # br,
], ]),
]) ])
if self.mavproxy is not None: if self.mavproxy is not None:
self.mavproxy.send("fence list\n") self.mavproxy.send("fence list\n")
@ -5061,23 +5057,25 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def test_poly_fence_avoidance_dont_breach_exclusion(self, target_system=1, target_component=1): 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") self.start_subtest("Ensure we stop before breaching an exclusion fence")
here = self.mav.location() here = self.mav.location()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
[ # east
[ # east
self.offset_location_ne(here, -50, 20), # bl self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl, self.offset_location_ne(here, -50, 40), # tl,
], [ # west ]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# west
self.offset_location_ne(here, -50, -20), # tl self.offset_location_ne(here, -50, -20), # tl
self.offset_location_ne(here, 50, -20), # tr self.offset_location_ne(here, 50, -20), # tr
self.offset_location_ne(here, 50, -40), # br self.offset_location_ne(here, 50, -40), # br
self.offset_location_ne(here, -50, -40), # bl, self.offset_location_ne(here, -50, -40), # bl,
], { ]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, {
"radius": 30, "radius": 30,
"loc": self.offset_location_ne(here, -60, 0), "loc": self.offset_location_ne(here, -60, 0),
}, }),
]) ])
if self.mavproxy is not None: if self.mavproxy is not None:
self.mavproxy.send("fence list\n") self.mavproxy.send("fence list\n")
@ -6631,22 +6629,22 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
'''ensure MAV_CMD_DO_FENCE_ENABLE mavlink command works''' '''ensure MAV_CMD_DO_FENCE_ENABLE mavlink command works'''
here = self.mav.location() here = self.mav.location()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
[ # east
[ # east
self.offset_location_ne(here, -50, 20), # bl self.offset_location_ne(here, -50, 20), # bl
self.offset_location_ne(here, 50, 20), # br self.offset_location_ne(here, 50, 20), # br
self.offset_location_ne(here, 50, 40), # tr self.offset_location_ne(here, 50, 40), # tr
self.offset_location_ne(here, -50, 40), # tl, self.offset_location_ne(here, -50, 40), # tl,
], [ # over the top of the vehicle ]),
(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [
# over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl, self.offset_location_ne(here, 50, -50), # tl,
] ]),
] ])
)
# enable: # enable:
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1) self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1)
@ -6847,17 +6845,15 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.progress("Ensure we can arm when we have an inclusion fence we are inside of") self.progress("Ensure we can arm when we have an inclusion fence we are inside of")
here = self.mav.location() here = self.mav.location()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
[ # over the top of the vehicle
[ # over the top of the vehicle
self.offset_location_ne(here, -50, -50), # bl self.offset_location_ne(here, -50, -50), # bl
self.offset_location_ne(here, -50, 50), # br self.offset_location_ne(here, -50, 50), # br
self.offset_location_ne(here, 50, 50), # tr self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, -50), # tl, self.offset_location_ne(here, 50, -50), # tl,
] ]),
] ])
)
self.delay_sim_time(5) self.delay_sim_time(5)
self.wait_ready_to_arm() self.wait_ready_to_arm()
@ -6869,17 +6865,15 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.progress("Now create a fence we are in breach of") self.progress("Now create a fence we are in breach of")
here = self.mav.location() here = self.mav.location()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
[ # over the top of the vehicle
[ # over the top of the vehicle
self.offset_location_ne(here, 20, 20), # bl self.offset_location_ne(here, 20, 20), # bl
self.offset_location_ne(here, 20, 50), # br self.offset_location_ne(here, 20, 50), # br
self.offset_location_ne(here, 50, 50), # tr self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, 20), # tl, self.offset_location_ne(here, 50, 20), # tl,
] ]),
] ])
)
self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False) self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False)
self.reboot_sitl() self.reboot_sitl()
@ -6890,34 +6884,30 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.clear_fence() self.clear_fence()
self.wait_ready_to_arm() self.wait_ready_to_arm()
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
[ # over the top of the vehicle
[ # over the top of the vehicle
self.offset_location_ne(here, 20, 20), # bl self.offset_location_ne(here, 20, 20), # bl
self.offset_location_ne(here, 20, 50), # br self.offset_location_ne(here, 20, 50), # br
self.offset_location_ne(here, 50, 50), # tr self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, 20), # tl, self.offset_location_ne(here, 50, 20), # tl,
] ]),
] ])
)
self.reboot_sitl() self.reboot_sitl()
self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120)
self.clear_fence() self.clear_fence()
self.wait_ready_to_arm() self.wait_ready_to_arm()
self.progress("Ensure we can arm after clearing polygon fence type enabled") self.progress("Ensure we can arm after clearing polygon fence type enabled")
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [
[ # over the top of the vehicle
[ # over the top of the vehicle
self.offset_location_ne(here, 20, 20), # bl self.offset_location_ne(here, 20, 20), # bl
self.offset_location_ne(here, 20, 50), # br self.offset_location_ne(here, 20, 50), # br
self.offset_location_ne(here, 50, 50), # tr self.offset_location_ne(here, 50, 50), # tr
self.offset_location_ne(here, 50, 20), # tl, self.offset_location_ne(here, 50, 20), # tl,
] ]),
] ])
)
self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120)
self.set_parameter('FENCE_TYPE', 2) self.set_parameter('FENCE_TYPE', 2)
self.wait_ready_to_arm() self.wait_ready_to_arm()

21
Tools/autotest/vehicle_test_suite.py
View File

@ -2106,10 +2106,8 @@ class TestSuite(ABC):
locs2.append(copy.copy(locs2[1])) locs2.append(copy.copy(locs2[1]))
return self.roundtrip_fence_using_fencepoint_protocol(locs2) return self.roundtrip_fence_using_fencepoint_protocol(locs2)
self.upload_fences_from_locations( self.upload_fences_from_locations([
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs),
[
locs
]) ])
def send_reboot_command(self): def send_reboot_command(self):
@ -11884,26 +11882,19 @@ Also, ignores heartbeats not from our target system'''
'''return mode vehicle should start in with default RC inputs set''' '''return mode vehicle should start in with default RC inputs set'''
return None return None
def upload_fences_from_locations(self, def upload_fences_from_locations(self, fences, target_system=1, target_component=1):
vertex_type,
list_of_list_of_locs,
target_system=1,
target_component=1):
seq = 0 seq = 0
items = [] items = []
for locs in list_of_list_of_locs:
for (vertex_type, locs) in fences:
if isinstance(locs, dict): if isinstance(locs, dict):
# circular fence # 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( item = self.mav.mav.mission_item_int_encode(
target_system, target_system,
target_component, target_component,
seq, # seq seq, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL, mavutil.mavlink.MAV_FRAME_GLOBAL,
v, vertex_type,
0, # current 0, # current
0, # autocontinue 0, # autocontinue
locs["radius"], # p1 locs["radius"], # p1