From dab90792f5e92d6d70b956d07bf45d85daceb508 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/GCS_Mavlink.cpp | 2 ++ ArduPlane/GCS_Plane.cpp | 2 ++ ArduPlane/Parameters.cpp | 4 +++- ArduPlane/Plane.h | 5 ++++- ArduPlane/commands_logic.cpp | 4 ++++ ArduPlane/control_modes.cpp | 2 ++ ArduPlane/events.cpp | 14 ++++++++++++-- ArduPlane/mode.h | 8 ++++++-- ArduPlane/mode_autoland.cpp | 3 +++ ArduPlane/mode_takeoff.cpp | 6 ++++-- ArduPlane/quadplane.h | 2 +- ArduPlane/takeoff.cpp | 4 ++++ Tools/scripts/build_options.py | 1 + 13 files changed, 48 insertions(+), 9 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index db5ba5c397db5..363e08565319d 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 19623d898b072..6b76d734cb3d7 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 1f26bb2fd59b0..0544b5fefc9e1 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 21db33698d80e..4e890111696b6 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 @@ -457,8 +458,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 75df07831025e..8ee6d7fd33307 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 64e8a87047cda..5d07b45534e2b 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 c4ab3211edcf0..1584775ad1ff0 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 c4a044998c359..66682600e38f5 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; @@ -858,7 +862,7 @@ class ModeTakeoff: public Mode bool have_autoenabled_fences; }; - +#if MODE_AUTOLAND_ENABLED class ModeAutoLand: public Mode { public: @@ -892,7 +896,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 a3931fb878662..3003838a21b38 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 1ae85efcce7f8..c57506ede8cd8 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 b68c2c27c7892..47d9154f938e0 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 79c2a7d79c008..68881182678a0 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 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