Plane: tilt quadplane fixes

ArduPlane/quadplane.cpp

@@ -31,7 +31,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
 
     // @Param: TRANSITION_MS
     // @DisplayName: Transition time
-    // @Description: Transition time in milliseconds after minimum airspeed is reached
+    // @Description: Transition time in milliseconds after minimum airspeed is reached. This is not used for tilt quadplanes, in tilt quadplanes transition completion occurs when the tilting rotors are fully tilted.
     // @Units: ms
     // @Range: 500 30000
     // @User: Advanced
@@ -1486,6 +1486,7 @@ void SLT_Transition::update()
         transition_state = TRANSITION_DONE;
         transition_start_ms = 0;
         transition_low_airspeed_ms = 0;
+        quadplane.assisted_flight = false;
     }
 
     if (transition_state < TRANSITION_DONE) {
@@ -1591,15 +1592,26 @@ void SLT_Transition::update()
         // transition time, but continue to stabilize
         const uint32_t transition_timer_ms = now - transition_low_airspeed_ms;
         const float trans_time_ms = constrain_float(quadplane.transition_time_ms,500,30000);
-        if (transition_timer_ms > unsigned(trans_time_ms)) {
+
+        // note that tiltrotors complete transition when tilt is fully
+        // forward, checked above
+        if (!quadplane.tiltrotor.enabled() && transition_timer_ms > unsigned(trans_time_ms)) {
             transition_state = TRANSITION_DONE;
             in_forced_transition = false;
             transition_start_ms = 0;
             transition_low_airspeed_ms = 0;
             gcs().send_text(MAV_SEVERITY_INFO, "Transition done");
+            quadplane.assisted_flight = false;
+        } else {
+            quadplane.assisted_flight = true;
         }
 
-        float transition_scale = (trans_time_ms - transition_timer_ms) / trans_time_ms;
+        float transition_scale;
+        if (quadplane.tiltrotor.enabled()) {
+            transition_scale = 1.0 - quadplane.tiltrotor.get_tilt_completion();
+        } else {
+            transition_scale = (trans_time_ms - transition_timer_ms) / trans_time_ms;
+        }
         float throttle_scaled = last_throttle * transition_scale;
 
         // set zero throttle mix, to give full authority to
@@ -1620,7 +1632,6 @@ void SLT_Transition::update()
             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);
 
         // set desired yaw to current yaw in both desired angle and

ArduPlane/servos.cpp

@@ -56,6 +56,7 @@ void Plane::throttle_slew_limit()
     if (g.takeoff_throttle_slewrate != 0 && quadplane.in_frwd_transition()) {
         slewrate = g.takeoff_throttle_slewrate;
     }
+    SRV_Channels::set_slew_rate(SRV_Channel::k_throttle_tilt, quadplane.tiltrotor.fully_fwd()?slewrate:0, 100, G_Dt);
 #endif
     SRV_Channels::set_slew_rate(SRV_Channel::k_throttle,      slewrate, 100, G_Dt);
     SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft,  slewrate, 100, G_Dt);

ArduPlane/tiltrotor.cpp

@@ -150,6 +150,8 @@ void Tiltrotor::setup()
     }
     quadplane.transition = transition;
 
+    SRV_Channels::set_range(SRV_Channel::k_throttle_tilt, 100); // match k_throttle
+
     setup_complete = true;
 }
 
@@ -229,7 +231,9 @@ void Tiltrotor::continuous_update(void)
 
         max_change = tilt_max_change(false);
 
