Plane: allow Qassist for tailsitters

ArduPlane/quadplane.cpp

@@ -827,6 +827,9 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
 {
     check_attitude_relax();
 
+    // normal control modes for VTOL and FW flight
+    if (in_vtol_mode()) {
+
         // tailsitter-only body-frame roll control options
         // Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw.
         if (is_tailsitter() &&
@@ -871,8 +874,6 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
             }
         }
 
-    // normal control modes for VTOL and FW flight
-    if (in_vtol_mode() || is_tailsitter()) {
         // use euler angle attitude control
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
                                                                       plane.nav_pitch_cd,
@@ -881,8 +882,13 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
         // use the fixed wing desired rates
         float roll_rate = plane.rollController.get_pid_info().target;
         float pitch_rate = plane.pitchController.get_pid_info().target;
+        if (is_tailsitter()) {
+            // tailsitter roll and yaw swapped due to change in reference frame
+            attitude_control->input_rate_bf_roll_pitch_yaw_2(yaw_rate_cds,pitch_rate*100.0f, -roll_rate*100.0f);
+        } else {
             attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds);
         }
+    }
 }
 
 // hold in stabilize with given throttle
@@ -900,7 +906,12 @@ void QuadPlane::hold_stabilize(float throttle_in)
         }
     } else {
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-        attitude_control->set_throttle_out(throttle_in, true, 0);
+        bool should_boost = true;
+        if (is_tailsitter() && assisted_flight) {
+            // tailsitters in forward flight should not use angle boost
+            should_boost = false;
+        }
+        attitude_control->set_throttle_out(throttle_in, should_boost, 0);
     }
 }
 
@@ -1563,12 +1574,12 @@ void QuadPlane::update_transition(void)
     /*
       see if we should provide some assistance
      */
-    bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
+    const bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
                                                                plane.get_throttle_input()>0 ||
                                                                plane.is_flying());
     if (q_assist_safe &&
         (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE ||
-        (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) {
+        (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed) && !is_contol_surface_tailsitter()))) {
         // the quad should provide some assistance to the plane
         assisted_flight = true;
         if (!is_tailsitter()) {

ArduPlane/quadplane.h

@@ -110,6 +110,9 @@ public:
     // return true when tailsitter frame configured
     bool is_tailsitter(void) const;
 
+    // return true when flying a control surface only tailsitter tailsitter
+    bool is_contol_surface_tailsitter(void) const;
+
     // return true when flying a tailsitter in VTOL
     bool tailsitter_active(void);
     

ArduPlane/tailsitter.cpp

@@ -31,6 +31,15 @@ bool QuadPlane::is_tailsitter(void) const
         && (tilt.tilt_type != TILT_TYPE_BICOPTER);
 }
 
+/*
+  return true when flying a control surface only tailsitter tailsitter
+ */
+bool QuadPlane::is_contol_surface_tailsitter(void) const
+{
+    return frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER
+           && ( is_zero(tailsitter.vectored_hover_gain) || !SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft));
+}
+
 /*
   check if we are flying as a tailsitter
  */
@@ -64,7 +73,7 @@ void QuadPlane::tailsitter_output(void)
     uint16_t mask = tailsitter.motor_mask;
 
     // handle forward flight modes and transition to VTOL modes
-    if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
+    if ((!tailsitter_active() || in_tailsitter_vtol_transition()) && !assisted_flight) {
         // in forward flight: set motor tilt servos and throttles using FW controller
         if (tailsitter.vectored_forward_gain > 0) {
             // thrust vectoring in fixed wing flight
@@ -102,8 +111,14 @@ void QuadPlane::tailsitter_output(void)
 
     // handle VTOL modes
     // the MultiCopter rate controller has already been run in an earlier call
-    // to motors_output() from quadplane.update()
+    // to motors_output() from quadplane.update(), unless we are in assisted flight
+    if (assisted_flight && tailsitter_transition_fw_complete()) {
+        hold_stabilize(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
+        motors_output(true);
+    } else {
         motors_output(false);
+    }
+
     plane.pitchController.reset_I();
     plane.rollController.reset_I();