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

@@ -883,6 +883,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:
@@ -818,6 +821,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,6 +946,10 @@ class ModeAutoLand: public Mode
     AP_Int16 final_wp_dist;
     AP_Int16 landing_dir_off;
     AP_Int8  options;
+    AP_Int16 climb_before_turn_alt;
+
+    bool reached_altitude;
+    bool done_climb;
 
     // Bitfields of AUTOLAND_OPTIONS
     enum class AutoLandOption {
@@ -968,6 +972,7 @@ class ModeAutoLand: public Mode
     Location land_start;
     AutoLandStage stage;
     void set_autoland_direction(const float heading);
+    void check_altitude (void);
 };
 #endif
 #if HAL_SOARING_ENABLED

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: Climb before turning altitude
+    // @Description: Vehicle will climb with limited turn ability (LEVEL_ROLL_LIMIT) upon mode entry to this altitude, before proceeding to loiter-to-alt and landing legs. If TERRAIN_FOLLOW is enabled, the altitude is above terrain, otherwise above HOME.
+    // @Range: 0 500
+    // @Increment: 1
+    // @Units: m
+    // @User: Standard
+    AP_GROUPINFO("CLIMB", 5, ModeAutoLand, climb_before_turn_alt,0),
+
+
     AP_GROUPEND
 };
 
@@ -138,12 +148,18 @@ bool ModeAutoLand::_enter()
     cmd_loiter.content.location = land_start;
     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;
-
+#if AP_TERRAIN_AVAILABLE
+    if (plane.terrain_enabled_in_current_mode()) {
+        cmd_loiter.content.location.terrain_alt = 1; // this allows terrain following to it as a glide slop destination
+    };
+#endif
     // land WP at home
     cmd_land.id = MAV_CMD_NAV_LAND;
     cmd_land.content.location = home;
 
     // start first leg toward the base leg loiter to alt point
+    reached_altitude = false;
+    done_climb = false;
     stage = AutoLandStage::LOITER;
     plane.start_command(cmd_loiter);
     return true;
@@ -152,14 +168,46 @@ bool ModeAutoLand::_enter()
 void ModeAutoLand::update()
 {
     plane.calc_nav_roll();
+
+    // check if initial climb before turn altitude has been reached
+bool const terrain_following_active = false;
+#if AP_TERRAIN_AVAILABLE
+    terrain_following_active = plane.terrain_enabled_in_current_mode();
+#endif
+    const float altitude = plane.relative_ground_altitude(false,terrain_following_active); //above terrain if enabled,otherwise above home
+    uint16_t roll_limit_cd;
+    if (!done_climb) {
+        // Constrain the roll limit during climb before turn. This also leaves
+        // some leeway for fighting wind.
+        roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
+        plane.nav_roll_cd = constrain_int16(plane.nav_roll_cd, -roll_limit_cd, roll_limit_cd);
+        //climb at TECS max until alt reached
+        plane.target_altitude.amsl_cm = plane.current_loc.alt + climb_before_turn_alt * 100.0f;
+#if AP_TERRAIN_AVAILABLE
+        plane.target_altitude.terrain_alt_cm = plane.target_altitude.amsl_cm;
+#endif
+        if (altitude > float(climb_before_turn_alt)) {
+        reached_altitude = true;
+        }
+    }
+    // if climb is done, setup glide slope once to loiter point. Once there, loiter and land will control altitude.
+    if (!done_climb && reached_altitude) {
+        plane.prev_WP_loc = plane.current_loc;
+        plane.setup_glide_slope();
+        done_climb = true;
+    }
+
     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);
     } else {
         plane.calc_throttle();
     }
+
+
 }
 
 void ModeAutoLand::navigate()
@@ -237,5 +285,16 @@ void ModeAutoLand::arm_check(void)
     }
 }
 
+// climb before turn altitude check
+void ModeAutoLand::check_altitude(void)
+{
+    const float altitude = plane.relative_ground_altitude(false,false);
+    if (altitude < (climb_before_turn_alt - 2)) { // required to assure test is met as TECs approaches this altitude
+        return;
+    }
+    reached_altitude = true;
+    return;
+}
+
 #endif // MODE_AUTOLAND_ENABLED