Skip to content

Commit

Permalink
Plane: added Q_APPROACH_DIST
Browse files Browse the repository at this point in the history
this sets a minimum distance to use the fixed wing approach logic. It
is an alternative to just disabling the approach with Q_OPTIONS which
some users do to avoid some short distance problems. This allows the
approach to still be used for longer distances in QRTL but have it
disabled for shorter distances
  • Loading branch information
tridge committed Sep 11, 2024
1 parent 9beca76 commit 33418bf
Show file tree
Hide file tree
Showing 2 changed files with 14 additions and 1 deletion.
12 changes: 11 additions & 1 deletion ArduPlane/quadplane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

Expand Down Expand Up @@ -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);
Expand Down
3 changes: 3 additions & 0 deletions ArduPlane/quadplane.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit 33418bf

Please sign in to comment.