diff --git a/Tools/autotest/Generic_Missions/CMAC-fence.txt b/Tools/autotest/Generic_Missions/CMAC-fence.txt new file mode 100644 index 0000000000..a6169e4e3c --- /dev/null +++ b/Tools/autotest/Generic_Missions/CMAC-fence.txt @@ -0,0 +1,6 @@ +-35.363720 149.163651 +-35.358738 149.165070 +-35.359295 149.154434 +-35.372292 149.157135 +-35.368290 149.166809 +-35.358738 149.165070 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index ccd8147ec0..e0eda4cdae 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1605,6 +1605,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): "FENCE_ENABLE": 1, "AVOID_ENABLE": 0, "FENCE_TYPE": 1, + "FENCE_ENABLE" : 1, }) self.change_alt(10) @@ -1692,6 +1693,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): # enable fence, disable avoidance self.set_parameters({ "AVOID_ENABLE": 0, + "FENCE_ENABLE" : 1, "FENCE_TYPE": 8, "FENCE_ALT_MIN": 20, }) @@ -1737,6 +1739,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.progress("Test Landing while fence floor enabled") self.set_parameters({ "AVOID_ENABLE": 0, + "FENCE_ENABLE" : 1, "FENCE_TYPE": 15, "FENCE_ALT_MIN": 10, "FENCE_ALT_MAX": 20, @@ -1757,16 +1760,100 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) + self.wait_landed_and_disarmed() self.assert_fence_enabled() - # Assert fence is not healthy + # Assert fence is not healthy since it was enabled manually self.assert_sensor_state(fence_bit, healthy=False) # Disable the fence using mavlink command to ensure cleaned up SITL state self.do_fence_disable() self.assert_fence_disabled() + def FenceFloorAutoDisableLanding(self): + """Ensures we can initiate and complete an RTL while the fence is enabled""" + + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.progress("Test Landing while fence floor enabled") + self.set_parameters({ + "AVOID_ENABLE": 0, + "FENCE_TYPE": 11, + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 20, + "FENCE_AUTOENABLE" : 1, + }) + + self.change_mode("GUIDED") + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(alt_min=15, mode="GUIDED") + + # Check fence is enabled + self.assert_fence_enabled() + + # Change to RC controlled mode + self.change_mode('LOITER') + + self.set_rc(3, 1800) + + self.wait_mode('RTL', timeout=120) + # Assert fence is not healthy now that we are in RTL + self.assert_sensor_state(fence_bit, healthy=False) + + self.wait_landed_and_disarmed(0) + # the breach should have cleared since we auto-disable the + # fence on landing + self.assert_fence_disabled() + + # Assert fences have gone now that we have landed and disarmed + self.assert_sensor_state(fence_bit, present=True, enabled=False) + + def FenceFloorAutoEnableOnArming(self): + """Ensures we can auto-enable fences on arming and still takeoff and land""" + + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.set_parameters({ + "AVOID_ENABLE": 0, + "FENCE_TYPE": 11, + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 20, + "FENCE_AUTOENABLE" : 3, + }) + + self.change_mode("GUIDED") + # Check fence is not enabled + self.assert_fence_disabled() + + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(alt_min=15, mode="GUIDED") + + # Check fence is enabled + self.assert_fence_enabled() + + # Change to RC controlled mode + self.change_mode('LOITER') + + self.set_rc(3, 1800) + + self.wait_mode('RTL', timeout=120) + # Assert fence is not healthy now that we are in RTL + self.assert_sensor_state(fence_bit, healthy=False) + + self.wait_landed_and_disarmed(0) + # the breach should have cleared since we auto-disable the + # fence on landing + self.assert_fence_disabled() + + # Assert fences have gone now that we have landed and disarmed + self.assert_sensor_state(fence_bit, present=True, enabled=False) + + # Disable the fence using mavlink command to ensure cleaned up SITL state + self.assert_fence_disabled() + def GPSGlitchLoiter(self, timeout=30, max_distance=20): """fly_gps_glitch_loiter_test. Fly south east in loiter and test reaction to gps glitch.""" @@ -7631,6 +7718,54 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.set_parameter("FENCE_ENABLE", 1) self.check_avoidance_corners() + def AvoidanceAltFence(self): + '''Test fence avoidance at minimum and maximum altitude''' + ex = None + try: + self.set_parameters({ + "FENCE_ENABLE": 1, + "FENCE_TYPE": 9, # min and max alt fence + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 30, + }) + + self.change_mode('LOITER') + self.wait_ekf_happy() + + tstart = self.get_sim_time() + self.takeoff(15, mode='LOITER') + self.progress("Increasing throttle, vehicle should stay below 30m") + self.set_rc(3, 1920) + + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + alt = self.get_altitude(relative=True) + self.progress("Altitude %s" % alt) + if alt > 30: + raise NotAchievedException("Breached maximum altitude (%s)" % (str(alt),)) + + self.progress("Decreasing, vehicle should stay above 10m") + self.set_rc(3, 1080) + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + alt = self.get_altitude(relative=True) + self.progress("Altitude %s" % alt) + if alt < 10: + raise NotAchievedException("Breached minimum altitude (%s)" % (str(alt),)) + + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.land_and_disarm() + self.disarm_vehicle(force=True) + if ex is not None: + raise ex + def ModeFollow(self): '''Fly follow mode''' foll_ofs_x = 30 # metres @@ -10509,6 +10644,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.AC_Avoidance_Proximity_AVOID_ALT_MIN, self.AC_Avoidance_Fence, self.AC_Avoidance_Beacon, + self.AvoidanceAltFence, self.BaroWindCorrection, self.SetpointGlobalPos, self.ThrowDoubleDrop, @@ -10528,6 +10664,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.MaxAltFenceAvoid, self.MinAltFence, self.FenceFloorEnabledLanding, + self.FenceFloorAutoDisableLanding, + self.FenceFloorAutoEnableOnArming, self.AutoTuneSwitch, self.GPSGlitchLoiter, self.GPSGlitchLoiter2, diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index e57294be11..49d1366189 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3698,6 +3698,332 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.fly_home_land_and_disarm(timeout=150) + def FenceMinAltAutoEnable(self): + '''Tests autoenablement of the alt min fence and fences on arming''' + self.set_parameters({ + "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 25, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 3, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # check we can takeoff again + for i in [1, 2]: + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + # max alt fence should now be enabled + if i == 1: + self.assert_fence_enabled() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + self.fly_home_land_and_disarm() + self.change_mode("FBWA") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.set_current_waypoint(0, check_afterwards=False) + self.set_rc(3, 1000) # lower throttle + + def FenceMinAltAutoEnableAbort(self): + '''Tests autoenablement of the alt min fence and fences on arming''' + self.set_parameters({ + "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 25, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 3, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + # max alt fence should now be enabled + self.assert_fence_enabled() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + + self.load_generic_mission("flaps.txt") + self.change_mode("AUTO") + self.wait_distance_to_waypoint(8, 100, 10000000) + self.set_current_waypoint(8) + # abort the landing + self.wait_altitude(10, 20, timeout=200, relative=True) + self.change_mode("CRUISE") + self.set_rc(2, 1200) + # self.set_rc(3, 1600) # raise throttle + self.wait_altitude(30, 40, timeout=200, relative=True) + + self.change_mode("AUTO") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.fly_home_land_and_disarm(timeout=150) + self.wait_disarmed() + + def FenceAutoEnableDisableSwitch(self): + '''Tests autoenablement of regular fences and manual disablement''' + self.set_parameters({ + "FENCE_TYPE": 11, # Set fence type to min alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 50, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 2, + "FENCE_OPTIONS" : 1, + "FENCE_ENABLE" : 1, + "FENCE_RADIUS" : 300, + "FENCE_RET_ALT" : 0, + "FENCE_RET_RALLY" : 0, + "FENCE_TOTAL" : 0, + "TKOFF_ALT" : 75, + "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality + }) + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + self.set_rc_from_map({7: 1000}) # Turn fence off with aux function + + self.wait_ready_to_arm() + cruise_alt = 75 + self.takeoff(cruise_alt, mode='TAKEOFF') + + self.progress("Fly above ceiling and check there is no breach") + self.set_rc(3, 2000) + self.change_altitude(self.homeloc.alt + cruise_alt + 80) + 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") + + self.progress("Return to cruise alt") + self.set_rc(3, 1500) + self.change_altitude(self.homeloc.alt + cruise_alt) + + self.progress("Fly below floor and check for no breach") + self.change_altitude(self.homeloc.alt + 25) + 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") + + self.progress("Fly above floor and check fence is not re-enabled") + self.set_rc(3, 2000) + self.change_altitude(self.homeloc.alt + 75) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence Ceiling re-enabled") + + self.progress("Return to cruise alt") + self.set_rc(3, 1500) + self.change_altitude(self.homeloc.alt + cruise_alt) + self.fly_home_land_and_disarm(timeout=250) + + def FenceEnableDisableSwitch(self): + '''Tests enablement and disablement of fences on a switch''' + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.set_parameters({ + "FENCE_TYPE": 4, # Set fence type to polyfence + "FENCE_ACTION": 6, # Set action to GUIDED + "FENCE_ALT_MIN": 10, + "FENCE_ENABLE" : 0, + "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality + }) + + self.progress("Checking fence is not present before being configured") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence enabled before being configured") + + self.wait_ready_to_arm() + # takeoff at a lower altitude to avoid immediately breaching polyfence + self.takeoff(alt=25) + self.change_mode("FBWA") + + self.load_fence("CMAC-fence.txt") + + self.set_rc_from_map({ + 3: 1500, + 7: 2000, + }) # Turn fence on with aux function + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Checking fence is initially OK") + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, + present=True, + enabled=True, + healthy=True, + verbose=False, + timeout=30) + + self.progress("Waiting for GUIDED") + tstart = self.get_sim_time() + mode = "GUIDED" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # check we are in breach + self.assert_fence_enabled() + + self.set_rc_from_map({ + 7: 1000, + }) # Turn fence off with aux function + + # wait to no longer be in breach + self.delay_sim_time(5) + self.assert_fence_disabled() + + self.fly_home_land_and_disarm(timeout=250) + self.do_fence_disable() # Ensure the fence is disabled after test + + def FenceEnableDisableAux(self): + '''Tests enablement and disablement of fences via aux command''' + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + enable = 0 + self.set_parameters({ + "FENCE_TYPE": 12, # Set fence type to polyfence + AltMin + "FENCE_ALT_MIN": 10, + "FENCE_ENABLE" : enable, + }) + + if not enable: + self.progress("Checking fence is not present before being configured") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence enabled before being configured") + + self.load_fence("CMAC-fence.txt") + + self.wait_ready_to_arm() + # takeoff at a lower altitude to avoid immediately breaching polyfence + self.takeoff(alt=25) + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + self.run_auxfunc( + 11, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Checking fence is initially OK") + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, + present=True, + enabled=True, + healthy=True, + verbose=False, + timeout=30) + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # check we are in breach + self.assert_fence_enabled() + self.assert_fence_sys_status(True, True, False) + + # wait until we get home + self.wait_distance_to_home(50, 100, timeout=200) + # now check we are now not in breach + self.assert_fence_sys_status(True, True, True) + # Turn fence off with aux function + self.run_auxfunc( + 11, + 0, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + # switch back to cruise + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + # re-enable the fences + self.run_auxfunc( + 11, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + + # wait to no longer be in breach + self.wait_distance_to_home(50, 100, timeout=200) + self.assert_fence_sys_status(True, True, True) + + # fly home and land with fences still enabled + self.fly_home_land_and_disarm(timeout=250) + self.do_fence_disable() # Ensure the fence is disabled after test + def FenceBreachedChangeMode(self): '''Tests manual mode change after fence breach, as set with FENCE_OPTIONS''' """ Attempts to change mode while a fence is breached. @@ -5447,6 +5773,11 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.FenceRTLRally, self.FenceRetRally, self.FenceAltCeilFloor, + self.FenceMinAltAutoEnable, + self.FenceMinAltAutoEnableAbort, + self.FenceAutoEnableDisableSwitch, + self.FenceEnableDisableSwitch, + self.FenceEnableDisableAux, self.FenceBreachedChangeMode, self.FenceNoFenceReturnPoint, self.FenceNoFenceReturnPointInclusion, diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 8e62d685be..22c38c5c50 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -4061,6 +4061,9 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, target_system=target_system, target_component=target_component) + self.set_parameters({ + "FENCE_TYPE": 2, # circle only + }) self.delay_sim_time(5) # let breaches clear # FIXME: should we allow this? self.progress("Ensure we can arm with no poly in place") @@ -4068,6 +4071,9 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.wait_ready_to_arm() self.arm_vehicle() self.disarm_vehicle() + self.set_parameters({ + "FENCE_TYPE": 6, # polyfence + circle + }) self.test_poly_fence_noarms_exclusion_circle(target_system=target_system, target_component=target_component) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 4677be0ab8..f3458fee9d 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2080,6 +2080,8 @@ class TestSuite(ABC): def load_fence(self, filename): filepath = os.path.join(testdir, self.current_test_name_directory, filename) + if not os.path.exists(filepath): + filepath = self.generic_mission_filepath_for_filename(filename) self.progress("Loading fence from (%s)" % str(filepath)) locs = [] for line in open(filepath, 'rb'): @@ -6994,22 +6996,17 @@ class TestSuite(ABC): self.mavproxy.expect("Loaded module relay") self.mavproxy.send("relay set %d %d\n" % (relay_num, on_off)) - def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - if value: - p1 = 1 - else: - p1 = 0 - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, - p1=p1, # param1 - want_result=want_result, - ) - def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(True, want_result=want_result) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, want_result=want_result) def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(False, want_result=want_result) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, want_result=want_result) + + def do_fence_disable_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, p2=8, want_result=want_result) + + def do_fence_enable_except_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, p2=7, want_result=want_result) ################################################# # WAIT UTILITIES