From a0b9ca9dd46b4709ff37aeae3720f7ee582917ac Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 21 Dec 2024 16:46:38 -0600 Subject: [PATCH] ArduPlane:add AUTOLAND build option --- ArduPlane/AP_Arming.cpp | 3 ++- ArduPlane/ArduPlane.cpp | 6 +++++- ArduPlane/GCS_Mavlink.cpp | 2 ++ ArduPlane/GCS_Plane.cpp | 2 ++ ArduPlane/Parameters.cpp | 4 +++- ArduPlane/Plane.h | 7 ++++++- ArduPlane/commands_logic.cpp | 4 ++++ ArduPlane/control_modes.cpp | 2 ++ ArduPlane/events.cpp | 14 ++++++++++++-- ArduPlane/mode.h | 10 ++++++++-- ArduPlane/mode_autoland.cpp | 3 +++ ArduPlane/mode_takeoff.cpp | 6 ++++-- ArduPlane/quadplane.h | 2 +- ArduPlane/takeoff.cpp | 4 ++++ Tools/scripts/build_options.py | 1 + 15 files changed, 59 insertions(+), 11 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index a72b630b0a52b3..93046fd3ac8ced 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -378,10 +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.takeoff_direction_initialized = false; +#endif send_arm_disarm_statustext("Throttle disarmed"); - return true; } diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 15c5dc6e7fb44b..48435b9405cc2a 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -684,7 +684,11 @@ void Plane::update_flight_stage(void) } #endif set_flight_stage(AP_FixedWing::FlightStage::NORMAL); - } else if ((control_mode != &mode_takeoff) && (control_mode != &mode_autoland)) { + } 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/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index db5ba5c397db5e..363e08565319d9 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -59,7 +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: diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index 19623d898b0724..6b76d734cb3d75 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -70,7 +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 1f26bb2fd59b04..0544b5fefc9e1c 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1027,10 +1027,12 @@ const AP_Param::Info Plane::var_info[] = { // @Group: TKOFF_ // @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_ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 21db33698d80ee..8c9ec8763da9ad 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -172,8 +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 @@ -327,7 +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 @@ -457,8 +460,10 @@ class Plane : public AP_Vehicle { 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 int32_t takeoff_initial_direction; //deg bool takeoff_direction_initialized; +#endif } takeoff_state; // ground steering controller state diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 75df07831025eb..8ee6d7fd333070 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -382,7 +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.takeoff_direction_initialized = false; +#endif auto_state.baro_takeoff_alt = barometer.get_altitude(); } @@ -557,9 +559,11 @@ bool Plane::verify_takeoff() 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.takeoff_initial_direction = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off); takeoff_state.takeoff_direction_initialized = true; gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.takeoff_initial_direction)); +#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 64e8a87047cdaa..5d07b45534e2b7 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -83,9 +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/events.cpp b/ArduPlane/events.cpp index c4ab3211edcf09..1584775ad1ff06 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -66,7 +66,10 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso #endif // HAL_QUADPLANE_ENABLED case Mode::Number::AUTO: - case Mode::Number::AUTOLAND: { +#if MODE_AUTOLAND_ENABLED + case Mode::Number::AUTOLAND: +#endif + { if (failsafe_in_landing_sequence()) { // don't failsafe in a landing sequence break; @@ -146,10 +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); } @@ -199,20 +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); } } else if (g.fs_action_long == FS_ACTION_LONG_RTL) { set_mode(mode_rtl, reason); +#endif } 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 @@ -221,7 +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.h b/ArduPlane/mode.h index c4a044998c3599..f1df8594a34701 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,7 +65,9 @@ class Mode #if HAL_QUADPLANE_ENABLED LOITER_ALT_QLAND = 25, #endif +#if MODE_AUTOLAND_ENABLED AUTOLAND = 26, +#endif }; // Constructor @@ -858,7 +864,7 @@ class ModeTakeoff: public Mode bool have_autoenabled_fences; }; - +#if MODE_AUTOLAND_ENABLED class ModeAutoLand: public Mode { public: @@ -892,7 +898,7 @@ class ModeAutoLand: public Mode 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 index a3931fb878662c..3003838a21b388 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -2,6 +2,8 @@ #include "Plane.h" #include +#if MODE_AUTOLAND_ENABLED + /* mode AutoLand parameters */ @@ -113,3 +115,4 @@ void ModeAutoLand::navigate() plane.verify_command(cmd); } } +#endif diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 1ae85efcce7f81..c57506ede8cd86 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -134,7 +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.takeoff_direction_initialized = false; +#endif } } } @@ -143,7 +145,7 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); takeoff_mode_setup = false; } - +#if MODE_AUTOLAND_ENABLED // 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) @@ -152,7 +154,7 @@ void ModeTakeoff::update() plane.takeoff_state.takeoff_direction_initialized = true; gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.takeoff_initial_direction)); } - +#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 b68c2c27c7892d..47d9154f938e00 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -63,7 +63,7 @@ class QuadPlane 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 79c2a7d79c008f..68881182678a0c 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -23,10 +23,14 @@ bool Plane::auto_takeoff_check(void) } // Reset states if process has been interrupted, except takeoff_direction_initialized if set +#if MODE_AUTOLAND_ENABLED bool takeoff_dir_initialized = takeoff_state.takeoff_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.takeoff_direction_initialized = takeoff_dir_initialized; //restore dir init state +#endif return false; } takeoff_state.last_check_ms = now; diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index ae83948aef536b..20af644858071f 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