AutotTest: Adds autotests for Plane to test RetRally flag added to AC_Fence

Tools/autotest/ArduPlane_Tests/FenceRetRally/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.390015	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.361027	149.164093	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.363136	149.162750	60.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365467	149.164215	55.000000	1
+10	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365009	149.165482	39.889999	1
+11	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.363041	149.165222	0.000000	1

Tools/autotest/arduplane.py

@@ -1296,6 +1296,85 @@ class AutoTestPlane(AutoTest):
         if ex is not None:
             raise ex
 
+    def test_fence_ret_rally(self):
+        """ Tests the FENCE_RET_RALLY flag, either returning to fence return point,
+            or rally point """
+        target_system = 1
+        target_component = 1
+        self.progress("Testing FENCE_ACTION_RTL with fence rally point")
+
+        self.wait_ready_to_arm()
+        self.homeloc = self.mav.location()
+
+        # Grab a location for fence return point, and upload it.
+        fence_loc = self.home_position_as_mav_location()
+        self.location_offset_ne(fence_loc, 50, 50)
+        fence_return_mission_items = [
+            self.mav.mav.mission_item_int_encode(
+                target_system,
+                target_component,
+                0, # seq
+                mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
+                mavutil.mavlink.MAV_CMD_NAV_FENCE_RETURN_POINT,
+                0, # current
+                0, # autocontinue
+                0, # p1
+                0, # p2
+                0, # p3
+                0, # p4
+                int(fence_loc.lat *1e7), # latitude
+                int(fence_loc.lng *1e7), # longitude
+                0, # altitude
+                mavutil.mavlink.MAV_MISSION_TYPE_FENCE
+            )
+        ]
+        self.upload_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
+                                            fence_return_mission_items)
+        self.delay_sim_time(1)
+
+        # Grab a location for rally point, and upload it.
+        rally_loc = self.home_position_as_mav_location()
+        self.location_offset_ne(rally_loc, -50, -50)
+        self.set_parameter("RALLY_TOTAL", 1)
+        self.mav.mav.rally_point_send(target_system,
+                                        target_component,
+                                        0, # sequence number
+                                        1, # total count
+                                        int(rally_loc.lat * 1e7),
+                                        int(rally_loc.lng * 1e7),
+                                        15,
+                                        0, # "break" alt?!
+                                        0, # "land dir"
+                                        0) # flags
+        self.delay_sim_time(1)
+
+        return_radius = 100
+        return_alt = 80
+        self.set_parameter("RTL_RADIUS", return_radius)
+        self.set_parameter("FENCE_ACTION", 5) # Set Fence Action to Guided
+        self.set_parameter("FENCE_TYPE", 8)   # Only use fence floor
+        self.set_parameter("FENCE_RET_ALT", return_alt)
+        self.do_fence_enable()
+        self.assert_fence_enabled()
+        
+        self.takeoff(alt=50, alt_max=300)
+        # Trigger fence breach, fly to rally location
+        self.set_parameter("FENCE_RET_RALLY", 1)
+        self.set_parameter("FENCE_ALT_MIN", 60)
+        self.wait_circling_point_with_radius(rally_loc, return_radius)
+        self.set_parameter("FENCE_ALT_MIN", 0) # Clear fence breach
+
+        # Fly up before re-triggering fence breach. Fly to fence return point
+        self.change_altitude(self.homeloc.alt+30)
+        self.set_parameter("FENCE_RET_RALLY", 0)
+        self.set_parameter("FENCE_ALT_MIN", 60)
+        self.wait_altitude(altitude_min=return_alt-3,
+                        altitude_max=return_alt+3,
+                        relative=True)
+        self.wait_circling_point_with_radius(fence_loc, return_radius)
+        self.do_fence_disable() # Disable fence so we can land
+        self.fly_home_land_and_disarm() # Pack it up, we're going home.
+
     def test_parachute(self):
         self.set_rc(9, 1000)
         self.set_parameter("CHUTE_ENABLED", 1)
@@ -2517,6 +2596,10 @@ class AutoTestPlane(AutoTest):
              "Test Fence RTL Rally",
              self.test_fence_rtl_rally),
 
+            ("FenceRetRally",
+             "Test Fence Ret_Rally",
+             self.test_fence_ret_rally),
+
             ("FenceAltCeilFloor",
              "Tests the fence ceiling and floor",
              self.test_fence_alt_ceil_floor),