Plane: correct RTL_AUTOLAND=1 behaviour

ArduPlane/mode_rtl.cpp

@@ -95,6 +95,13 @@ void ModeRTL::navigate()
     }
 #endif
 
+    uint16_t radius = abs(plane.g.rtl_radius);
+    if (radius > 0) {
+        plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1;
+    }
+
+    plane.update_loiter(radius);
+
     if (!plane.auto_state.checked_for_autoland) {
         if ((plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) ||
             (plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START &&
@@ -116,13 +123,6 @@ void ModeRTL::navigate()
                 plane.auto_state.checked_for_autoland = true;
             }
     }
-
-    uint16_t radius = abs(plane.g.rtl_radius);
-    if (radius > 0) {
-        plane.loiter.direction = (plane.g.rtl_radius < 0) ? -1 : 1;
-    }
-
-    plane.update_loiter(radius);
 }
 
 #if HAL_QUADPLANE_ENABLED

Tools/autotest/quadplane.py

@@ -1662,6 +1662,115 @@ def DCMClimbRate(self):
         # Force disarm
         self.disarm_vehicle(force=True)
 
+    def RTL_AUTOLAND_1(self):
+        '''test behaviour when RTL_AUTOLAND==1'''
+
+        self.set_parameters({
+            "RTL_AUTOLAND": 1,
+        })
+
+        # when RTL is entered and RTL_AUTOLAND is 1 we should fly home
+        # then to the landing sequence.  This mission puts the landing
+        # sequence well to the West of home so if we go directly there
+        # we won't come within 200m of home
+        wps = self.create_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
+            # fly North
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30),
+            # add a waypoint 1km North (which we will look for and trigger RTL
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30),
+
+            # *exciting* landing sequence is ~1km West and points away from Home.
+            self.create_MISSION_ITEM_INT(
+                mavutil.mavlink.MAV_CMD_DO_LAND_START,
+            ),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5),
+            (mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0),
+        ])
+        self.check_mission_upload_download(wps)
+
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+
+        self.arm_vehicle()
+        self.wait_current_waypoint(3)  # will be 2km North here
+        self.change_mode('RTL')
+
+        self.wait_distance_to_home(100, 200, timeout=120)
+        self.wait_current_waypoint(7)
+
+        self.fly_home_land_and_disarm()
+
+    def send_reposition_to_loc(self, loc):
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_DO_REPOSITION,
+            0,
+            1,  # reposition flags; 1 means "change to guided"
+            0,
+            0,
+            int(loc.lat * 1e7),
+            int(loc.lng * 1e7),
+            20,    # alt
+            frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
+        )
+
+    def reposition_to_loc(self, loc, accuracy=100):
+        self.send_reposition_to_loc(loc)
+        self.wait_location(
+            loc,
+            accuracy=accuracy,
+            minimum_duration=20,
+            timeout=120,
+        )
+
+    def RTL_AUTOLAND_1_FROM_GUIDED(self):
+        '''test behaviour when RTL_AUTOLAND==1 and entering from guided'''
+
+        self.set_parameters({
+            "RTL_AUTOLAND": 1,
+        })
+
+        # when RTL is entered and RTL_AUTOLAND is 1 we should fly home
+        # then to the landing sequence.  This mission puts the landing
+        # sequence well to the West of home so if we go directly there
+        # we won't come within 200m of home
+        wps = self.create_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
+            # fly North
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, 30),
+            # add a waypoint 1km North (which we will look for and trigger RTL
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, 30),
+
+            # *exciting* landing sequence is ~1km West and points away from Home.
+            self.create_MISSION_ITEM_INT(
+                mavutil.mavlink.MAV_CMD_DO_LAND_START,
+            ),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1000, 30),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1300, 15),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, -1600, 5),
+            (mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, -1750, 0),
+        ])
+        self.check_mission_upload_download(wps)
+        self.set_current_waypoint(0, check_afterwards=False)
+
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+
+        here = self.mav.location()
+        guided_loc = self.offset_location_ne(here, 500, -500)
+
+        self.arm_vehicle()
+        self.wait_current_waypoint(3)  # will be 2km North here
+        self.reposition_to_loc(guided_loc)
+        self.send_cmd_do_set_mode('RTL')
+
+        self.wait_distance_to_home(100, 200, timeout=120)
+        self.wait_current_waypoint(7)
+
+        self.fly_home_land_and_disarm()
+
     def tests(self):
         '''return list of all tests'''
 
@@ -1706,5 +1815,7 @@ def tests(self):
             self.MAV_CMD_NAV_TAKEOFF,
             self.Q_GUIDED_MODE,
             self.DCMClimbRate,
+            self.RTL_AUTOLAND_1,  # as in fly-home then go to landing sequence
+            self.RTL_AUTOLAND_1_FROM_GUIDED,  # as in fly-home then go to landing sequence
         ])
         return ret