Plane: improved POSITION1 speed profile

allow acceleration up to the Q_WP_SPEED, and assume a decel profile
matching Q_TRANS_DECEL for approach speed

ArduPlane/quadplane.cpp

@@ -2591,9 +2591,6 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
 {
     state = s;
     last_state_change_ms = AP_HAL::millis();
-    if (state == QPOS_POSITION1) {
-        speed_scale = 0;
-    }
 }
 
 /*
@@ -2608,6 +2605,12 @@ void QuadPlane::vtol_position_controller(void)
     const Location &loc = plane.next_WP_loc;
     uint32_t now_ms = AP_HAL::millis();
 
+    // distance that we switch to QPOS_POSITION2
+    const float position2_dist_threshold = 5.0;
+
+    // target speed when we reach position2 threshold
+    const float position2_target_speed = 2.0;
+
     check_attitude_relax();
 
     // horizontal position control
@@ -2760,47 +2763,31 @@ void QuadPlane::vtol_position_controller(void)
 
         const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc);
         const float distance = diff_wp.length();
-        Vector2f groundspeed = ahrs.groundspeed_vector();
-        float speed_towards_target = distance>1?(diff_wp.normalized() * groundspeed):0;
-        if (poscontrol.speed_scale <= 0) {
-            // initialise scaling so we start off targeting our
-            // current linear speed towards the target. If this is
-            // less than the wpnav speed then the wpnav speed is used
-            // land_speed_scale is then used to linearly change
-            // velocity as we approach the waypoint, aiming for zero
-            // speed at the waypoint
-            // setup land_speed_scale so at current distance we
-            // maintain speed towards target, and slow down as we
-            // approach
 
-            // max_speed will control how fast we will fly. It will always decrease
+        // calculate speed we should be at to reach the position2
-            poscontrol.max_speed = MAX(speed_towards_target, wp_nav->get_default_speed_xy() * 0.01);
+        // target speed at the position2 distance threshold, assuming
-            poscontrol.speed_scale = poscontrol.max_speed / MAX(distance, 1);
+        // Q_TRANS_DECEL is correct
+        const float stopping_speed = safe_sqrt(MAX(0, distance-position2_dist_threshold) * 2 * transition_decel) + position2_target_speed;
+
+        float target_speed = stopping_speed;
+
+        // maximum configured VTOL speed
+        const float wp_speed = pos_control->get_max_speed_xy_cms() * 0.01;
+        const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared();
+
+        if (current_speed_sq < sq(wp_speed)) {
+            // if we are below the WP speed then don't ask for more
+            // than WP speed. The POSITION1 controller can be used
+            // when faster than WP speed if we are coming from fixed
+            // wing flight, but we don't want to accelerate to speeds
+            // beyond the configured max VTOL speed
+            target_speed = MIN(target_speed, wp_speed);
         }
 
         // run fixed wing navigation
         plane.nav_controller->update_waypoint(plane.current_loc, loc);
 
-        /*
+        Vector2f target_speed_xy = diff_wp.normalized() * target_speed;
-          calculate target velocity, not dropping it below 2m/s
-         */
-        const float final_speed = 2.0f;
-        Vector2f target_speed_xy = diff_wp * poscontrol.speed_scale;
-        float target_speed = target_speed_xy.length();
-        if (distance < 1) {
-            // prevent numerical error before switching to POSITION2
-            target_speed_xy = {0.1, 0.1};
-        }
-        if (target_speed < final_speed) {
-            // until we enter the loiter we always aim for at least 2m/s
-            target_speed_xy = target_speed_xy.normalized() * final_speed;
-            poscontrol.max_speed = final_speed;
-        } else if (target_speed > poscontrol.max_speed) {
-            // we never speed up during landing approaches
-            target_speed_xy = target_speed_xy.normalized() * poscontrol.max_speed;
-        } else {
-            poscontrol.max_speed = target_speed;
-        }
         pos_control->set_vel_desired_xy_cms(target_speed_xy * 100);
 
         // reset position controller xy target to current position
@@ -2841,7 +2828,7 @@ void QuadPlane::vtol_position_controller(void)
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
                                                                       plane.nav_pitch_cd,
                                                                       desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
-        if (plane.auto_state.wp_distance < 5) {
+        if (plane.auto_state.wp_distance < position2_dist_threshold) {
             poscontrol.set_state(QPOS_POSITION2);
             poscontrol.pilot_correction_done = false;
             gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f",

ArduPlane/quadplane.h

@@ -477,9 +477,6 @@ private:
         uint32_t time_since_state_start_ms() const {
             return AP_HAL::millis() - last_state_change_ms;
         }
-        float speed_scale;
-        Vector2f target_velocity;
-        float max_speed;
         Vector3f target_cm;
         Vector3f target_vel_cms;
         bool slow_descent:1;