diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index e13d600e9447fe..b8eec4829a6909 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -946,10 +946,7 @@ 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 { @@ -957,6 +954,7 @@ class ModeAutoLand: public Mode }; 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 diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index 9259012bb82ca0..1cf32cfe0c8e6a 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/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,9 +148,24 @@ 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 @@ -158,10 +173,15 @@ bool ModeAutoLand::_enter() 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; } @@ -169,32 +189,10 @@ 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