From 4fd568b83b31489b53200bcc7318eced5b137a19 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 18 Jan 2025 14:12:04 -0600 Subject: [PATCH] ArduPlane: add climb before turn to AUTOLAND --- ArduPlane/Parameters.cpp | 2 +- ArduPlane/Plane.h | 1 + ArduPlane/altitude.cpp | 6 ++++ ArduPlane/mode.h | 5 +++ ArduPlane/mode_autoland.cpp | 65 +++++++++++++++++++++++++++++++++++-- 5 files changed, 75 insertions(+), 4 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 061d3cb5ac6598..600ad52abc06e0 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -340,7 +340,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TERRAIN_FOLLOW // @DisplayName: Use terrain following // @Description: This enables terrain following for CRUISE mode, FBWB mode, RTL and for rally points. To use this option you also need to set TERRAIN_ENABLE to 1, which enables terrain data fetching from the GCS, and you need to have a GCS that supports sending terrain data to the aircraft. When terrain following is enabled then CRUISE and FBWB mode will hold height above terrain rather than height above home. In RTL the return to launch altitude will be considered to be a height above the terrain. Rally point altitudes will be taken as height above the terrain. This option does not affect mission items, which have a per-waypoint flag for whether they are height above home or height above the terrain. To use terrain following missions you need a ground station which can set the waypoint type to be a terrain height waypoint when creating the mission. - // @Bitmask: 0: Enable all modes, 1:FBWB, 2:Cruise, 3:Auto, 4:RTL, 5:Avoid_ADSB, 6:Guided, 7:Loiter, 8:Circle, 9:QRTL, 10:QLand, 11:Qloiter + // @Bitmask: 0: Enable all modes, 1:FBWB, 2:Cruise, 3:Auto, 4:RTL, 5:Avoid_ADSB, 6:Guided, 7:Loiter, 8:Circle, 9:QRTL, 10:QLand, 11:Qloiter, 12:AUTOLAND // @User: Standard GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1dda8aa741f40e..2c2ee6938a44aa 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -883,6 +883,7 @@ class Plane : public AP_Vehicle { QRTL = 1U << 9, QLAND = 1U << 10, QLOITER = 1U << 11, + AUTOLAND = 1U << 12, }; struct TerrainLookupTable{ Mode::Number mode_num; diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index e96e778b82fa50..a81dc1c3ac1f02 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: @@ -818,6 +821,9 @@ const Plane::TerrainLookupTable Plane::Terrain_lookup[] = { {Mode::Number::QLAND, terrain_bitmask::QLAND}, {Mode::Number::QLOITER, terrain_bitmask::QLOITER}, #endif +#if MODE_AUTOLAND_ENABLED + {Mode::Number::AUTOLAND, terrain_bitmask::AUTOLAND}, +#endif }; bool Plane::terrain_enabled_in_current_mode() const diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index c5cf09fb4988a2..e13d600e9447fe 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; + + 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..4d2d8c50a2d2db 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. // @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 TERRAIN_FOLLOW is enabled, 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; - +#if AP_TERRAIN_AVAILABLE + 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 + }; +#endif // land WP at home cmd_land.id = MAV_CMD_NAV_LAND; 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 +168,46 @@ bool ModeAutoLand::_enter() void ModeAutoLand::update() { plane.calc_nav_roll(); + + // check if initial climb before turn altitude has been reached +bool const 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; + } + 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