diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3c0d2e18fd259f..484acf03c077ef 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -538,6 +538,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("BCK_PIT_LIM", 38, QuadPlane, q_bck_pitch_lim, 10.0f), + // @Param: APPROACH_DIST + // @DisplayName: Q mode approach distance + // @Description: The minimum distance from the destination to use the fixed wing airbrake and approach code for landing approach. This is useful if you don't want the fixed wing approach logic to be used when you are close to the destination. Set to zero to use the default value of 1.5 times the calculated stopping_distance based on cruise airspeed and Q_TRANS_DECEL. + // @Units: m + // @Range: 0.0 1000 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("APPROACH_DIST", 39, QuadPlane, approach_distance, 0), + AP_GROUPEND }; @@ -2160,7 +2169,8 @@ void QuadPlane::run_xy_controller(float accel_limit) void QuadPlane::poscontrol_init_approach(void) { const float dist = plane.current_loc.get_distance(plane.next_WP_loc); - if (option_is_set(QuadPlane::OPTION::DISABLE_APPROACH)) { + if (option_is_set(QuadPlane::OPTION::DISABLE_APPROACH) || + (is_positive(approach_distance) && dist < approach_distance)) { // go straight to QPOS_POSITION1 poscontrol.set_state(QPOS_POSITION1); gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ea3bba87791321..6a810f23259cf1 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -609,6 +609,9 @@ class QuadPlane return (options.get() & int32_t(option)) != 0; } + // minimum distance to be from destination to use approach logic + AP_Float approach_distance; + AP_Float takeoff_failure_scalar; AP_Float maximum_takeoff_airspeed; uint32_t takeoff_start_time_ms;