From 215901be30367b023d291240f7c4d0d71d020d6b Mon Sep 17 00:00:00 2001 From: James O'Shannessy <12959316+joshanne@users.noreply.github.com> Date: Fri, 18 Dec 2020 20:14:42 +1100 Subject: [PATCH] Autotest: Correct the logic for fence based autotest functions Adds corrections to enabling fence using aux function. Correctly test fences statically. Only uploaded fences can be checked using a fence file, so we check those first. Then we add steps to check tin can, max and minm all set the fence as present, as expected. Plane will support MAV_PROTOCOL_CAPABILITY_MISSION_FENCE, so we assert that it does support it. To test ceiling and floor, leverage some existing functions for takeoff, change altitude and land. Check for respective breach. Add a floor breach check to copter. --- .../FenceAltCeilFloor/CMAC-fence.txt | 6 - .../FenceAltCeilFloor/CMAC-mission.txt | 10 -- .../FenceAltCeilFloor/flaps.txt | 13 ++ Tools/autotest/arducopter.py | 48 +++++- Tools/autotest/arduplane.py | 143 ++++++++---------- 5 files changed, 122 insertions(+), 98 deletions(-) delete mode 100644 Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-fence.txt delete mode 100644 Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-mission.txt create mode 100644 Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/flaps.txt diff --git a/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-fence.txt b/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-fence.txt deleted file mode 100644 index a6169e4e3c..0000000000 --- a/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-fence.txt +++ /dev/null @@ -1,6 +0,0 @@ --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/ArduPlane_Tests/FenceAltCeilFloor/CMAC-mission.txt b/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-mission.txt deleted file mode 100644 index ec0225131c..0000000000 --- a/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-mission.txt +++ /dev/null @@ -1,10 +0,0 @@ -QGC WPL 110 -0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165086 585.090027 1 -1 0 3 22 15.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359628 149.164405 100.000000 1 -3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.360380 149.157558 60.000000 1 -4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364289 149.156969 100.000000 1 -5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.369329 149.158092 140.000000 1 -6 0 0 189 0.000000 0.000000 0.000000 0.000000 -35.369329 149.158092 0.000000 1 -7 0 3 31 0.000000 -75.000000 0.000000 1.000000 -35.367154 149.164864 100.000000 1 -8 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363200 149.165233 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/flaps.txt b/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/flaps.txt new file mode 100644 index 0000000000..1c38e7f211 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/flaps.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.358878 149.163760 80.000000 1 +6 0 0 177 2.000000 3.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.358867 149.164409 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359862 149.164697 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.360670 149.164857 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 40adcde0ae..eab76659bf 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1102,7 +1102,7 @@ class AutoTestCopter(AutoTest): def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_behave_slide): using_mode = "LOITER" # must be something which adjusts velocity! self.change_mode(using_mode) - self.set_parameter("FENCE_ENABLE", 1) # fence + self.set_parameter("FENCE_AUTOENABLE", 1) # fence self.set_parameter("FENCE_TYPE", 2) # circle fence_radius = 15 self.set_parameter("FENCE_RADIUS", fence_radius) @@ -1306,7 +1306,7 @@ class AutoTestCopter(AutoTest): """Hold loiter position.""" # enable fence, disable avoidance - self.set_parameter("FENCE_ENABLE", 1) + self.set_parameter("FENCE_AUTOENABLE", 1) self.set_parameter("AVOID_ENABLE", 0) self.set_parameter("FENCE_TYPE", 1) @@ -1335,6 +1335,42 @@ class AutoTestCopter(AutoTest): self.zero_throttle() + # fly_alt_fence_test - fly up until you hit the fence + def fly_alt_min_fence_test(self): + self.takeoff(50, mode="LOITER", timeout=120) + """Hold loiter position.""" + self.mavproxy.send('switch 5\n') # loiter mode + self.wait_mode('LOITER') + + # enable fence, disable avoidance + self.set_parameter("FENCE_AUTOENABLE", 1) + self.set_parameter("AVOID_ENABLE", 0) + self.set_parameter("FENCE_TYPE", 8) + + self.change_alt(10) + + # first east + self.progress("turn east") + self.set_rc(4, 1580) + self.wait_heading(160) + self.set_rc(4, 1500) + + # fly forward (east) at least 20m + self.set_rc(2, 1100) + self.wait_distance(20) + + # stop flying forward and start flying based on input: + self.set_rc(2, 1500) + self.set_rc(3, 1200) + + # wait for fence to trigger + self.wait_mode('RTL', timeout=120) + + self.wait_rtl_complete() + + self.zero_throttle() + + def fly_gps_glitch_loiter_test(self, timeout=30, max_distance=20): """fly_gps_glitch_loiter_test. Fly south east in loiter and test reaction to gps glitch.""" @@ -5032,7 +5068,7 @@ class AutoTestCopter(AutoTest): ex = None try: self.load_fence("copter-avoidance-fence.txt") - self.set_parameter("FENCE_ENABLE", 0) + self.set_parameter("FENCE_AUTOENABLE", 1) self.set_parameter("PRX_TYPE", 10) self.set_parameter("RC10_OPTION", 40) # proximity-enable self.reboot_sitl() @@ -5135,7 +5171,7 @@ class AutoTestCopter(AutoTest): ex = None try: self.load_fence("copter-avoidance-fence.txt") - self.set_parameter("FENCE_ENABLE", 1) + self.set_parameter("FENCE_AUTOENABLE", 1) self.check_avoidance_corners() except Exception as e: self.print_exception_caught(e) @@ -6479,6 +6515,10 @@ class AutoTestCopter(AutoTest): "Test Max Alt Fence", self.fly_alt_max_fence_test), # 26s + ("MinAltFence", + "Test Max Alt Fence", + self.fly_alt_min_fence_test), #26s + ("AutoTuneSwitch", "Fly AUTOTUNE on a switch", self.fly_autotune_switch), # 105s diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index f8c693de1b..58d476c457 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -969,16 +969,14 @@ class AutoTestPlane(AutoTest): self.set_parameter("RC7_OPTION", 11) # AC_Fence uses Aux switch functionality self.set_parameter("FENCE_ACTION", 4) # Fence action Brake - self.set_parameter("FENCE_ENABLE", 1) - - self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, - present=True, - enabled=False, - healthy=True) self.set_rc_from_map({ 3: 1000, 7: 2000, - }) + }) # Turn fence on with aux function + + + self.mavproxy.expect("fence enabled") + self.delay_sim_time(1) # This is needed else the SYS_STATUS may not have updated self.progress("Checking fence is initially OK") self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, @@ -1108,6 +1106,7 @@ class AutoTestPlane(AutoTest): ex = None try: self.progress("Checking for bizarre healthy-when-not-present-or-enabled") + self.set_parameter("FENCE_TYPE", 4) # Start by only setting polygon fences, otherwise fence will report present self.assert_fence_sys_status(False, False, True) self.load_fence("CMAC-fence.txt") m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) @@ -1149,6 +1148,19 @@ class AutoTestPlane(AutoTest): self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, False, False, True) if self.get_parameter("FENCE_TOTAL") != 0: raise NotAchievedException("Expected zero points remaining") + self.assert_fence_sys_status(False, False, True) + self.do_fence_disable() + + # ensure that a fence is present if it is tin can, min alt or max alt + self.progress("Test other fence types (tin-can, min alt, max alt") + self.set_parameter("FENCE_TYPE", 1) # max alt + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_TYPE", 8) # min alt + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_TYPE", 2) # tin can + self.assert_fence_sys_status(True, False, True) + + except Exception as e: self.print_exception_caught(e) @@ -1334,8 +1346,8 @@ class AutoTestPlane(AutoTest): self.change_mode('MANUAL') - self.progress("Asserting we don't support transfer of fence via mission item protocol") - self.assert_no_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) + self.progress("Asserting we do support transfer of fence via mission item protocol") + self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) # grab home position: self.mav.recv_match(type='HOME_POSITION', blocking=True) @@ -2333,76 +2345,51 @@ class AutoTestPlane(AutoTest): raise ex def test_fence_alt_ceil_floor(self): - ex = None - target_system = 1 - target_component = 1 - try: - self.progress("Testing Fence Enable") + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + self.set_parameter("FENCE_TYPE", 9) # Set fence type to max and min alt + self.set_parameter("FENCE_ACTION", 0) # Set action to report + self.set_parameter("FENCE_ALT_MAX", 200) + self.set_parameter("FENCE_ALT_MIN", 100) - # Load Fence - self.load_fence("CMAC-fence.txt") - if self.get_parameter("FENCE_TOTAL") == 0: - raise NotAchievedException("Expected fence to be present") - - # Load Mission - self.load_mission("CMAC-mission.txt") - - # Set up fence parameters - self.set_parameter("FENCE_AUTOENABLE", 1) # Enable/Disable on Takeoff/Landing - self.set_parameter("FENCE_ACTION", 1) # RTL - self.set_parameter("FENCE_TYPE", 13) # Set Fence Type to Alt Max/Min and Polygon - self.set_parameter("FENCE_ALT_MIN", 80) - self.set_parameter("FENCE_ALT_MAX", 120) - - # Now we can arm the vehicle and kick off the flight - self.wait_ready_to_arm() - self.arm_vehicle() - self.change_mode("AUTO") - - # On takeoff complete, check fence is enabled - self.progress("Looking for takeoff complete") - self.mavproxy.expect("APM: Takeoff complete at [0-9]+\.[0-9]+m") - #self.progress("Looking for fence enable") - #self.mavproxy.expect("fence enabled") - - # Fly until we breach fence floor where we enter RTL - self.progress("Looking for fence breach") - self.mavproxy.expect("fence breach") - - # As we RTL, we should re-enter fence zone. Wait until we're inside - self.progress("Waiting for return to altitude") - self.wait_altitude(altitude_min=100, altitude_max=110, timeout=30, relative=True) - - # Skip the waypoint outside fence, continue mission - self.progress("Skipping to next waypoint") - self.mav.waypoint_set_current_send(4) - self.change_mode("AUTO") - self.drain_mav() - - # Fly until we breach fence ceiling where we enter RTL - self.mavproxy.expect("fence breach") - - # As we RTL, we should re-enter fence zone. Wait until we're inside - self.wait_altitude(altitude_min=100, altitude_max=110, timeout=60, relative=True) - - # Skip the waypoint outside fence, continue mission - self.progress("Skipping to next waypoint") - self.mav.waypoint_set_current_send(6) - self.change_mode("AUTO") - self.drain_mav() - - # Continue through to landing sequence, which disables the fence - self.progress("Continuing mission to land") - self.mavproxy.expect("APM: Fence disabled \(auto disable\)") - - # Once landed, wait till disarmed - self.wait_disarmed(timeout=120) - except Exception as e: - self.progress("Exception caught:") - self.progress(self.get_exception_stacktrace(e)) - ex = e - if ex is not None: - raise ex + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + cruise_alt = 150 + self.takeoff(cruise_alt) + + self.do_fence_enable() + + self.progress("Fly above ceiling and check for breach") + self.change_altitude(self.homeloc.alt + cruise_alt + 80) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + if ((m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence Ceiling did not breach") + + self.progress("Return to cruise alt and check for breach clear") + self.change_altitude(self.homeloc.alt + cruise_alt) + + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence breach did not clear") + + + self.progress("Fly below floor and check for breach") + self.change_altitude(self.homeloc.alt + cruise_alt - 80) + + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + if ((m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence Floor did not breach") + + self.do_fence_disable() + + self.fly_home_land_and_disarm(timeout=150) + + + def tests(self): '''return list of all tests'''