ArduPlane: add climb before turn to AUTOLAND

ArduPlane/Parameters.cpp

@@ -340,7 +340,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @Param: TERRAIN_FOLLOW
     // @DisplayName: Use terrain following
     // @Description: This enables terrain following for CRUISE mode, FBWB mode, RTL and for rally points. To use this option you also need to set TERRAIN_ENABLE to 1, which enables terrain data fetching from the GCS, and you need to have a GCS that supports sending terrain data to the aircraft. When terrain following is enabled then CRUISE and FBWB mode will hold height above terrain rather than height above home. In RTL the return to launch altitude will be considered to be a height above the terrain. Rally point altitudes will be taken as height above the terrain. This option does not affect mission items, which have a per-waypoint flag for whether they are height above home or height above the terrain. To use terrain following missions you need a ground station which can set the waypoint type to be a terrain height waypoint when creating the mission.
-    // @Bitmask: 0: Enable all modes, 1:FBWB, 2:Cruise, 3:Auto, 4:RTL, 5:Avoid_ADSB, 6:Guided, 7:Loiter, 8:Circle, 9:QRTL, 10:QLand, 11:Qloiter
+    // @Bitmask: 0: Enable all modes, 1:FBWB, 2:Cruise, 3:Auto, 4:RTL, 5:Avoid_ADSB, 6:Guided, 7:Loiter, 8:Circle, 9:QRTL, 10:QLand, 11:Qloiter, 12:AUTOLAND
     // @User: Standard
     GSCALAR(terrain_follow, "TERRAIN_FOLLOW",  0),
 

ArduPlane/Plane.h

@@ -875,6 +875,7 @@ class Plane : public AP_Vehicle {
         QRTL            = 1U << 9,
         QLAND           = 1U << 10,
         QLOITER         = 1U << 11,
+        AUTOLAND        = 1U << 12,
     };
     struct TerrainLookupTable{
        Mode::Number mode_num;

ArduPlane/altitude.cpp

@@ -66,6 +66,9 @@ void Plane::setup_glide_slope(void)
       the new altitude as quickly as possible.
      */
     switch (control_mode->mode_number()) {
+#if MODE_AUTOLAND_ENABLED
+    case Mode::Number::AUTOLAND:
+#endif
     case Mode::Number::RTL:
     case Mode::Number::AVOID_ADSB:
     case Mode::Number::GUIDED:
@@ -807,6 +810,9 @@ const Plane::TerrainLookupTable Plane::Terrain_lookup[] = {
     {Mode::Number::QLAND, terrain_bitmask::QLAND},
     {Mode::Number::QLOITER, terrain_bitmask::QLOITER},
 #endif
+#if MODE_AUTOLAND_ENABLED
+    {Mode::Number::AUTOLAND, terrain_bitmask::AUTOLAND},
+#endif
 };
 
 bool Plane::terrain_enabled_in_current_mode() const

ArduPlane/mode.h

@@ -946,13 +946,15 @@ class ModeAutoLand: public Mode
     AP_Int16 final_wp_dist;
     AP_Int16 landing_dir_off;
     AP_Int8  options;
+    AP_Int16 climb_min;
 
     // Bitfields of AUTOLAND_OPTIONS
     enum class AutoLandOption {
         AUTOLAND_DIR_ON_ARM     = (1U << 0), // set dir for autoland on arm if compass in use.
     };
 
     enum class AutoLandStage {
+        CLIMB,
         LOITER,
         LANDING
     };
@@ -963,8 +965,10 @@ class ModeAutoLand: public Mode
 
 protected:
     bool _enter() override;
+    AP_Mission::Mission_Command cmd_climb;
     AP_Mission::Mission_Command cmd_loiter;
     AP_Mission::Mission_Command cmd_land;
+    uint32_t entry_alt;
     Location land_start;
     AutoLandStage stage;
     void set_autoland_direction(const float heading);

ArduPlane/mode_autoland.cpp

@@ -38,10 +38,20 @@ const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Autoland mode options
     // @Description: Enables optional autoland mode behaviors
-    // @Bitmask: 1: When set if there is a healthy compass in use the compass heading will be captured at arming and used for the AUTOLAND mode's initial takeoff direction instead of capturing ground course in NAV_TAKEOFF or Mode TAKEOFF or other modes.
+    // @Bitmask: 0: When set if there is a healthy compass in use the compass heading will be captured at arming and used for the AUTOLAND mode's initial takeoff direction instead of capturing ground course in NAV_TAKEOFF or Mode TAKEOFF or other modes.
     // @User: Standard
     AP_GROUPINFO("OPTIONS", 4, ModeAutoLand, options, 0),
 
+    // @Param: CLIMB
+    // @DisplayName: Minimum climb before turning upon entry
+    // @Description: Vehicle will climb with limited turn ability (LEVEL_ROLL_LIMIT) upon mode entry by at least this amount, before proceeding to loiter-to-alt and landing legs.
+    // @Range: 0 100
+    // @Increment: 1
+    // @Units: m
+    // @User: Standard
+    AP_GROUPINFO("CLIMB", 5, ModeAutoLand, climb_min, 0),
+
+
     AP_GROUPEND
 };
 
@@ -139,21 +149,55 @@ bool ModeAutoLand::_enter()
     cmd_loiter.content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, corrected_loiter_radius);
     cmd_loiter.content.location.loiter_ccw = bearing_err_deg>0? 1 :0;
 
