Plane: initial support for tailsitter transitions

ArduPlane/ArduPlane.cpp

@@ -175,8 +175,13 @@ void Plane::ahrs_update()
     }
 
     // calculate a scaled roll limit based on current pitch
-    roll_limit_cd = aparm.roll_limit_cd * cosf(ahrs.pitch);
-    pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
+    roll_limit_cd = aparm.roll_limit_cd;
+    pitch_limit_min_cd = aparm.pitch_limit_min_cd;
+
+    if (!quadplane.tailsitter_active()) {
+        roll_limit_cd *= ahrs.cos_pitch();
+        pitch_limit_min_cd *= fabsf(ahrs.cos_roll());
+    }
 
     // updated the summed gyro used for ground steering and
     // auto-takeoff. Dot product of DCM.c with gyro vector gives earth

ArduPlane/Attitude.cpp

@@ -671,6 +671,11 @@ void Plane::update_load_factor(void)
         // no roll limits when inverted
         return;
     }
+    if (quadplane.tailsitter_active()) {
+        // no limits while hovering
+        return;
+    }
+       
 
     float max_load_factor = smoothed_airspeed / aparm.airspeed_min;
     if (max_load_factor <= 1) {

ArduPlane/GCS_Mavlink.cpp

@@ -99,13 +99,23 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
 
 void Plane::send_attitude(mavlink_channel_t chan)
 {
+    float r = ahrs.roll;
+    float p = ahrs.pitch - radians(g.pitch_trim_cd*0.01f);
+    float y = ahrs.yaw;
+    
+    if (quadplane.tailsitter_active()) {
+        r = quadplane.ahrs_view->roll;
+        p = quadplane.ahrs_view->pitch;
+        y = quadplane.ahrs_view->yaw;
+    }
+    
     const Vector3f &omega = ahrs.get_gyro();
     mavlink_msg_attitude_send(
         chan,
         millis(),
-        ahrs.roll,
-        ahrs.pitch - radians(g.pitch_trim_cd*0.01f),
-        ahrs.yaw,
+        r,
+        p,
+        y,
         omega.x,
         omega.y,
         omega.z);

ArduPlane/quadplane.cpp

@@ -334,6 +334,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @Values: 0:Continuous,1:Binary
     AP_GROUPINFO("TILT_TYPE", 47, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS),
 
+    // @Param: Q_TAILSIT_ANGLE
+    // @DisplayName: Tailsitter transition angle
+    // @Description: This is the angle at which tailsitter aircraft will change from VTOL control to fixed wing control.
+    // @Range: 5 80
+    AP_GROUPINFO("TAILSIT_ANGLE", 48, QuadPlane, tailsitter.transition_angle, 30),
+    
     AP_GROUPEND
 };
 
@@ -377,6 +383,7 @@ bool QuadPlane::setup(void)
     float loop_delta_t = 1.0 / plane.scheduler.get_loop_rate_hz();
 
     enum AP_Motors::motor_frame_class motor_class;
+    enum Rotation rotation = ROTATION_NONE;
         
     /*
       cope with upgrade from old AP_Motors values for frame_class
@@ -455,6 +462,7 @@ bool QuadPlane::setup(void)
     case AP_Motors::MOTOR_FRAME_TAILSITTER:
         motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz());
         var_info = AP_MotorsTailsitter::var_info;
+        rotation = ROTATION_PITCH_90;
         break;
     default:
         motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz());
@@ -468,13 +476,20 @@ bool QuadPlane::setup(void)
     }
 
     AP_Param::load_object_from_eeprom(motors, var_info);
-    attitude_control = new AC_AttitudeControl_Multi(ahrs, aparm, *motors, loop_delta_t);
+
+    // create the attitude view used by the VTOL code
+    ahrs_view = ahrs.create_view(rotation);
+    if (ahrs_view == nullptr) {
+        goto failed;
+    }
+    
+    attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, loop_delta_t);
     if (!attitude_control) {
         hal.console->printf("%s attitude_control\n", strUnableToAllocate);
         goto failed;
     }
     AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info);
-    pos_control = new AC_PosControl(ahrs, inertial_nav, *motors, *attitude_control,
+    pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control,
                                     p_alt_hold, p_vel_z, pid_accel_z,
                                     p_pos_xy, pi_vel_xy);
     if (!pos_control) {
@@ -1011,6 +1026,7 @@ void QuadPlane::update_transition(void)
      */
     if (have_airspeed &&
         assistance_needed(aspeed) &&
+        !is_tailsitter() &&
         (plane.auto_throttle_mode ||
          plane.channel_throttle->get_control_in()>0 ||
          plane.is_flying())) {
@@ -1025,6 +1041,14 @@ void QuadPlane::update_transition(void)
         assisted_flight = false;
     }
 
+    if (is_tailsitter()) {
+        if (transition_state == TRANSITION_ANGLE_WAIT &&
+            (ahrs_view->pitch_sensor < -tailsitter.transition_angle*100 || plane.fly_inverted())) {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition done");
+            transition_state = TRANSITION_DONE;
+        }
+    }
+    
     // if rotors are fully forward then we are not transitioning
     if (tiltrotor_fully_fwd()) {
         transition_state = TRANSITION_DONE;
@@ -1092,6 +1116,19 @@ void QuadPlane::update_transition(void)
         break;
     }
 
+    case TRANSITION_ANGLE_WAIT: {
+        motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
+        assisted_flight = true;
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 
+                                                                      -(tailsitter.transition_angle+15)*100,
+                                                                      0,
+                                                                      smoothing_gain);
+        attitude_control->set_throttle_out(1.0f, true, 0);
+        run_rate_controller();
+        motors_output();
+        break;
+    }
+        
     case TRANSITION_DONE:
         if (!tilt.motors_active && !is_tailsitter()) {
             motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
@@ -1143,6 +1180,8 @@ void QuadPlane::update(void)
         transition_start_ms = 0;
         if (throttle_wait && !plane.is_flying()) {
             transition_state = TRANSITION_DONE;
+        } else if (is_tailsitter()) {
+            transition_state = TRANSITION_ANGLE_WAIT;
         } else {
             transition_state = TRANSITION_AIRSPEED_WAIT;
         }

ArduPlane/quadplane.h

@@ -75,9 +75,12 @@ public:
     // see if we are flying from vtol point of view
     bool is_flying_vtol(void);
 
-    // return true when flying a tailsitter
+    // return true when tailsitter frame configured
     bool is_tailsitter(void);
 
+    // return true when flying a tailsitter in VTOL
+    bool tailsitter_active(void);
+    
     // create outputs for tailsitters
     void tailsitter_output(void);
     
@@ -255,6 +258,7 @@ private:
     enum {
         TRANSITION_AIRSPEED_WAIT,
         TRANSITION_TIMER,
+        TRANSITION_ANGLE_WAIT,
         TRANSITION_DONE
     } transition_state;
 
@@ -320,6 +324,14 @@ private:
         bool motors_active:1;
     } tilt;
 
