From dc584a8a64194edb87f1c9a2738f4bc01e69a231 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 Nov 2024 15:39:37 +1100 Subject: [PATCH 1/7] autotest: added non-compass takeoff test autotest:add test for autoland mode --- Tools/autotest/arduplane.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2aa1390402db4..cf94ae2c875c0 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4239,6 +4239,20 @@ def FlyEachFrame(self): self.fly_mission(mission_file, strict=False, quadplane=quadplane, mission_timeout=400.0) self.wait_disarmed() + def AutoLandMode(self): + '''Test AUTOLAND mode''' + self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"]) + self.context_collect('STATUSTEXT') + self.takeoff(alt=80, mode='TAKEOFF') + self.wait_text("Autoland direction", check_context=True) + self.change_mode(26) + self.wait_disarmed(120) + self.progress("Check the landed heading matches takeoff") + self.wait_heading(173, accuracy=5, timeout=1) + loc = mavutil.location(-35.362938, 149.165085, 585, 173) + if self.get_distance(loc, self.mav.location()) > 35: + raise NotAchievedException("Did not land close to home") + def RCDisableAirspeedUse(self): '''Test RC DisableAirspeedUse option''' self.set_parameter("RC9_OPTION", 106) @@ -6607,6 +6621,7 @@ def tests1b(self): self.MAV_CMD_DO_AUX_FUNCTION, self.SmartBattery, self.FlyEachFrame, + self.AutoLandMode, self.RCDisableAirspeedUse, self.AHRS_ORIENTATION, self.AHRSTrim, From 81a34357dcc281892912b5d96e7efc32367ffdc4 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Thu, 28 Nov 2024 20:40:29 -0600 Subject: [PATCH 2/7] AP_Vehicle: add AutoLand fixed-wing mode --- libraries/AP_Vehicle/ModeReason.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Vehicle/ModeReason.h b/libraries/AP_Vehicle/ModeReason.h index a1936d8e455bf..0c31652780dc5 100644 --- a/libraries/AP_Vehicle/ModeReason.h +++ b/libraries/AP_Vehicle/ModeReason.h @@ -71,4 +71,5 @@ enum class ModeReason : uint8_t { MODE_TAKEOFF_FAILSAFE = 51, DDS_COMMAND = 52, AUX_FUNCTION = 53, + FIXED_WING_AUTOLAND = 54, }; From 45fa1d2c7129802c996208c06717ff88f8b9ab14 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sun, 22 Dec 2024 13:39:44 -0600 Subject: [PATCH 3/7] AP_Landing: add AutoLand fixed-wing mode --- libraries/AP_Landing/AP_Landing.cpp | 12 ++++++++++++ libraries/AP_Landing/AP_Landing.h | 2 ++ 2 files changed, 14 insertions(+) diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 16cb32630e30b..ac080c52dbdfd 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -463,6 +463,18 @@ void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Lo } } +/* + reset landing state + */ +void AP_Landing::reset(void) +{ + initial_slope = 0; + slope = 0; + type_slope_flags.post_stats = false; + type_slope_flags.has_aborted_due_to_slope_recalc = false; + type_slope_stage = SlopeStage::NORMAL; +} + /* Restart a landing by first checking for a DO_LAND_START and jump there. Otherwise decrement waypoint so we would re-start diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 80349010bc9da..bd85696c961fc 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -66,6 +66,8 @@ class AP_Landing { void convert_parameters(void); + void reset(void); + void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); bool verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed); From d0de2ebe3730d6bfd5c773e2d0e8399cb46f24a2 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sun, 22 Dec 2024 13:39:44 -0600 Subject: [PATCH 4/7] ArduPlane: add AutoLand fixed-wing mode --- ArduPlane/AP_Arming.cpp | 5 +- ArduPlane/GCS_MAVLink_Plane.cpp | 6 ++ ArduPlane/GCS_Plane.cpp | 3 + ArduPlane/Parameters.cpp | 6 ++ ArduPlane/Parameters.h | 2 + ArduPlane/Plane.cpp | 6 +- ArduPlane/Plane.h | 14 +++- ArduPlane/commands_logic.cpp | 10 ++- ArduPlane/control_modes.cpp | 5 ++ ArduPlane/defines.h | 3 + ArduPlane/events.cpp | 26 ++++++- ArduPlane/mode.cpp | 4 ++ ArduPlane/mode.h | 41 +++++++++++ ArduPlane/mode_autoland.cpp | 118 ++++++++++++++++++++++++++++++++ ArduPlane/mode_takeoff.cpp | 14 +++- ArduPlane/quadplane.h | 3 +- ArduPlane/takeoff.cpp | 17 +++-- 17 files changed, 269 insertions(+), 14 deletions(-) create mode 100644 ArduPlane/mode_autoland.cpp diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index fca35f8d088fe..25a43be25cab3 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -378,8 +378,11 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec // DO_CHANGE_SPEED commands plane.new_airspeed_cm = -1; +#if MODE_AUTOLAND_ENABLED + // takeoff direction always cleared on disarm + plane.takeoff_state.initial_direction.initialized = false; +#endif send_arm_disarm_statustext("Throttle disarmed"); - return true; } diff --git a/ArduPlane/GCS_MAVLink_Plane.cpp b/ArduPlane/GCS_MAVLink_Plane.cpp index 17b723fdc511b..3ae02911466d5 100644 --- a/ArduPlane/GCS_MAVLink_Plane.cpp +++ b/ArduPlane/GCS_MAVLink_Plane.cpp @@ -59,6 +59,9 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case Mode::Number::GUIDED: case Mode::Number::CIRCLE: case Mode::Number::TAKEOFF: +#if MODE_AUTOLAND_ENABLED + case Mode::Number::AUTOLAND: +#endif #if HAL_QUADPLANE_ENABLED case Mode::Number::QRTL: case Mode::Number::LOITER_ALT_QLAND: @@ -1588,6 +1591,9 @@ uint8_t GCS_MAVLINK_Plane::send_available_mode(uint8_t index) const &plane.mode_takeoff, #if HAL_SOARING_ENABLED &plane.mode_thermal, +#endif +#if MODE_AUTOLAND_ENABLED + &plane.mode_autoland, #endif }; diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index a193f44e0858d..6b76d734cb3d7 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -70,6 +70,9 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) case Mode::Number::GUIDED: case Mode::Number::CIRCLE: case Mode::Number::TAKEOFF: +#if MODE_AUTOLAND_ENABLED + case Mode::Number::AUTOLAND: +#endif #if HAL_QUADPLANE_ENABLED case Mode::Number::QRTL: case Mode::Number::LOITER_ALT_QLAND: diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index df62bcd653714..6e6991fd54b01 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1028,6 +1028,12 @@ const AP_Param::Info Plane::var_info[] = { // @Path: mode_takeoff.cpp GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff), +#if MODE_AUTOLAND_ENABLED + // @Group: AUTOLAND_ + // @Path: mode_autoland.cpp + GOBJECT(mode_autoland, "AUTOLAND_", ModeAutoLand), +#endif + #if AP_PLANE_GLIDER_PULLUP_ENABLED // @Group: PUP_ // @Path: pullup.cpp diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index e7cc5326f4280..9945869250b4b 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -363,6 +363,8 @@ class Parameters { k_param_pullup = 270, k_param_quicktune, + k_param_mode_autoland, + }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index 1990ca0ed2e78..2fb6cb1eebdc5 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -690,7 +690,11 @@ void Plane::update_flight_stage(void) } #endif set_flight_stage(AP_FixedWing::FlightStage::NORMAL); - } else if (control_mode != &mode_takeoff) { + } else if ((control_mode != &mode_takeoff) +#if MODE_AUTOLAND_ENABLED + && (control_mode != &mode_autoland) +#endif + ) { // If not in AUTO then assume normal operation for normal TECS operation. // This prevents TECS from being stuck in the wrong stage if you switch from // AUTO to, say, FBWB during a landing, an aborted landing or takeoff. diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 88ade0d53465f..b272a13b11ff1 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -172,7 +172,9 @@ class Plane : public AP_Vehicle { friend class ModeTakeoff; friend class ModeThermal; friend class ModeLoiterAltQLand; - +#if MODE_AUTOLAND_ENABLED + friend class ModeAutoLand; +#endif #if AP_EXTERNAL_CONTROL_ENABLED friend class AP_ExternalControl_Plane; #endif @@ -326,6 +328,9 @@ class Plane : public AP_Vehicle { #endif // QAUTOTUNE_ENABLED #endif // HAL_QUADPLANE_ENABLED ModeTakeoff mode_takeoff; +#if MODE_AUTOLAND_ENABLED + ModeAutoLand mode_autoland; +#endif #if HAL_SOARING_ENABLED ModeThermal mode_thermal; #endif @@ -454,6 +459,13 @@ class Plane : public AP_Vehicle { float throttle_lim_min; uint32_t throttle_max_timer_ms; uint32_t level_off_start_time_ms; + // Good candidate for keeping the initial time for TKOFF_THR_MAX_T. +#if MODE_AUTOLAND_ENABLED + struct { + float heading; // deg + bool initialized; + } initial_direction; +#endif } takeoff_state; // ground steering controller state diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 624dc2749c597..af99693635b98 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -382,6 +382,9 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) // zero locked course steer_state.locked_course_err = 0; steer_state.hold_course_cd = -1; +#if MODE_AUTOLAND_ENABLED + takeoff_state.initial_direction.initialized = false; +#endif auto_state.baro_takeoff_alt = barometer.get_altitude(); } @@ -550,12 +553,17 @@ bool Plane::verify_takeoff() trust_ahrs_yaw |= ahrs.dcm_yaw_initialised(); #endif if (trust_ahrs_yaw && steer_state.hold_course_cd == -1) { - const float min_gps_speed = 5; + const float min_gps_speed = GPS_GND_CRS_MIN_SPD; if (auto_state.takeoff_speed_time_ms == 0 && gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.ground_speed() > min_gps_speed && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { auto_state.takeoff_speed_time_ms = millis(); +#if MODE_AUTOLAND_ENABLED + plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off); + takeoff_state.initial_direction.initialized = true; + gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.initial_direction.heading)); +#endif } if (auto_state.takeoff_speed_time_ms != 0 && millis() - auto_state.takeoff_speed_time_ms >= 2000) { diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 7f15e0e2663f9..5d07b45534e2b 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -83,6 +83,11 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::TAKEOFF: ret = &mode_takeoff; break; +#if MODE_AUTOLAND_ENABLED + case Mode::Number::AUTOLAND: + ret = &mode_autoland; + break; +#endif //MODE_AUTOLAND_ENABLED case Mode::Number::THERMAL: #if HAL_SOARING_ENABLED ret = &mode_thermal; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 561e1deb39285..b0941e85badc5 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -10,6 +10,8 @@ #define TAKEOFF_RUDDER_WARNING_TIMEOUT 3000 //ms that GCS warning about not returning arming rudder to neutral repeats +#define GPS_GND_CRS_MIN_SPD 5 // m/s, used to set when intial_direction.heading is captured in NAV_TAKEOFF and Mode TAKEOFF + // failsafe // ---------------------- enum failsafe_state { @@ -45,6 +47,7 @@ enum failsafe_action_long { FS_ACTION_LONG_GLIDE = 2, FS_ACTION_LONG_PARACHUTE = 3, FS_ACTION_LONG_AUTO = 4, + FS_ACTION_LONG_AUTOLAND = 5, }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 64127c633c181..d5ab51c603f5b 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -65,7 +65,11 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso break; #endif // HAL_QUADPLANE_ENABLED - case Mode::Number::AUTO: { + case Mode::Number::AUTO: +#if MODE_AUTOLAND_ENABLED + case Mode::Number::AUTOLAND: +#endif + { if (failsafe_in_landing_sequence()) { // don't failsafe in a landing sequence break; @@ -145,6 +149,12 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason set_mode(mode_fbwa, reason); } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { set_mode(mode_auto, reason); +#if MODE_AUTOLAND_ENABLED + } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) { + if (!set_mode(mode_autoland, reason)) { + set_mode(mode_rtl, reason); + } +#endif } else { set_mode(mode_rtl, reason); } @@ -194,14 +204,23 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason set_mode(mode_fbwa, reason); } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) { set_mode(mode_auto, reason); +#if MODE_AUTOLAND_ENABLED + } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) { + if (!set_mode(mode_autoland, reason)) { + set_mode(mode_rtl, reason); + } +#endif } else if (g.fs_action_long == FS_ACTION_LONG_RTL) { set_mode(mode_rtl, reason); } break; - case Mode::Number::RTL: if (g.fs_action_long == FS_ACTION_LONG_AUTO) { set_mode(mode_auto, reason); +#if MODE_AUTOLAND_ENABLED + } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) { + set_mode(mode_autoland, reason); +#endif } break; #if HAL_QUADPLANE_ENABLED @@ -210,6 +229,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::LOITER_ALT_QLAND: #endif case Mode::Number::INITIALISING: +#if MODE_AUTOLAND_ENABLED + case Mode::Number::AUTOLAND: +#endif break; } gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe On: %s", (reason == ModeReason:: GCS_FAILSAFE) ? "GCS" : "RC Long", control_mode->name()); diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 8bece6d4187c3..7be469079d7cc 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -141,6 +141,10 @@ bool Mode::enter() // Make sure the flight stage is correct for the new mode plane.update_flight_stage(); + + // reset landing state + plane.landing.reset(); + #if HAL_QUADPLANE_ENABLED if (quadplane.enabled()) { diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 1b22fd1531ede..393aa61d69c71 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -15,6 +15,10 @@ #define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED #endif +#ifndef MODE_AUTOLAND_ENABLED +#define MODE_AUTOLAND_ENABLED 1 +#endif + #include class AC_PosControl; @@ -61,6 +65,9 @@ class Mode #if HAL_QUADPLANE_ENABLED LOITER_ALT_QLAND = 25, #endif +#if MODE_AUTOLAND_ENABLED + AUTOLAND = 26, +#endif // Mode number 30 reserved for "offboard" for external/lua control. }; @@ -859,7 +866,41 @@ class ModeTakeoff: public Mode bool have_autoenabled_fences; }; +#if MODE_AUTOLAND_ENABLED +class ModeAutoLand: public Mode +{ +public: + ModeAutoLand(); + + Number mode_number() const override { return Number::AUTOLAND; } + const char *name() const override { return "AUTOLAND"; } + const char *name4() const override { return "ALND"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + + void navigate() override; + + bool allows_throttle_nudging() const override { return true; } + bool does_auto_navigation() const override { return true; } + + bool does_auto_throttle() const override { return true; } + + + // var_info for holding parameter information + static const struct AP_Param::GroupInfo var_info[]; + + AP_Int16 final_wp_alt; + AP_Int16 final_wp_dist; + AP_Int16 landing_dir_off; + +protected: + bool _enter() override; + AP_Mission::Mission_Command cmd; + bool land_started; +}; +#endif #if HAL_SOARING_ENABLED class ModeThermal: public Mode diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp new file mode 100644 index 0000000000000..7a22e46e9812b --- /dev/null +++ b/ArduPlane/mode_autoland.cpp @@ -0,0 +1,118 @@ +#include "mode.h" +#include "Plane.h" +#include + +#if MODE_AUTOLAND_ENABLED + +/* + mode AutoLand parameters + */ +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 + // @Range: 0 200 + // @Increment: 1 + // @Units: m + // @User: Standard + AP_GROUPINFO("WP_ALT", 1, ModeAutoLand, final_wp_alt, 55), + + // @Param: WP_DIST + // @DisplayName: Final approach WP distance + // @Description: This is the distance from Home that the final approach waypoint is set. The waypoint point will be in the opposite direction of takeoff (the direction the plane is facing when the plane sets its takeoff heading) + // @Range: 0 700 + // @Increment: 1 + // @Units: m + // @User: Standard + AP_GROUPINFO("WP_DIST", 2, ModeAutoLand, final_wp_dist, 400), + + // @Param: DIR_OFF + // @DisplayName: Landing direction offset from takeoff + // @Description: The captured takeoff direction (at arming,if TKOFF_OPTION bit1 is set, or after ground course is established in autotakeoffs)is offset by this amount to create a different landing direction and approach. + // @Range: -360 360 + // @Increment: 1 + // @Units: deg + // @User: Standard + AP_GROUPINFO("DIR_OFF", 3, ModeAutoLand, landing_dir_off, 0), + + AP_GROUPEND +}; + +ModeAutoLand::ModeAutoLand() : + Mode() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +bool ModeAutoLand::_enter() +{ + //must be flying to enter + if (!plane.is_flying()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!"); + return false; + } + + //takeoff direction must be set and must not be a quadplane, otherwise since flying switch to RTL so this can be used as FS action +#if HAL_QUADPLANE_ENABLED + if (quadplane.available() && !quadplane.option_is_set(QuadPlane::OPTION::ALLOW_FW_LAND)) { + gcs().send_text(MAV_SEVERITY_WARNING, "Option not set to allow fixed wing autoland"); + return false; + } +#endif + if (!plane.takeoff_state.initial_direction.initialized) { + gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set,must use autotakeoff"); + return false; + } + + + //setup final approach waypoint + plane.prev_WP_loc = plane.current_loc; + const Location &home = ahrs.get_home(); + plane.set_target_altitude_current(); + plane.next_WP_loc = home; + const float bearing = wrap_360(plane.takeoff_state.initial_direction.heading + 180); + plane.next_WP_loc.offset_bearing(bearing, final_wp_dist); + plane.next_WP_loc.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME); + + // create a command to fly to final approach waypoint and start it + cmd.id = MAV_CMD_NAV_WAYPOINT; + cmd.content.location = plane.next_WP_loc; + plane.start_command(cmd); + land_started = false; + return true; +} + +void ModeAutoLand::update() +{ + plane.calc_nav_roll(); + 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() +{ + // check to see if if we have reached final approach waypoint, switch to NAV_LAND command and start it once if so + if (!land_started){ + if (plane.verify_nav_wp(cmd)){ + const Location &home_loc = ahrs.get_home(); + cmd.id = MAV_CMD_NAV_LAND; + cmd.content.location = home_loc; + land_started = true; + plane.prev_WP_loc = plane.current_loc; + plane.next_WP_loc = home_loc; + plane.start_command(cmd); + plane.set_flight_stage(AP_FixedWing::FlightStage::LAND); + } + return; + //otherwise keep flying the current command + } else { + plane.verify_command(cmd); + } +} +#endif diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 42cdcb3d45673..8f18f6d1b2eeb 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -134,6 +134,9 @@ void ModeTakeoff::update() plane.takeoff_state.throttle_max_timer_ms = millis(); takeoff_mode_setup = true; plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function. +#if MODE_AUTOLAND_ENABLED + plane.takeoff_state.initial_direction.initialized = false; +#endif } } } @@ -142,7 +145,16 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); takeoff_mode_setup = false; } - +#if MODE_AUTOLAND_ENABLED + // set initial_direction.heading + const float min_gps_speed = GPS_GND_CRS_MIN_SPD; + if (!(plane.takeoff_state.initial_direction.initialized) && (plane.gps.ground_speed() > min_gps_speed) + && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { + plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off); + plane.takeoff_state.initial_direction.initialized = true; + gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading)); + } +#endif // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we // pass the target location. The check for target location prevents us // flying towards a wrong location if we can't climb. diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index e4489fd0d17c1..47d9154f938e0 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -62,7 +62,8 @@ class QuadPlane friend class ModeQAutotune; friend class ModeQAcro; friend class ModeLoiterAltQLand; - + friend class ModeAutoLand; + QuadPlane(AP_AHRS &_ahrs); static QuadPlane *get_singleton() { diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index d7617e2f5696b..3f18b405aeb9b 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -22,12 +22,17 @@ bool Plane::auto_takeoff_check(void) return false; } - // Reset states if process has been interrupted - if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) { - memset(&takeoff_state, 0, sizeof(takeoff_state)); - return false; - } - + // Reset states if process has been interrupted, except initial_direction.initialized if set +#if MODE_AUTOLAND_ENABLED + bool takeoff_dir_initialized = takeoff_state.initial_direction.initialized; +#endif + if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) { + memset(&takeoff_state, 0, sizeof(takeoff_state)); +#if MODE_AUTOLAND_ENABLED + takeoff_state.initial_direction.initialized = takeoff_dir_initialized; //restore dir init state +#endif + return false; + } takeoff_state.last_check_ms = now; //check if waiting for rudder neutral after rudder arm From 6962e1c5534d52ff222156ac962212188cc5683c Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sun, 22 Dec 2024 13:39:44 -0600 Subject: [PATCH 5/7] Tools: add AutoLand fixed-wing mode --- Tools/scripts/build_options.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index ae83948aef536..20af644858071 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -226,6 +226,7 @@ def config_option(self): Feature('Plane', 'PLANE_GUIDED_SLEW', 'AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', 'Enable Offboard-guided slew commands', 0, None), # noqa:401 Feature('Plane', 'PLANE_GLIDER_PULLUP', 'AP_PLANE_GLIDER_PULLUP_ENABLED', 'Enable Glider pullup support', 0, None), Feature('Plane', 'QUICKTUNE', 'AP_QUICKTUNE_ENABLED', 'Enable VTOL quicktune', 0, None), + Feature('Plane', 'AUTOLAND_MODE', 'MODE_AUTOLAND_ENABLED', 'Enable Fixed Wing Autolanding mode', 0, None), Feature('RC', 'RC_Protocol', 'AP_RCPROTOCOL_ENABLED', "Enable Serial RC Protocols", 0, None), # NOQA: E501 Feature('RC', 'RC_CRSF', 'AP_RCPROTOCOL_CRSF_ENABLED', "Enable CRSF", 0, "RC_Protocol"), # NOQA: E501 From 76f686d6c71fe800a9b2da07d6fa673c9fe6bd00 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Dec 2024 12:01:25 +1100 Subject: [PATCH 6/7] Plane: added base leg WP to autoland this gives a cleaner landing, keeping the plane in the part of the field where the pilot is currently flying --- ArduPlane/mode.h | 4 +- ArduPlane/mode_autoland.cpp | 98 ++++++++++++++++++++++++++----------- 2 files changed, 71 insertions(+), 31 deletions(-) diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 393aa61d69c71..6e5d0b56327f0 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -897,8 +897,8 @@ class ModeAutoLand: public Mode protected: bool _enter() override; - AP_Mission::Mission_Command cmd; - bool land_started; + AP_Mission::Mission_Command cmd[3]; + uint8_t stage; }; #endif #if HAL_SOARING_ENABLED diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index 7a22e46e9812b..e9ec9e8e96442 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -64,21 +64,62 @@ bool ModeAutoLand::_enter() return false; } - - //setup final approach waypoint plane.prev_WP_loc = plane.current_loc; - const Location &home = ahrs.get_home(); plane.set_target_altitude_current(); - plane.next_WP_loc = home; - const float bearing = wrap_360(plane.takeoff_state.initial_direction.heading + 180); - plane.next_WP_loc.offset_bearing(bearing, final_wp_dist); - plane.next_WP_loc.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME); - - // create a command to fly to final approach waypoint and start it - cmd.id = MAV_CMD_NAV_WAYPOINT; - cmd.content.location = plane.next_WP_loc; - plane.start_command(cmd); - land_started = false; + + /* + landing is in 3 steps: + 1) a base leg waypoint + 2) a land start WP, with crosstrack + 3) the landing WP, with crosstrack + + the base leg point is 90 degrees off from the landing leg + */ + const Location &home = ahrs.get_home(); + + /* + first calculate the starting waypoint we will use when doing the + NAV_LAND. This is offset by final_wp_dist from home, in a + direction 180 degrees from takeoff direction + */ + Location land_start_WP = home; + land_start_WP.offset_bearing(wrap_360(plane.takeoff_state.initial_direction.heading + 180), final_wp_dist); + land_start_WP.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME); + land_start_WP.change_alt_frame(Location::AltFrame::ABSOLUTE); + + /* + now create the initial target waypoint for the base leg. We + choose if we will do a right or left turn onto the landing based + on where we are when we enter the landing mode + */ + const float bearing_to_current_deg = wrap_180(degrees(land_start_WP.get_bearing(plane.current_loc))); + const float bearing_err_deg = wrap_180(wrap_180(plane.takeoff_state.initial_direction.heading) - bearing_to_current_deg); + const float bearing_offset_deg = bearing_err_deg > 0? -90 : 90; + const float base_leg_length = final_wp_dist * 0.333; + cmd[0].id = MAV_CMD_NAV_WAYPOINT; + cmd[0].content.location = land_start_WP; + cmd[0].content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, base_leg_length); + // set a 1m acceptance radius, we want to fly all the way to this waypoint + cmd[0].p1 = 1; + + /* + create the WP for the start of the landing + */ + cmd[1].content.location = land_start_WP; + cmd[1].id = MAV_CMD_NAV_WAYPOINT; + + // and the land WP + cmd[2].id = MAV_CMD_NAV_LAND; + cmd[2].content.location = home; + + // start first leg + stage = 0; + plane.start_command(cmd[0]); + + // don't crosstrack initially + plane.auto_state.crosstrack = false; + plane.auto_state.next_wp_crosstrack = true; + return true; } @@ -96,23 +137,22 @@ void ModeAutoLand::update() } void ModeAutoLand::navigate() -{ - // check to see if if we have reached final approach waypoint, switch to NAV_LAND command and start it once if so - if (!land_started){ - if (plane.verify_nav_wp(cmd)){ - const Location &home_loc = ahrs.get_home(); - cmd.id = MAV_CMD_NAV_LAND; - cmd.content.location = home_loc; - land_started = true; - plane.prev_WP_loc = plane.current_loc; - plane.next_WP_loc = home_loc; - plane.start_command(cmd); - plane.set_flight_stage(AP_FixedWing::FlightStage::LAND); - } +{ + if (stage == 2) { + // we are on final landing leg + plane.set_flight_stage(AP_FixedWing::FlightStage::LAND); + plane.verify_command(cmd[stage]); return; - //otherwise keep flying the current command - } else { - plane.verify_command(cmd); + } + + // see if we have completed the leg + if (plane.verify_nav_wp(cmd[stage])) { + stage++; + plane.prev_WP_loc = plane.next_WP_loc; + plane.auto_state.next_turn_angle = 90; + plane.start_command(cmd[stage]); + plane.auto_state.crosstrack = true; + plane.auto_state.next_wp_crosstrack = true; } } #endif From b6aa8ca23c7886418649eb03bf6edf98e90623d8 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 28 Dec 2024 08:27:45 -0600 Subject: [PATCH 7/7] ArduPlane:bug fix --- ArduPlane/takeoff.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 3f18b405aeb9b..740daeaf05758 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -25,11 +25,13 @@ bool Plane::auto_takeoff_check(void) // Reset states if process has been interrupted, except initial_direction.initialized if set #if MODE_AUTOLAND_ENABLED bool takeoff_dir_initialized = takeoff_state.initial_direction.initialized; + float takeoff_dir = takeoff_state.initial_direction.heading; #endif if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) { memset(&takeoff_state, 0, sizeof(takeoff_state)); #if MODE_AUTOLAND_ENABLED takeoff_state.initial_direction.initialized = takeoff_dir_initialized; //restore dir init state + takeoff_state.initial_direction.heading = takeoff_dir; #endif return false; }