ArduPlane: add climb before turn to AUTOLAND

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:

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;
+    uint16_t climb_to_alt_target;
+    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

@@ -10,7 +10,7 @@
 const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
     // @Param: WP_ALT
     // @DisplayName: Final approach WP altitude
-    // @Description: This is the target altitude above HOME for final approach waypoint
+    // @Description: This is the target altitude above TERRAIN, if available, or HOME, if not, for final approach waypoint
     // @Range: 0 200
     // @Increment: 1
     // @Units: m
@@ -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 a value less than AUTOLAND_WP_ALT is entered, AUTOLAND_WP_ALT will be used. If TERRAIN is enabled and healthy, 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
 };
 
@@ -114,7 +124,7 @@ bool ModeAutoLand::_enter()
      */
     land_start = home;
     land_start.offset_bearing(plane.takeoff_state.initial_direction.heading, -final_wp_dist);
-    land_start.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME);
+    land_start.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_TERRAIN);
     land_start.change_alt_frame(Location::AltFrame::ABSOLUTE);
 
     /*
@@ -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;
+    cmd_loiter.content.location.terrain_alt = 1;
 
     // land WP at home
     cmd_land.id = MAV_CMD_NAV_LAND;
     cmd_land.content.location = home;
 
+    //set climb before turn alt target
+    climb_to_alt_target = (climb_before_turn_alt < final_wp_alt)? final_wp_alt : climb_before_turn_alt;
+
     // 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
+    const float altitude = plane.relative_ground_altitude(false,true); //above terrain if enabled,otherwise above home
+    // climb to climb_before_turn alt: if terrian enabled its delta above terrain, otherwise above home 
+
+
+    // Constrain the roll limit if climb before turn options are set when below that altitude,
+    //that way if something goes wrong the plane will
+    // eventually turn back and go to landing waypoint instead of going perfectly straight. This also leaves
+    // some leeway for fighting wind.
+    uint16_t roll_limit_cd;
+    if (!done_climb) {
+        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);
+        plane.target_altitude.amsl_cm = plane.current_loc.alt + climb_to_alt_target * 100.0f;
+#if AP_TERRAIN_AVAILABLE
+        plane.target_altitude.terrain_alt_cm = plane.target_altitude.amsl_cm; //climb at TECS max until alt reached
+#endif
+        if (altitude > float(climb_to_alt_target - 2)) { // required to assure test is met as TECs approaches this altitude
+        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