Plane: autoland: rework climb before turn

ArduPlane/mode.h

@@ -946,17 +946,15 @@ 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;
+    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
     };
@@ -967,12 +965,12 @@ 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;
     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

@@ -43,13 +43,13 @@ const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
     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
+    // @DisplayName: Minimum climb before turning altitude
+    // @Description: Vehicle will climb with limited turn ability (LEVEL_ROLL_LIMIT) upon mode entry by at least this altitude, before proceeding to loiter-to-alt and landing legs.
+    // @Range: 0 100
     // @Increment: 1
     // @Units: m
     // @User: Standard
-    AP_GROUPINFO("CLIMB", 5, ModeAutoLand, climb_before_turn_alt,0),
+    AP_GROUPINFO("CLIMB", 5, ModeAutoLand, climb_min, 0),
 
 
     AP_GROUPEND
@@ -148,53 +148,51 @@ 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;
+
+    // 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; // this allows terrain following to it as a glide slop destination
+        cmd_loiter.content.location.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN);
     };
 #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);
+    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();
 
-    // check if initial climb before turn altitude has been reached
-bool 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;
+    // 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();
@@ -213,6 +211,20 @@ bool terrain_following_active = false;
 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);
+
+        ftype alt_diff;
+        if (plane.reached_loiter_target() || !plane.current_loc.get_alt_distance(plane.prev_WP_loc, alt_diff) || (alt_diff > climb_min)) {
+            // 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)) {
@@ -285,16 +297,5 @@ 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