Skip to content

Commit

Permalink
autotest: update plane switch fence test
Browse files Browse the repository at this point in the history
the FENCE_AUTOENABLE option should be honoured even with a fence
switch in the disable position
  • Loading branch information
tridge committed Dec 11, 2024
1 parent 076782f commit 9dc6e1c
Showing 1 changed file with 50 additions and 9 deletions.
59 changes: 50 additions & 9 deletions Tools/autotest/arduplane.py
Original file line number Diff line number Diff line change
Expand Up @@ -3661,7 +3661,7 @@ def FenceMinAltAutoEnableAbort(self):
def FenceAutoEnableDisableSwitch(self):
'''Tests autoenablement of regular fences and manual disablement'''
self.set_parameters({
"FENCE_TYPE": 11, # Set fence type to min alt
"FENCE_TYPE": 9, # Set fence type to min alt, max alt
"FENCE_ACTION": 1, # Set action to RTL
"FENCE_ALT_MIN": 50,
"FENCE_ALT_MAX": 100,
Expand All @@ -3672,35 +3672,60 @@ def FenceAutoEnableDisableSwitch(self):
"FENCE_RET_ALT" : 0,
"FENCE_RET_RALLY" : 0,
"FENCE_TOTAL" : 0,
"RTL_ALTITUDE" : 75,
"TKOFF_ALT" : 75,
"RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality
})
self.reboot_sitl()
self.context_collect("STATUSTEXT")

fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
# Grab Home Position
self.mav.recv_match(type='HOME_POSITION', blocking=True)
self.set_rc_from_map({7: 1000}) # Turn fence off with aux function
self.set_rc(7, 1000) # Turn fence off with aux function, does not impact later auto-enable

self.wait_ready_to_arm()

self.progress("Check fence disabled at boot")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
if (m.onboard_control_sensors_enabled & fence_bit):
raise NotAchievedException("Fence is enabled at boot")

cruise_alt = 75
self.takeoff(cruise_alt, mode='TAKEOFF')

self.progress("Fly above ceiling and check there is no breach")
self.progress("Fly above ceiling and check there is a breach")
self.change_mode('FBWA')
self.set_rc(3, 2000)
self.change_altitude(cruise_alt + 80, relative=True)
self.set_rc(2, 1000)

self.wait_statustext("Max Alt fence breached", timeout=10, check_context=True)
self.wait_mode('RTL')

m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (not (m.onboard_control_sensors_health & fence_bit)):
raise NotAchievedException("Fence Ceiling breached")
if (m.onboard_control_sensors_health & fence_bit):
raise NotAchievedException("Fence ceiling not breached")

self.set_rc(3, 1500)
self.set_rc(2, 1500)

self.progress("Wait for RTL alt reached")
self.wait_altitude(cruise_alt-5, cruise_alt+5, relative=True, timeout=30)

self.progress("Return to cruise alt")
self.set_rc(3, 1500)
self.change_altitude(cruise_alt, relative=True)

self.progress("Check fence breach cleared")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
if (not (m.onboard_control_sensors_health & fence_bit)):
raise NotAchievedException("Fence breach not cleared")

self.progress("Fly below floor and check for breach")
self.set_rc(2, 2000)
self.wait_statustext("Min Alt fence breached", timeout=10, check_context=True)
self.wait_mode("RTL")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (m.onboard_control_sensors_health & fence_bit):
raise NotAchievedException("Fence floor not breached")

Expand All @@ -3710,10 +3735,26 @@ def FenceAutoEnableDisableSwitch(self):
self.set_rc(3, 2000)
self.change_altitude(75, relative=True)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Got (%s)" % str(m))
if (not (m.onboard_control_sensors_enabled & fence_bit)):
raise NotAchievedException("Fence Floor not enabled")

self.progress("Toggle fence enable/disable")
self.set_rc(7, 2000)
self.delay_sim_time(2)
self.set_rc(7, 1000)
self.delay_sim_time(2)

self.progress("Check fence is disabled")
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
if (m.onboard_control_sensors_enabled & fence_bit):
raise NotAchievedException("Fence disable with switch failed")

self.progress("Fly below floor and check for no breach")
self.change_altitude(40, relative=True)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
if (not (m.onboard_control_sensors_health & fence_bit)):
raise NotAchievedException("Fence floor breached")

self.progress("Return to cruise alt")
self.set_rc(3, 1500)
self.change_altitude(cruise_alt, relative=True)
Expand Down

0 comments on commit 9dc6e1c

Please sign in to comment.