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..a412982f157de8 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -946,6 +946,9 @@ 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 +971,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..6bda9f6f53c27c 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/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., 1:Climb to TKOFF_LVL_ALT before turning upon mode entry., 2: Climb to AUTOLAND_WP_ALT before turning upon mode entry (overrides bit 1)., 3: Climb to RTL_ALTITUDE before turning upon mode entry (overrides bits 1 and 2). // @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. + // @Range: 0 500 + // @Increment: 1 + // @Units: m + // @User: Standard + AP_GROUPINFO("CLIMB", 5, ModeAutoLand, climb_before_turn_alt,0), + + AP_GROUPEND }; @@ -72,7 +82,7 @@ bool ModeAutoLand::_enter() gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set"); return false; } - + climb_before_turn_alt = (climb_before_turn_alt < final_wp_alt)? final_wp_alt : climb_before_turn_alt; plane.set_target_altitude_current(); plane.auto_state.next_wp_crosstrack = true; @@ -144,6 +154,8 @@ 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); return true; @@ -152,14 +164,39 @@ bool ModeAutoLand::_enter() void ModeAutoLand::update() { plane.calc_nav_roll(); + + // 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. + check_altitude(); + 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); + // set CLIMB before turn alt as target if needed + if (climb_before_turn_alt > final_wp_alt) { + plane.target_altitude.amsl_cm = plane.home.alt + climb_before_turn_alt * 100.0f; + } + } + // climb is done, use glide slope to loiter point. Once there loiter and land 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 +274,16 @@ void ModeAutoLand::arm_check(void) } } +// climb before turn altitude check +void ModeAutoLand::check_altitude(void) +{ + const uint16_t 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