diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index dc71a92778..c6cb1e56d2 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2622,6 +2622,7 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) if (s == QPOS_POSITION1) { reached_wp_speed = false; qp.attitude_control->reset_yaw_target_and_rate(); + pos1_start_speed = plane.ahrs.groundspeed_vector().length(); } else if (s == QPOS_POSITION2) { // POSITION2 changes target speed, so we need to change it // back to normal @@ -2846,6 +2847,9 @@ void QuadPlane::vtol_position_controller(void) const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared(); const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); + // limit target speed to initial pos1 speed, but at least twice the Q_WP_SPEED + target_speed = MIN(MAX(poscontrol.pos1_start_speed, 2*wp_speed), target_speed); + if (poscontrol.reached_wp_speed || current_speed_sq < sq(wp_speed) || wp_speed > 1.35*scaled_wp_speed) { diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index f916536b13..5e28d22f4c 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -492,6 +492,7 @@ private: uint32_t last_log_ms; bool reached_wp_speed; uint32_t last_run_ms; + float pos1_start_speed; private: uint32_t last_state_change_ms; enum position_control_state state;