From 7213288ae36f651ac5b865bc5021f46c59b0249b Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 15 Nov 2024 07:55:40 -0600 Subject: [PATCH] wip:params and framework --- ArduPlane/GCS_Mavlink.cpp | 1 + ArduPlane/GCS_Plane.cpp | 1 + ArduPlane/Parameters.cpp | 4 +++ ArduPlane/Parameters.h | 2 +- ArduPlane/Plane.h | 1 + ArduPlane/control_modes.cpp | 3 ++ ArduPlane/events.cpp | 4 ++- ArduPlane/mode.h | 38 +++++++++++++++++++++++++ ArduPlane/mode_autoland.cpp | 56 +++++++++++++++++++++++++++++++++++++ 9 files changed, 108 insertions(+), 2 deletions(-) create mode 100644 ArduPlane/mode_autoland.cpp 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 b2db9099f8f51..fd45e22de2cc0 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 24645b3b88471..605afbeaa9598 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 64b5ba6d95647..b0a87e057e9ae 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -326,6 +326,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 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/events.cpp b/ArduPlane/events.cpp index 64127c633c181..9ee6e78f8e510 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; @@ -169,6 +170,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason #endif // HAL_QUADPLANE_ENABLED case Mode::Number::AUTO: + case Mode::Number::AUTOLAND: if (failsafe_in_landing_sequence()) { // don't failsafe in a landing sequence break; diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 8260924afca89..97ed4166d4ae6 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -55,6 +55,7 @@ class Mode #if HAL_QUADPLANE_ENABLED LOITER_ALT_QLAND = 25, #endif + AUTOLAND = 26, }; // Constructor @@ -835,6 +836,43 @@ 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; + + +private: + + + }; #if HAL_SOARING_ENABLED diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp new file mode 100644 index 0000000000000..b18622e59d5cb --- /dev/null +++ b/ArduPlane/mode_autoland.cpp @@ -0,0 +1,56 @@ +#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 for final approach waypoint + // @Range: 0 200 + // @Increment: 1 + // @Units: m + // @User: Standard + AP_GROUPINFO("WP_ALT", 1, ModeAutoLand, final_wp_alt, 55), + + // @Param: 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() +{ +// upon enter we setup a final wp and land wp based on home and takeoff_dir + const Location &home = ahrs.get_home(); + plane.set_target_altitude_current(); + plane.next_WP_loc.offset_bearing(bearing_cd, home.alt*0.01f + final_wp_alt); + plane.prev_WP_loc = plane.current_loc; + plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); + return true; +} + +void ModeAutoLand::update() +{ + plane.calc_nav_roll(); + plane.update_fbwb_speed_height(); +} + +void ModeAutoLand::navigate() +{ + +}