Plane: limiting POS1 landing target speed

prevent very high target speeds when the target velocity profile is
above the initial speed in POSITION1. Always allow up to 2*Q_WP_SPEED,
but don't go above the initial speed

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) {

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;