+    // May need to climb first
+    bool climb_first = false;
+    if (climb_min > 0) {
+        // Copy loiter and update target altitude to current altitude plus climb altitude
+        cmd_climb = cmd_loiter;
+        float abs_alt;
+        if (plane.current_loc.get_alt_m(Location::AltFrame::ABSOLUTE, abs_alt)) {
+            // Add 10m to ensure full rate climb past target altitude
+            cmd_climb.content.location.set_alt_m(abs_alt + climb_min + 10, Location::AltFrame::ABSOLUTE);
+            climb_first = true;
+        }
+    }
+
+#if AP_TERRAIN_AVAILABLE
+    // Update loiter location to be relative terrain if enabled
+    if (plane.terrain_enabled_in_current_mode()) {
+        cmd_loiter.content.location.terrain_alt = 1;
+    };
+#endif
     // land WP at home
     cmd_land.id = MAV_CMD_NAV_LAND;
     cmd_land.content.location = home;
-
+
+    entry_alt = plane.current_loc.alt;
+
     // start first leg toward the base leg loiter to alt point
-    stage = AutoLandStage::LOITER;
-    plane.start_command(cmd_loiter);
+    if (climb_first) {
+        stage = AutoLandStage::CLIMB;
+        plane.start_command(cmd_climb);
+
+    } else {
+        stage = AutoLandStage::LOITER;
+        plane.start_command(cmd_loiter);
+    }
+
     return true;
 }
 
 void ModeAutoLand::update()
 {
     plane.calc_nav_roll();
+
+    // Apply level roll limit in climb stage
+    if (stage == AutoLandStage::CLIMB) {
+        plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
+        plane.nav_roll_cd = constrain_int16(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd);
+    }
+
     plane.calc_nav_pitch();
-    plane.set_offset_altitude_location(plane.prev_WP_loc, plane.next_WP_loc);
     if (plane.landing.is_throttle_suppressed()) {
         // if landing is considered complete throttle is never allowed, regardless of landing type
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
@@ -165,6 +209,21 @@ void ModeAutoLand::update()
 void ModeAutoLand::navigate()
 {
     switch (stage) {
+    case AutoLandStage::CLIMB:
+        // Update loiter, although roll limit is applied the vehicle will still navigate (slowly)
+        plane.update_loiter(cmd_climb.p1);
+
+        int32_t alt_diff;
+        alt_diff = plane.current_loc.alt - entry_alt;
+        if (plane.reached_loiter_target() || (alt_diff > climb_min * 100)) {
+            // Reached destination or cant get alt or Climb is done, move onto loiter
+            plane.auto_state.next_wp_crosstrack = true;
+            stage = AutoLandStage::LOITER;
+            plane.start_command(cmd_loiter);
+            plane.prev_WP_loc = plane.current_loc;
+        }
+        break;
+
     case AutoLandStage::LOITER:
         // check if we have arrived and completed loiter at base leg waypoint
         if (plane.verify_loiter_to_alt(cmd_loiter)) {

Tools/autotest/ArduPlane_Tests/AutoLandMode/autoland_mission.txt

@@ -0,0 +1,4 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165238	584.089966	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	0.000000	0.000000	100.000000	1
+2	0	10	19	20.000000	0.000000	1.000000	0.000000	-35.342005	149.100266	100.000000	1

Tools/autotest/arduplane.py

@@ -4243,13 +4243,20 @@ def AutoLandMode(self):
         '''Test AUTOLAND mode'''
         self.set_parameters({
             "AUTOLAND_DIR_OFF": 45,
+            "TERRAIN_FOLLOW": 1,
+            "AUTOLAND_CLIMB": 300,
         })
         self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"])
         self.context_collect('STATUSTEXT')
-        self.takeoff(alt=80, mode='TAKEOFF')
+        self.load_mission("autoland_mission.txt")
+        self.install_terrain_handlers_context()
+        self.change_mode("AUTO")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
         self.wait_text("Autoland direction", check_context=True)
+        self.wait_waypoint(2, 2, max_dist=100)
         self.change_mode(26)
-        self.wait_disarmed(120)
+        self.wait_disarmed(400)
         self.progress("Check the landed heading matches takeoff plus offset")
         self.wait_heading(218, accuracy=5, timeout=1)
         loc = mavutil.location(-35.362938, 149.165085, 585, 218)