From 68ca6aed626ec18b85ccf33af70b609b7f952be0 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Thu, 28 Nov 2024 20:40:29 -0600 Subject: [PATCH 1/3] 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 f1241e02f874b80ecabddbf83af2c0e5e53616df Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Thu, 28 Nov 2024 20:40:29 -0600 Subject: [PATCH 2/3] ArduPlane: add AutoLand fixed-wing mode --- ArduPlane/ArduPlane.cpp | 4 +- ArduPlane/GCS_Mavlink.cpp | 1 + ArduPlane/GCS_Plane.cpp | 1 + ArduPlane/Parameters.cpp | 4 ++ ArduPlane/Parameters.h | 2 +- ArduPlane/Plane.h | 4 ++ ArduPlane/commands_logic.cpp | 6 +- ArduPlane/control_modes.cpp | 3 + ArduPlane/defines.h | 3 + ArduPlane/events.cpp | 14 ++++- ArduPlane/mode.h | 40 +++++++++++++ ArduPlane/mode_autoland.cpp | 106 +++++++++++++++++++++++++++++++++++ ArduPlane/mode_takeoff.cpp | 10 ++++ 13 files changed, 194 insertions(+), 4 deletions(-) create mode 100644 ArduPlane/mode_autoland.cpp diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 7d27791de5571..d24de4427f900 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -684,11 +684,13 @@ 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) && (control_mode != &mode_autoland)) { // 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. set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + } else if (control_mode == &mode_autoland) { + set_flight_stage(AP_FixedWing::FlightStage::LAND); } return; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 298b4e47a5195..0d1b505ea7998 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -59,6 +59,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case Mode::Number::GUIDED: case Mode::Number::CIRCLE: case Mode::Number::TAKEOFF: + case Mode::Number::AUTOLAND: #if HAL_QUADPLANE_ENABLED case Mode::Number::QRTL: case Mode::Number::LOITER_ALT_QLAND: diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index a193f44e0858d..19623d898b072 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -70,6 +70,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) case Mode::Number::GUIDED: case Mode::Number::CIRCLE: case Mode::Number::TAKEOFF: + case Mode::Number::AUTOLAND: #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 04a5ba3a59525..6bf94712efbff 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1027,6 +1027,10 @@ const AP_Param::Info Plane::var_info[] = { // @Group: TKOFF_ // @Path: mode_takeoff.cpp GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff), + + // @Group: AUTOLAND_ + // @Path: mode_autoland.cpp + GOBJECT(mode_autoland, "AUTOLAND_", ModeAutoLand), #if AP_PLANE_GLIDER_PULLUP_ENABLED // @Group: PUP_ diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index e7cc5326f4280..39cb0fd56a1e7 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -100,7 +100,7 @@ class Parameters { k_param_sonar_old, // unused k_param_log_bitmask, k_param_BoardConfig, - k_param_rssi_range, // unused, replaced by rssi_ library parameters + k_param_mode_autoland, // was rssi_range k_param_flapin_channel_old, // unused, moved to RC_OPTION k_param_flaperon_output, // unused k_param_gps, diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index d2d35a7b14e0a..b578db48500de 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -172,6 +172,7 @@ class Plane : public AP_Vehicle { friend class ModeTakeoff; friend class ModeThermal; friend class ModeLoiterAltQLand; + friend class ModeAutoLand; #if AP_EXTERNAL_CONTROL_ENABLED friend class AP_ExternalControl_Plane; @@ -326,6 +327,7 @@ class Plane : public AP_Vehicle { #endif // QAUTOTUNE_ENABLED #endif // HAL_QUADPLANE_ENABLED ModeTakeoff mode_takeoff; + ModeAutoLand mode_autoland; #if HAL_SOARING_ENABLED ModeThermal mode_thermal; #endif @@ -454,6 +456,8 @@ class Plane : public AP_Vehicle { float throttle_lim_min; uint32_t throttle_max_timer_ms; // Good candidate for keeping the initial time for TKOFF_THR_MAX_T. + int32_t takeoff_initial_direction; //deg + bool takeoff_direction_initialized; } takeoff_state; // ground steering controller state diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 90c3e024800c6..1e4c8ceb5cc0e 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -380,6 +380,7 @@ 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; + takeoff_state.takeoff_direction_initialized = false; auto_state.baro_takeoff_alt = barometer.get_altitude(); } @@ -548,12 +549,15 @@ 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(); + takeoff_state.takeoff_initial_direction = gps.ground_course(); + takeoff_state.takeoff_direction_initialized = true; + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff initial direction= %u",int(takeoff_state.takeoff_initial_direction)); } 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..64e8a87047cda 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -83,6 +83,9 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num) case Mode::Number::TAKEOFF: ret = &mode_takeoff; break; + case Mode::Number::AUTOLAND: + ret = &mode_autoland; + break; case Mode::Number::THERMAL: #if HAL_SOARING_ENABLED ret = &mode_thermal; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a3da572611c77..ae11459070ccc 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 takeoff_initial_direction 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..c4ab3211edcf0 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -65,7 +65,8 @@ 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: + case Mode::Number::AUTOLAND: { if (failsafe_in_landing_sequence()) { // don't failsafe in a landing sequence break; @@ -145,6 +146,10 @@ 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); + } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) { + if(!set_mode(mode_autoland, reason)) { + set_mode(mode_rtl, reason); + } } else { set_mode(mode_rtl, reason); } @@ -194,6 +199,10 @@ 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); + } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) { + if(!set_mode(mode_autoland, reason)) { + set_mode(mode_rtl, reason); + } } else if (g.fs_action_long == FS_ACTION_LONG_RTL) { set_mode(mode_rtl, reason); } @@ -202,6 +211,8 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::RTL: if (g.fs_action_long == FS_ACTION_LONG_AUTO) { set_mode(mode_auto, reason); + } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) { + set_mode(mode_autoland, reason); } break; #if HAL_QUADPLANE_ENABLED @@ -210,6 +221,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason case Mode::Number::LOITER_ALT_QLAND: #endif case Mode::Number::INITIALISING: + case Mode::Number::AUTOLAND: 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.h b/ArduPlane/mode.h index 213e547c6a668..d65dc789f7923 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -61,6 +61,7 @@ class Mode #if HAL_QUADPLANE_ENABLED LOITER_ALT_QLAND = 25, #endif + AUTOLAND = 26, }; // Constructor @@ -856,6 +857,45 @@ class ModeTakeoff: public Mode // flag that we have already called autoenable fences once in MODE TAKEOFF bool have_autoenabled_fences; +}; + +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 "AUTOLAND"; } + + // 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; + +protected: + bool _enter() override; + AP_Mission::Mission_Command cmd; + bool land_started; + + +private: + + + }; #if HAL_SOARING_ENABLED diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp new file mode 100644 index 0000000000000..fde14fd75471e --- /dev/null +++ b/ArduPlane/mode_autoland.cpp @@ -0,0 +1,106 @@ +#include "mode.h" +#include "Plane.h" +#include + +/* + 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), + + 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()) { + gcs().send_text(MAV_SEVERITY_WARNING, "AutoLand is fixed wing only mode"); + return false; + } +#endif + if (!plane.takeoff_state.takeoff_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; + uint16_t bearing_cd = wrap_360_cd((plane.takeoff_state.takeoff_initial_direction + 180)*100); + plane.next_WP_loc.offset_bearing(bearing_cd * 0.01f, 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; + gcs().send_text(MAV_SEVERITY_WARNING, "AutoLanding"); + 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); + } + return; + //otherwise keep flying the current command + } else { + plane.verify_command(cmd); + } +} diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 5f93cc4729b2b..06bfc995d4c1a 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -122,6 +122,7 @@ 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. + plane.takeoff_state.takeoff_direction_initialized = false; } } } @@ -130,6 +131,15 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); takeoff_mode_setup = false; } + + // set takeoff_initial_direction + const float min_gps_speed = GPS_GND_CRS_MIN_SPD; + if (!(plane.takeoff_state.takeoff_direction_initialized) && (plane.gps.ground_speed() > min_gps_speed) + && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { + plane.takeoff_state.takeoff_initial_direction = plane.gps.ground_course(); + plane.takeoff_state.takeoff_direction_initialized = true; + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff initial direction= %u",int(plane.takeoff_state.takeoff_initial_direction)); + } // 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 From 2d5e36d840acd66c872be9f0fa73a37efbe8e39c Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sun, 1 Dec 2024 18:16:09 -0600 Subject: [PATCH 3/3] autotest:add AUTOLAND mode test --- ArduPlane/Parameters.h | 4 +++- ArduPlane/mode.h | 6 ------ ArduPlane/mode_autoland.cpp | 1 - Tools/autotest/arduplane.py | 15 +++++++++++++++ 4 files changed, 18 insertions(+), 8 deletions(-) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 39cb0fd56a1e7..9945869250b4b 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -100,7 +100,7 @@ class Parameters { k_param_sonar_old, // unused k_param_log_bitmask, k_param_BoardConfig, - k_param_mode_autoland, // was rssi_range + k_param_rssi_range, // unused, replaced by rssi_ library parameters k_param_flapin_channel_old, // unused, moved to RC_OPTION k_param_flaperon_output, // unused k_param_gps, @@ -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/mode.h b/ArduPlane/mode.h index d65dc789f7923..e1c9589472c00 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -890,12 +890,6 @@ class ModeAutoLand: public Mode bool _enter() override; AP_Mission::Mission_Command cmd; bool land_started; - - -private: - - - }; #if HAL_SOARING_ENABLED diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index fde14fd75471e..e9d13858035a2 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -68,7 +68,6 @@ bool ModeAutoLand::_enter() cmd.content.location = plane.next_WP_loc; plane.start_command(cmd); land_started = false; - gcs().send_text(MAV_SEVERITY_WARNING, "AutoLanding"); return true; } diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 0ce13e5c3e327..686d6bd686838 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4201,6 +4201,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("Takeoff initial 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) @@ -6438,6 +6452,7 @@ def tests1b(self): self.MAV_CMD_DO_AUX_FUNCTION, self.SmartBattery, self.FlyEachFrame, + self.AutoLandMode, self.RCDisableAirspeedUse, self.AHRS_ORIENTATION, self.AHRSTrim,