Plane: allow Qassist for tailsitters

ArduPlane/quadplane.cpp

@@ -827,52 +827,53 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
 {
     check_attitude_relax();
 
-    // 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() &&
-        (tailsitter.input_type & TAILSITTER_INPUT_BF_ROLL)) {
-
-        if (!(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) {
-            // In multicopter input mode, the roll and yaw stick axes are independent of pitch
-            attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(false,
-                                                                             plane.nav_roll_cd,
-                                                                             plane.nav_pitch_cd,
-                                                                             yaw_rate_cds);
-            return;
-        } else {
-            // In plane input mode, the roll and yaw sticks are swapped
-            // and their effective axes rotate from yaw to roll and vice versa
-            // as pitch goes from zero to 90.
-            // So it is necessary to also rotate their scaling.
-
-            // Get the roll angle and yaw rate limits
-            int16_t roll_limit = aparm.angle_max;
-            // separate limit for tailsitter roll, if set
-            if (tailsitter.max_roll_angle > 0) {
-                roll_limit = tailsitter.max_roll_angle * 100.0f;
+    // 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() &&
+            (tailsitter.input_type & TAILSITTER_INPUT_BF_ROLL)) {
+
+            if (!(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) {
+                // In multicopter input mode, the roll and yaw stick axes are independent of pitch
+                attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(false,
+                                                                                plane.nav_roll_cd,
+                                                                                plane.nav_pitch_cd,
+                                                                                yaw_rate_cds);
+                return;
+            } else {
+                // In plane input mode, the roll and yaw sticks are swapped
+                // and their effective axes rotate from yaw to roll and vice versa
+                // as pitch goes from zero to 90.
+                // So it is necessary to also rotate their scaling.
+
+                // Get the roll angle and yaw rate limits
+                int16_t roll_limit = aparm.angle_max;
+                // separate limit for tailsitter roll, if set
+                if (tailsitter.max_roll_angle > 0) {
+                    roll_limit = tailsitter.max_roll_angle * 100.0f;
+                }
+                // Prevent a divide by zero
+                float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f;
+                float yaw2roll_scale = roll_limit / yaw_rate_limit;
+
+                // Rotate as a function of Euler pitch and swap roll/yaw
+                float euler_pitch = radians(.01f * plane.nav_pitch_cd);
+                float spitch = fabsf(sinf(euler_pitch));
+                float y2r_scale = linear_interpolate(1, yaw2roll_scale, spitch, 0, 1);
+
+                float p_yaw_rate = plane.nav_roll_cd / y2r_scale;
+                float p_roll_angle = -y2r_scale * yaw_rate_cds;
+
+                attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(true,
+                                                                                p_roll_angle,
+                                                                                plane.nav_pitch_cd,
+                                                                                p_yaw_rate);
+                return;
             }
-            // Prevent a divide by zero
-            float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f;
-            float yaw2roll_scale = roll_limit / yaw_rate_limit;
-
-            // Rotate as a function of Euler pitch and swap roll/yaw
-            float euler_pitch = radians(.01f * plane.nav_pitch_cd);
-            float spitch = fabsf(sinf(euler_pitch));
-            float y2r_scale = linear_interpolate(1, yaw2roll_scale, spitch, 0, 1);
-
-            float p_yaw_rate = plane.nav_roll_cd / y2r_scale;
-            float p_roll_angle = -y2r_scale * yaw_rate_cds;
-
-            attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(true,
-                                                                             p_roll_angle,
-                                                                             plane.nav_pitch_cd,
-                                                                             p_yaw_rate);
-            return;
         }
-    }
 
-    // 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,7 +882,12 @@ 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;
-        attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds);
+        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);
+        }
     }
 }
 
@@ -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) ||
-                                                         plane.get_throttle_input()>0 ||
-                                                         plane.is_flying());
+    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

@@ -26,11 +26,20 @@
  */
 bool QuadPlane::is_tailsitter(void) const
 {
-    return available() 
+    return available()
         && ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (tailsitter.motor_mask != 0))
         && (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()
-    motors_output(false);
+    // 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();