diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a3548e2b28..2a6a1d5201 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2085,6 +2085,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 @@ -2303,6 +2304,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 45ecc0347b..57ef9173a7 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -436,6 +436,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;