+    // tailsitter control variables
+    struct {
+        AP_Int8 transition_angle;
+    } tailsitter;
+
+    // the attitude view of the VTOL attitude controller
+    AP_AHRS_View *ahrs_view;
+
     // time when motors were last active
     uint32_t last_motors_active_ms;
     

ArduPlane/tailsitter.cpp

@@ -26,12 +26,21 @@ bool QuadPlane::is_tailsitter(void)
     return available() && frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER;
 }
 
+/*
+  check if we are flying as a tailsitter
+ */
+bool QuadPlane::tailsitter_active(void)
+{
+    return is_tailsitter() && in_vtol_mode();
+}
+
 /*
   run output for tailsitters
  */
 void QuadPlane::tailsitter_output(void)
 {
-    if (is_tailsitter() && in_vtol_mode()) {
+    if (tailsitter_active()) {
         motors_output();
     }
 }
+

ArduPlane/takeoff.cpp

@@ -58,7 +58,7 @@ bool Plane::auto_takeoff_check(void)
         goto no_launch;
     }
 
-    if (!is_tailsitter()) {
+    if (!quadplane.is_tailsitter()) {
         // Check aircraft attitude for bad launch
         if (ahrs.pitch_sensor <= -3000 ||
             ahrs.pitch_sensor >= 4500 ||