-        float new_throttle = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01, 0, 1);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle_tilt, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle));
+        float new_throttle = constrain_float(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_throttle_tilt)*0.01, 0, 1);
+
         if (current_tilt < get_fully_forward_tilt()) {
             current_throttle = constrain_float(new_throttle,
                                                     current_throttle-max_change,
@@ -245,7 +249,7 @@ void Tiltrotor::continuous_update(void)
         }
         if (!quadplane.motor_test.running) {
             // the motors are all the way forward, start using them for fwd thrust
-            const uint16_t mask = is_zero(current_throttle)?0U:tilt_mask.get();
+            const uint16_t mask = tilt_mask.get();
             motors->output_motor_mask(current_throttle, mask, plane.rudder_dt);
         }
         return;
@@ -363,7 +367,8 @@ void Tiltrotor::binary_update(void)
         // all the way forward and run them as a forward motor
         binary_slew(true);
 
-        float new_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01f;
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle_tilt, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle));
+        float new_throttle = constrain_float(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_throttle_tilt)*0.01, 0, 1);
         if (current_tilt >= 1) {
             const uint16_t mask = is_zero(new_throttle)?0U:tilt_mask.get();
             // the motors are all the way forward, start using them for fwd thrust
@@ -489,6 +494,9 @@ void Tiltrotor::tilt_compensate_angle(float *thrust, uint8_t num_motors, float n
             thrust[i] *= scale;
         }
     }
+    if (!fully_fwd()) {
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle_tilt, largest_tilted*100);
+    }
 }
 
 /*
@@ -769,4 +777,15 @@ bool Tiltrotor::tilt_over_max_angle(void) const
     return (current_tilt > MIN(tilt_threshold, get_forward_flight_tilt()));
 }
 
+/*
+  get a proportion of how far we are through the 2nd part of a transition
+  0 means tilt is less than Q_TILT_MAX. 1 means fully forward
+*/
+float Tiltrotor::get_tilt_completion() const
+{
+    const float tilt_threshold = (max_angle_deg/90.0f);
+    const float completion_tilt = get_forward_flight_tilt();
+    return constrain_float((current_tilt - tilt_threshold) / (completion_tilt - tilt_threshold), 0, 1);
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/tiltrotor.h

@@ -55,6 +55,10 @@ friend class Tiltrotor_Transition;
     float get_fully_forward_tilt() const;
     float get_forward_flight_tilt() const;
 
+    // get a proportion of how far we are through the 2nd part of a transition
+    // 0 means tilt is less than Q_TILT_MAX. 1 means fully forward
+    float get_tilt_completion() const;
+
     // update yaw target for tiltrotor transition
     void update_yaw_target();
 

Tools/autotest/default_params/quadplane.parm

@@ -1,6 +1,7 @@
 ARMING_RUDDER    2
 AIRSPEED_MAX    35
 AIRSPEED_MIN    13
+Q_ASSIST_SPEED   11
 ARSPD_USE        1
 AUTOTUNE_LEVEL   8
 COMPASS_OFS2_X   -0.420265
@@ -44,7 +45,6 @@ PTCH_RATE_I       0.212500
 PTCH_RATE_IMAX    0.888889
 PTCH_RATE_P       0.309954
 PTCH2SRV_RLL     1
-Q_ASSIST_SPEED   18
 Q_ENABLE         1
 RALLY_INCL_HOME  0
 RALLY_LIMIT_KM   5

Tools/autotest/quadplane.py

@@ -826,7 +826,8 @@ def QAssist(self):
         self.change_mode("FBWA")
 
         # disable stall prevention so roll angle is not limited
-        self.set_parameter("STALL_PREVENTION", 0)
+        self.set_parameters({"STALL_PREVENTION": 0,
+                             "Q_ASSIST_SPEED": 15})
 
         thr_min_pwm = self.get_parameter("Q_M_PWM_MIN")
         lim_roll_deg = self.get_parameter("ROLL_LIMIT_DEG")
@@ -1936,6 +1937,7 @@ def AHRSFlyForwardFlag(self):
         self.set_parameters({
             "LOG_DISARMED": 1,
             "LOG_REPLAY": 1,
+            "Q_ASSIST_SPEED": 15,
         })
         self.reboot_sitl()
 

libraries/SRV_Channel/SRV_Channel.h

@@ -189,6 +189,7 @@ class SRV_Channel {
         k_rcin15_mapped         = 154,
         k_rcin16_mapped         = 155,
         k_lift_release          = 156,
+        k_throttle_tilt         = 157,
         k_nr_aux_servo_functions         ///< This must be the last enum value (only add new values _before_ this one)
     } Aux_servo_function_t;