diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index e96e778b82fa50..263879d0287276 100644 --- a/ArduPlane/altitude.cpp +++ b/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: diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index c5cf09fb4988a2..5b559e698e3094 100644 --- a/ArduPlane/mode.h +++ b/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 diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index 4727cc585015af..d2facf49dfedef 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/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 }; @@ -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