From fe2e75b2a0be533b5ed5a13d915d6b3573e368a1 Mon Sep 17 00:00:00 2001 From: Ep Pravitra Date: Wed, 24 Jul 2024 15:18:42 +0700 Subject: [PATCH] Plane: option to immediately climb in AUTO mode (not doing glide slope) add comment in Parameters.cpp Update ArduPlane/altitude.cpp Co-authored-by: Peter Hall <33176108+IamPete1@users.noreply.github.com> clarification in FLIGHT_OPTIONS description change the comment param comment change Update ArduPlane/Parameters.cpp Co-authored-by: Peter Barker --- ArduPlane/Parameters.cpp | 1 + ArduPlane/altitude.cpp | 7 +++++++ ArduPlane/defines.h | 1 + 3 files changed, 9 insertions(+) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 65198d9cb5..7ff30ad427 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1096,6 +1096,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode // @Bitmask: 12: Enable FBWB style loiter altitude control // @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces + // @Bitmask: 14: In AUTO - climb to next waypoint altitude immediately instead of linear climb // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 8b7970e631..6e32f61943 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -81,6 +81,13 @@ void Plane::setup_glide_slope(void) break; case Mode::Number::AUTO: + + //climb without doing glide slope if option is enabled + if (!above_location_current(next_WP_loc) && plane.flight_option_enabled(FlightOptions::IMMEDIATE_CLIMB_IN_AUTO)) { + reset_offset_altitude(); + break; + } + // we only do glide slide handling in AUTO when above 20m or // when descending. The 20 meter threshold is arbitrary, and // is basically to prevent situations where we try to slowly diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index f4a8f9fc45..b2939557ad 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -168,6 +168,7 @@ enum FlightOptions { DISABLE_GROUND_PID_SUPPRESSION = (1<<11), ENABLE_LOITER_ALT_CONTROL = (1<<12), INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13), + IMMEDIATE_CLIMB_IN_AUTO = (1<<14), }; enum CrowFlapOptions {