Plane: improvements to POSITION1 controller

this improves 4 things in the POSITION1 controller based on logs from
4.2.0beta2. The changes are designed to increase the tolerance to
an incorrect value for Q_TRANS_DECEL, reducing landing overshoot

1) we fix the initialisation of the acceleration. The
   init_xy_controller() function assumes zero accel, so we need to
   call set_accel_desired_xy_cmss() just after that init to get the
   correct accel. Thanks to Leonard for this fix

2) if we decel more than expected due to too low Q_TRANS_DECEL we
   need to reduce the target speed, rather than putting the nose down

3) lower the default Q_P_JERK_XY to a value more appropriate for most
   quadplanes (Leonard suggested a value of 2)

4) fixed the pitch envelope from Q_BACKTRANS_MS to start after the
   airbrake phase is complete

ArduPlane/quadplane.cpp

@@ -470,6 +470,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = {
     { "Q_LOIT_SPEED",     500 },
     { "Q_WP_SPEED",       500 },
     { "Q_WP_ACCEL",       100 },
+    { "Q_P_JERK_XY",      2   },
 };
 
 /*
@@ -2175,7 +2176,8 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
             // never do a rate reset, if attitude control is not active it will be automaticaly reset before running, see: last_att_control_ms
             // if it is active then the rate control should not be reset at all
             qp.attitude_control->reset_yaw_target_and_rate(false);
-            pos1_start_speed = plane.ahrs.groundspeed_vector().length();
+            pos1_speed_limit = plane.ahrs.groundspeed_vector().length();
+            done_accel_init = false;
         } else if (s == QPOS_AIRBRAKE) {
             // start with zero integrator on vertical throttle
             qp.pos_control->get_accel_z_pid().set_integrator(0);
@@ -2422,11 +2424,13 @@ void QuadPlane::vtol_position_controller(void)
         float target_speed = stopping_speed;
 
         // maximum configured VTOL speed
-        const float wp_speed = wp_nav->get_default_speed_xy() * 0.01;
+        const float wp_speed = MAX(1.0, wp_nav->get_default_speed_xy() * 0.01);
         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);
+        // limit target speed to a the pos1 speed limit, which starts out at the initial speed
+        // but is adjusted if we start putting our nose down. We always allow at least twice
+        // the WP speed
+        target_speed = MIN(MAX(poscontrol.pos1_speed_limit, 2*wp_speed), target_speed);
 
         if (poscontrol.reached_wp_speed ||
             rel_groundspeed_sq < sq(wp_speed) ||
@@ -2452,17 +2456,39 @@ void QuadPlane::vtol_position_controller(void)
             target_speed_xy_cms = diff_wp_norm * target_speed * 100;
             target_accel_cms = diff_wp_norm * (-target_accel*100);
         }
+        const float target_speed_ms = target_speed_xy_cms.length() * 0.01;
 
         target_speed_xy_cms += landing_velocity * 100;
-        poscontrol.target_speed = target_speed_xy_cms.length()*0.01;
+        poscontrol.target_speed = target_speed_ms;
         poscontrol.target_accel = target_accel;
 
+        if (!poscontrol.reached_wp_speed &&
+            rel_groundspeed_sq < sq(target_speed_ms) &&
+            rel_groundspeed_sq > sq(2*wp_speed) &&
+            plane.nav_pitch_cd < 0) {
+            // we have slowed down more than expected, likely due to
+            // drag from the props and we're starting to put our nose
+            // down as a result. We want to accept the slowdown and
+            // re-calculate the target speed profile
+            poscontrol.pos1_speed_limit = sqrtf(rel_groundspeed_sq);
+        }
+
         // use input shaping and abide by accel and jerk limits
         pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel_cms);
 
         // run horizontal velocity controller
         run_xy_controller(MAX(target_accel, transition_decel)*1.5);
 
+        if (!poscontrol.done_accel_init) {
+            /*
+              the pos controller init assumes zero accel, we need to
+              override that so that we can start decelerating more
+              quickly at the start of POSITION1
+             */
+            poscontrol.done_accel_init = true;
+            pos_control->set_accel_desired_xy_cmss(target_accel_cms);
+        }
+        
         // nav roll and pitch are controller by position controller
         plane.nav_roll_cd = pos_control->get_roll_cd();
         plane.nav_pitch_cd = pos_control->get_pitch_cd();

ArduPlane/quadplane.h

@@ -447,7 +447,8 @@ private:
         uint32_t last_log_ms;
         bool reached_wp_speed;
         uint32_t last_run_ms;
-        float pos1_start_speed;
+        float pos1_speed_limit;
+        bool done_accel_init;
         Vector2f velocity_match;
         uint32_t last_velocity_match_ms;
         float target_speed;