Plane: quadplane: enhance tiltrotor transtion to better deal with tiltrotors with all motors tilting

ArduPlane/quadplane.cpp

@@ -1477,6 +1477,7 @@ void SLT_Transition::update()
         transition_low_airspeed_ms = now;
         if (have_airspeed && aspeed > plane.aparm.airspeed_min && !quadplane.assisted_flight) {
             transition_state = TRANSITION_TIMER;
+            airspeed_reached_tilt = quadplane.tiltrotor.current_tilt;
             gcs().send_text(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", (double)aspeed);
         }
         quadplane.assisted_flight = true;
@@ -1502,6 +1503,11 @@ void SLT_Transition::update()
             quadplane.attitude_control->reset_yaw_target_and_rate();
             quadplane.attitude_control->rate_bf_yaw_target(quadplane.ahrs.get_gyro().z);
         }
+        if (quadplane.tiltrotor.enabled() && !quadplane.tiltrotor.has_fw_motor()) {
+            // tilt rotors without decidated fw motors do not have forward throttle output in this stage
+            // prevent throttle I wind up
+            plane.TECS_controller.reset_throttle_I();
+        }
 
         last_throttle = motors->get_throttle();
 
@@ -1541,6 +1547,14 @@ void SLT_Transition::update()
             // will stop stabilizing
             throttle_scaled = 0.01;
         }
+        if (quadplane.tiltrotor.enabled() && !quadplane.tiltrotor.has_vtol_motor() && !quadplane.tiltrotor.has_fw_motor()) {
+            // All motors tilting, Use a combination of vertical and forward throttle based on curent tilt angle
+            // scale from all VTOL throttle at airspeed_reached_tilt to all forward throttle at fully forward tilt
+            // this removes a step change in throttle once assistance is stoped
+            const float ratio = (constrain_float(quadplane.tiltrotor.current_tilt, airspeed_reached_tilt, quadplane.tiltrotor.get_fully_forward_tilt()) - airspeed_reached_tilt) / (quadplane.tiltrotor.get_fully_forward_tilt() - airspeed_reached_tilt);
+            const float fw_throttle = MAX(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),0) * 0.01;
+            throttle_scaled = constrain_float(throttle_scaled * (1.0-ratio) + fw_throttle * ratio, 0.0, 1.0);
+        }
         quadplane.assisted_flight = true;
         quadplane.hold_stabilize(throttle_scaled);
 

ArduPlane/tiltrotor.cpp

@@ -107,6 +107,22 @@ void Tiltrotor::setup()
 
     _is_vectored = tilt_mask != 0 && type == TILT_TYPE_VECTORED_YAW;
 
+    // true if a fixed forward motor is configured, either throttle, throttle left  or throttle right.
+    // bicopter tiltrotors use throttle left and right as tilting motors, so they don't count in that case.
+    _have_fw_motor = SRV_Channels::function_assigned(SRV_Channel::k_throttle) ||
+                    ((SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) || SRV_Channels::function_assigned(SRV_Channel::k_throttleRight))
+                        && (type != TILT_TYPE_BICOPTER));
+
+
+    // check if there are any perminant VTOL motors
+    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
+        if (motors->is_motor_enabled(i) && ((tilt_mask & (1U<<1)) == 0)) {
+            // enabled motor not set in tilt mask
+            _have_vtol_motor = true;
+            break;
+        }
+    }
+
     if (quadplane.motors_var_info == AP_MotorsMatrix::var_info && _is_vectored) {
         // we will be using vectoring for yaw
         motors->disable_yaw_torque();

ArduPlane/tiltrotor.h

@@ -57,6 +57,10 @@ public:
 
     bool is_vectored() const { return enabled() && _is_vectored; }
 
+    bool has_fw_motor() const { return _have_fw_motor; }
+
+    bool has_vtol_motor() const { return _have_vtol_motor; }
+
     bool motors_active() const { return enabled() && _motors_active; }
 
     AP_Int8 enable;
@@ -90,6 +94,12 @@ private:
 
     bool setup_complete;
 
+    // true if a fixed forward motor is setup
+    bool _have_fw_motor;
+
+    // true if all motors tilt with no fixed VTOL motor
+    bool _have_vtol_motor;
+
     // refences for convenience
     QuadPlane& quadplane;
     AP_MotorsMulticopter*& motors;

ArduPlane/transition.h

@@ -110,5 +110,8 @@ protected:
     uint32_t last_fw_mode_ms;
     int32_t last_fw_nav_pitch_cd;
 
+    // tiltrotor tilt angle when airspeed wait transition stage completes
+    float airspeed_reached_tilt;
+
 };