From 346bcdb869577e70ccf2c4450a9d210333eff707 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 18 Jan 2025 14:12:04 -0600 Subject: [PATCH 1/2] ArduPlane: add climb before turn to AUTOLAND Co-authored-by: IamPete1 --- ArduPlane/Parameters.cpp | 2 +- ArduPlane/Plane.h | 1 + ArduPlane/altitude.cpp | 6 ++++ ArduPlane/mode.h | 4 +++ ArduPlane/mode_autoland.cpp | 69 ++++++++++++++++++++++++++++++++++--- 5 files changed, 76 insertions(+), 6 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index f107918e55fc9..0f58953964b6f 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 86783bed8c151..8da5b047f1adf 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -875,6 +875,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 40219f26e6012..addd1db465f98 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: @@ -807,6 +810,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 c5cf09fb4988a..6a3c0dd2ac40b 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -946,6 +946,7 @@ class ModeAutoLand: public Mode AP_Int16 final_wp_dist; AP_Int16 landing_dir_off; AP_Int8 options; + AP_Int16 climb_min; // Bitfields of AUTOLAND_OPTIONS enum class AutoLandOption { @@ -953,6 +954,7 @@ class ModeAutoLand: public Mode }; enum class AutoLandStage { + CLIMB, LOITER, LANDING }; @@ -963,8 +965,10 @@ 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; + uint32_t entry_alt; Location land_start; AutoLandStage stage; void set_autoland_direction(const float heading); diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index 4727cc585015a..347ddfa06d812 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: Minimum climb before turning upon entry + // @Description: Vehicle will climb with limited turn ability (LEVEL_ROLL_LIMIT) upon mode entry by at least this amount, before proceeding to loiter-to-alt and landing legs. + // @Range: 0 100 + // @Increment: 1 + // @Units: m + // @User: Standard + AP_GROUPINFO("CLIMB", 5, ModeAutoLand, climb_min, 0), + + AP_GROUPEND }; @@ -139,21 +149,55 @@ bool ModeAutoLand::_enter() 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; + }; +#endif // land WP at home cmd_land.id = MAV_CMD_NAV_LAND; cmd_land.content.location = home; - + + entry_alt = plane.current_loc.alt; + // start first leg toward the base leg loiter to alt point - 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; } void ModeAutoLand::update() { plane.calc_nav_roll(); + + // 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(); - 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); @@ -165,6 +209,21 @@ void ModeAutoLand::update() 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); + + int32_t alt_diff; + alt_diff = plane.current_loc.alt - entry_alt; + if (plane.reached_loiter_target() || (alt_diff > climb_min * 100)) { + // 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)) { From ee70a67313bc779c28ce66b2f34c06f4f9a48b49 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Mon, 10 Feb 2025 14:51:24 -0600 Subject: [PATCH 2/2] Tools:enhance AUTOLAND autotest --- .../ArduPlane_Tests/AutoLandMode/autoland_mission.txt | 4 ++++ Tools/autotest/arduplane.py | 11 +++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) create mode 100644 Tools/autotest/ArduPlane_Tests/AutoLandMode/autoland_mission.txt diff --git a/Tools/autotest/ArduPlane_Tests/AutoLandMode/autoland_mission.txt b/Tools/autotest/ArduPlane_Tests/AutoLandMode/autoland_mission.txt new file mode 100644 index 0000000000000..0f1a47ef515b0 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/AutoLandMode/autoland_mission.txt @@ -0,0 +1,4 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165238 584.089966 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 1 +2 0 10 19 20.000000 0.000000 1.000000 0.000000 -35.342005 149.100266 100.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index a05ac4f33c907..915522ee11047 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4243,13 +4243,20 @@ def AutoLandMode(self): '''Test AUTOLAND mode''' self.set_parameters({ "AUTOLAND_DIR_OFF": 45, + "TERRAIN_FOLLOW": 1, + "AUTOLAND_CLIMB": 300, }) self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"]) self.context_collect('STATUSTEXT') - self.takeoff(alt=80, mode='TAKEOFF') + self.load_mission("autoland_mission.txt") + self.install_terrain_handlers_context() + self.change_mode("AUTO") + self.wait_ready_to_arm() + self.arm_vehicle() self.wait_text("Autoland direction", check_context=True) + self.wait_waypoint(2, 2, max_dist=100) self.change_mode(26) - self.wait_disarmed(120) + self.wait_disarmed(400) self.progress("Check the landed heading matches takeoff plus offset") self.wait_heading(218, accuracy=5, timeout=1) loc = mavutil.location(-35.362938, 149.165085, 585, 218)