From ba66e1d276ba19671618bbacef0467ed757ae109 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 18 Jan 2025 14:12:04 -0600 Subject: [PATCH] ArduPlane:add options for minimum alt before turn to Autoland --- ArduPlane/altitude.cpp | 3 +++ ArduPlane/mode.h | 6 +++++ ArduPlane/mode_autoland.cpp | 46 +++++++++++++++++++++++++++++++++++-- 3 files changed, 53 insertions(+), 2 deletions(-) diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index e96e778b82fa5..263879d028727 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 c5cf09fb4988a..1a2bd7e42be1e 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -946,10 +946,15 @@ class ModeAutoLand: public Mode AP_Int16 final_wp_dist; AP_Int16 landing_dir_off; AP_Int8 options; + bool reached_altitude; + bool done_climb; // Bitfields of AUTOLAND_OPTIONS enum class AutoLandOption { AUTOLAND_DIR_ON_ARM = (1U << 0), // set dir for autoland on arm if compass in use. + AUTOLAND_CLIMB_LVL_ALT = (1U << 1), // climb straight to above LEVEL_ALT before first turn. + AUTOLAND_CLIMB_WP_ALT = (1U << 2), // climb straight to above final_wp_alt before first turn. + AUTOLAND_CLIMB_RTL_ALT = (1U << 3), // climb straight to above RTL_ALTITUDE before first turn. }; enum class AutoLandStage { @@ -968,6 +973,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 4727cc585015a..3f1f487e87432 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -38,7 +38,7 @@ 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), @@ -144,6 +144,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 +154,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 RTL alt as target if needed + if (autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_CLIMB_RTL_ALT)) { + plane.target_altitude.amsl_cm = plane.home.alt + plane.get_RTL_altitude_cm(); + } + } + // 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 +264,20 @@ void ModeAutoLand::arm_check(void) } } +// climb before turn options +void ModeAutoLand::check_altitude(void) +{ + const uint16_t altitude = plane.relative_ground_altitude(false,false); + if (autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_CLIMB_RTL_ALT) && altitude < (plane.g.RTL_altitude - 2)) { + return; + } else if (autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_CLIMB_WP_ALT) && altitude < (final_wp_alt -2)) { + return; + } else if (autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_CLIMB_LVL_ALT) && altitude < (plane.mode_takeoff.level_alt -2)) { + return; + } + reached_altitude = true; + return; +} + #endif // MODE_AUTOLAND_ENABLED