From 1a33ca3ebeb8ceb40522ee2d458ca8b3a1f549c6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 6 Nov 2021 09:53:25 +1100 Subject: [PATCH] Plane: fixed a bug in POSITION1 speed thresholds (for plane 4.1) we were comparing two different speeds in the threshold for going to Q_WP_SPEED limit. The reason the two speeds were different was the wp_nav init happened before the defaults were setup for quadplanes this fixes both bugs --- ArduPlane/quadplane.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 7581093224..ddad00e32a 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -814,7 +814,6 @@ bool QuadPlane::setup(void) motors->set_update_rate(rc_speed); motors->set_interlock(true); attitude_control->parameter_sanity_check(); - wp_nav->wp_and_spline_init(); // TODO: update this if servo function assignments change // used by relax_attitude_control() to control special behavior for vectored tailsitters @@ -859,6 +858,9 @@ bool QuadPlane::setup(void) tailsitter.transition_rate_fw.set_and_save(tailsitter.transition_angle_fw / (transition_time_ms/2000.0f)); } + // init wp_nav variables after detaults are setup + wp_nav->wp_and_spline_init(); + // param count will have changed AP_Param::invalidate_count(); @@ -2832,7 +2834,7 @@ void QuadPlane::vtol_position_controller(void) float target_speed = stopping_speed; // maximum configured VTOL speed - const float wp_speed = pos_control->get_max_speed_xy_cms() * 0.01; + const float wp_speed = wp_nav->get_default_speed_xy() * 0.01; const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared(); const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle()));