Plane: added Q_TRANS_DECEL

this gives a deceleration rate for QRTL transition from RTL, and also
for transitions to QLOITER

ArduPlane/ArduPlane.cpp

@@ -813,8 +813,13 @@ void Plane::update_navigation()
         if (quadplane.available() && quadplane.rtl_mode == 1 &&
             (nav_controller->reached_loiter_target() ||
              location_passed_point(current_loc, prev_WP_loc, next_WP_loc) ||
-             auto_state.wp_distance < qrtl_radius) &&
+             auto_state.wp_distance < MAX(qrtl_radius, quadplane.stopping_distance())) &&
             AP_HAL::millis() - last_mode_change_ms > 1000) {
+            /*
+              for a quadplane in RTL mode we switch to QRTL when we
+              are within the maximum of the stopping distance and the
+              RTL_RADIUS
+             */
             set_mode(QRTL, MODE_REASON_UNKNOWN);
             break;
         } else if (g.rtl_autoland == 1 &&

ArduPlane/quadplane.cpp

@@ -406,6 +406,23 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand
     AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
 
+    AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
+    
+    AP_GROUPEND
+};
+
+// second table of user settable parameters for quadplanes, this
+// allows us to go beyond the 64 parameter limit
+const AP_Param::GroupInfo QuadPlane::var_info2[] = {
+    // @Param: TRANS_DECEL
+    // @DisplayName: Transition deceleration
+    // @Description: This is deceleration rate that will be used in calculating the stopping distance when transitioning from fixed wing flight to multicopter flight.
+    // @Units: m/s/s
+    // @Increment: 0.1
+    // @Range: 0.2 5
+    // @User: Standard
+    AP_GROUPINFO("TRANS_DECEL", 1, QuadPlane, transition_decel, 2.0),
+
     AP_GROUPEND
 };
 
@@ -446,6 +463,7 @@ QuadPlane::QuadPlane(AP_AHRS_NavEKF &_ahrs) :
     ahrs(_ahrs)
 {
     AP_Param::setup_object_defaults(this, var_info);
+    AP_Param::setup_object_defaults(this, var_info2);
 }
 
 
@@ -845,8 +863,18 @@ void QuadPlane::control_hover(void)
 
 void QuadPlane::init_loiter(void)
 {
-    Vector3f stopping_point;
-    wp_nav->get_loiter_stopping_point_xy(stopping_point);
+    /*
+      calculate stopping point based on Q_TRANS_DECEL. This allows the
+      user to setup for a more or less agressive stop when entering
+      QLOITER
+     */
+    Vector3f stopping_point = inertial_nav.get_position();
+    Vector3f vel = inertial_nav.get_velocity();
+    if (!vel.is_zero()) {
+        vel.z = 0;
+        stopping_point += vel.normalized() * stopping_distance() * 100;
+    }
+    
     wp_nav->init_loiter_target(stopping_point);
 
     // initialize vertical speed and acceleration
@@ -861,6 +889,9 @@ void QuadPlane::init_loiter(void)
 
     // remember initial pitch
     loiter_initial_pitch_cd = MAX(plane.ahrs.pitch_sensor, 0);
+
+    // prevent re-init of target position
+    last_loiter_ms = AP_HAL::millis();
 }
 
 void QuadPlane::init_land(void)
@@ -2557,3 +2588,14 @@ bool QuadPlane::in_transition(void) const
         (transition_state == TRANSITION_AIRSPEED_WAIT ||
          transition_state == TRANSITION_TIMER);
 }
+
+/*
+  calculate current stopping distance for a quadplane in fixed wing flight
+ */
+float QuadPlane::stopping_distance(void)
+{
+    // use v^2/(2*accel). This is only quite approximate as the drag
+    // varies with pitch, but it gives something for the user to
+    // control the transition distance in a reasonable way
+    return plane.ahrs.groundspeed_vector().length_squared() / (2 * transition_decel);
+}

ArduPlane/quadplane.h

@@ -23,6 +23,7 @@ public:
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
+    static const struct AP_Param::GroupInfo var_info2[];
 
     void control_run(void);
     void control_auto(const Location &loc);
@@ -221,8 +222,14 @@ private:
     void setup_defaults(void);
     void setup_defaults_table(const struct defaults_struct *defaults, uint8_t count);
 
+    // calculate a stopping distance for fixed-wing to vtol transitions
+    float stopping_distance(void);
+    
     AP_Int16 transition_time_ms;
 
+    // transition deceleration, m/s/s
+    AP_Float transition_decel;
+    
     AP_Int16 rc_speed;
 
     // min and max PWM for throttle