Plane: smoother tailsitter transitions

this smooths out tailsitter transitions in both directions, adding a
new FW to VTOL transition state

ArduPlane/Attitude.cpp

@@ -364,15 +364,25 @@ void Plane::stabilize()
     }
     float speed_scaler = get_speed_scaler();
 
+    if (quadplane.in_tailsitter_vtol_transition()) {
+        /*
+          during transition to vtol in a tailsitter try to raise the
+          nose rapidly while keeping the wings level
+         */
+        nav_pitch_cd = constrain_float((quadplane.tailsitter.transition_angle+5)*100, 5500, 8500),
+        nav_roll_cd = 0;
+    }
+    
     if (control_mode == TRAINING) {
         stabilize_training(speed_scaler);
     } else if (control_mode == ACRO) {
         stabilize_acro(speed_scaler);
-    } else if (control_mode == QSTABILIZE ||
+    } else if ((control_mode == QSTABILIZE ||
                 control_mode == QHOVER ||
                 control_mode == QLOITER ||
                 control_mode == QLAND ||
-               control_mode == QRTL) {
+                control_mode == QRTL) &&
+               !quadplane.in_tailsitter_vtol_transition()) {
         quadplane.control_run();
     } else {
         if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {

ArduPlane/quadplane.cpp

@@ -1240,9 +1240,9 @@ void QuadPlane::update_transition(void)
     }
 
     if (is_tailsitter()) {
-        if (transition_state == TRANSITION_ANGLE_WAIT &&
-            tailsitter_transition_complete()) {
-            gcs().send_text(MAV_SEVERITY_INFO, "Transition done");
+        if (transition_state == TRANSITION_ANGLE_WAIT_FW &&
+            tailsitter_transition_fw_complete()) {
+            gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done");
             transition_state = TRANSITION_DONE;
         }
     }
@@ -1330,19 +1330,25 @@ void QuadPlane::update_transition(void)
         break;
     }
 
-    case TRANSITION_ANGLE_WAIT: {
+    case TRANSITION_ANGLE_WAIT_FW: {
         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,
+        plane.nav_pitch_cd = constrain_float(-(tailsitter.transition_angle+5)*100, -8500, -2500);
+        plane.nav_roll_cd = 0;
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, 
+                                                                      plane.nav_pitch_cd,
                                                                       0,
                                                                       smoothing_gain);
-        attitude_control->set_throttle_out(1.0f, true, 0);
+        attitude_control->set_throttle_out(motors->get_throttle_hover(), true, 0);
         run_rate_controller();
         motors_output();
         break;
     }
 
+    case TRANSITION_ANGLE_WAIT_VTOL:
+        // nothing to do, this is handled in the fw attitude controller
+        break;
+
     case TRANSITION_DONE:
         if (!tilt.motors_active && !is_tailsitter()) {
             motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
@@ -1394,6 +1400,8 @@ void QuadPlane::update(void)
     if (!in_vtol_mode()) {
         update_transition();
     } else {
+        uint32_t now = AP_HAL::millis();
+
         assisted_flight = false;
         
         // give full authority to attitude control
@@ -1404,18 +1412,52 @@ void QuadPlane::update(void)
 
         // output to motors
         motors_output();
+
+        if (now - last_vtol_mode_ms > 1000 && is_tailsitter()) {
+            /*
+              we are just entering a VTOL mode as a tailsitter, set
+              our transition state so the fixed wing controller brings
+              the nose up before we start trying to fly as a
+              multicopter
+             */
+            transition_state = TRANSITION_ANGLE_WAIT_VTOL;
+            transition_start_ms = now;
+        } else if (is_tailsitter() &&
+                   transition_state == TRANSITION_ANGLE_WAIT_VTOL) {
+            if (tailsitter_transition_vtol_complete()) {
+                /*
+                  we have completed transition to VTOL as a tailsitter,
+                  setup for the back transition when needed
+                */
+                gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done");
+                transition_state = TRANSITION_ANGLE_WAIT_FW;
+                transition_start_ms = now;
+            }
+        } else {
+            /*
+              setup the transition state appropriately for next time we go into a non-VTOL mode
+            */
             transition_start_ms = 0;
             if (throttle_wait && !plane.is_flying()) {
                 transition_state = TRANSITION_DONE;
             } else if (is_tailsitter()) {
-            transition_state = TRANSITION_ANGLE_WAIT;
-            transition_start_ms = AP_HAL::millis();
+                /*
+                  setup for the transition back to fixed wing for later
+                 */
+                transition_state = TRANSITION_ANGLE_WAIT_FW;
+                transition_start_ms = now;
             } else {
+                /*
+                  setup for airspeed wait for later
+                 */
                 transition_state = TRANSITION_AIRSPEED_WAIT;
             }
             last_throttle = motors->get_throttle();
         }
             
+        last_vtol_mode_ms = now;        
+    }
+
     // disable throttle_wait when throttle rises above 10%
     if (throttle_wait &&
         (plane.channel_throttle->get_control_in() > 10 ||
@@ -1494,6 +1536,15 @@ void QuadPlane::motors_output(void)
         return;
     }
 
+    if (in_tailsitter_vtol_transition()) {
+        /*
+          don't run the motor outputs while in tailsitter->vtol
+          transition. That is taken care of by the fixed wing
+          stabilisation code
+         */
+        return;
+    }
+
     // see if motors should be shut down
     check_throttle_suppression();
     
@@ -1626,7 +1677,7 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state)
 /*
   are we in a VTOL auto state?
  */
-bool QuadPlane::in_vtol_auto(void)
+bool QuadPlane::in_vtol_auto(void) const
 {
     if (!enable) {
         return false;
@@ -1659,7 +1710,7 @@ bool QuadPlane::in_vtol_auto(void)
 /*
   are we in a VTOL mode?
  */
-bool QuadPlane::in_vtol_mode(void)
+bool QuadPlane::in_vtol_mode(void) const
 {
     if (!enable) {
         return false;

ArduPlane/quadplane.h

@@ -55,14 +55,19 @@ public:
     */
     bool in_transition(void) const;
 
+    /*
+      return true if we are a tailsitter transitioning to VTOL flight
+    */
+    bool in_tailsitter_vtol_transition(void) const;
+    
     bool handle_do_vtol_transition(enum MAV_VTOL_STATE state);
 
     bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
     bool do_vtol_land(const AP_Mission::Mission_Command& cmd);
     bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
     bool verify_vtol_land(void);
-    bool in_vtol_auto(void);
-    bool in_vtol_mode(void);
+    bool in_vtol_auto(void) const;
+    bool in_vtol_mode(void) const;
 
     // vtol help for is_flying()
     bool is_flying(void);
@@ -80,7 +85,7 @@ public:
     bool is_flying_vtol(void);
 
     // return true when tailsitter frame configured
-    bool is_tailsitter(void);
+    bool is_tailsitter(void) const;
 
     // return true when flying a tailsitter in VTOL
     bool tailsitter_active(void);
@@ -91,8 +96,11 @@ public:
     // handle different tailsitter input types
     void tailsitter_check_input(void);
     
-    // check if we have completed transition
-    bool tailsitter_transition_complete(void);
+    // check if we have completed transition to fixed wing
+    bool tailsitter_transition_fw_complete(void);
+
+    // check if we have completed transition to vtol
+    bool tailsitter_transition_vtol_complete(void) const;
     
     // user initiated takeoff for guided mode
     bool do_user_takeoff(float takeoff_altitude);
@@ -294,7 +302,8 @@ private:
     enum {
         TRANSITION_AIRSPEED_WAIT,
         TRANSITION_TIMER,
-        TRANSITION_ANGLE_WAIT,
+        TRANSITION_ANGLE_WAIT_FW,
+        TRANSITION_ANGLE_WAIT_VTOL,
         TRANSITION_DONE
     } transition_state;
 
@@ -400,6 +409,9 @@ private:
     uint32_t last_pidz_active_ms;
     uint32_t last_pidz_init_ms;
 
+    // time when we were last in a vtol control mode
+    uint32_t last_vtol_mode_ms;
+    
     void tiltrotor_slew(float tilt);
     void tiltrotor_binary_slew(bool forward);
     void tiltrotor_update(void);

ArduPlane/tailsitter.cpp

@@ -21,7 +21,7 @@
 /*
   return true when flying a tailsitter
  */
-bool QuadPlane::is_tailsitter(void)
+bool QuadPlane::is_tailsitter(void) const
 {
     return available() && frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER;
 }
@@ -37,8 +37,8 @@ bool QuadPlane::tailsitter_active(void)
     if (in_vtol_mode()) {
         return true;
     }
-    // check if we are in ANGLE_WAIT transition
-    if (transition_state == TRANSITION_ANGLE_WAIT) {
+    // check if we are in ANGLE_WAIT fixed wing transition
+    if (transition_state == TRANSITION_ANGLE_WAIT_FW) {
         return true;
     }
     return false;
@@ -52,7 +52,7 @@ void QuadPlane::tailsitter_output(void)
     if (!is_tailsitter()) {
         return;
     }
-    if (!tailsitter_active()) {
+    if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
         if (tailsitter.vectored_forward_gain > 0) {
             // thrust vectoring in fixed wing flight
             float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
@@ -65,6 +65,19 @@ void QuadPlane::tailsitter_output(void)
             SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 0);
             SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 0);
         }
+        if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying() && hal.util->get_soft_armed()) {
+            /*
+              during transitions to vtol mode set the throttle to the
+              hover throttle, and set the altitude controller
+              integrator to the same throttle level
+             */
+            uint8_t throttle = motors->get_throttle_hover() * 100;
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle);
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle);
+            SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
+            pid_accel_z.set_integrator(throttle*10);
+        }
         return;
     }
 
@@ -120,14 +133,37 @@ void QuadPlane::tailsitter_output(void)
 /*
   return true when we have completed enough of a transition to switch to fixed wing control
  */
-bool QuadPlane::tailsitter_transition_complete(void)
+bool QuadPlane::tailsitter_transition_fw_complete(void)
 {
     if (plane.fly_inverted()) {
         // transition immediately
         return true;
     }
+    int32_t roll_cd = labs(ahrs_view->roll_sensor);
+    if (roll_cd > 9000) {
+        roll_cd = 18000 - roll_cd;
+    }
     if (labs(ahrs_view->pitch_sensor) > tailsitter.transition_angle*100 ||
-        labs(ahrs_view->roll_sensor) > tailsitter.transition_angle*100 ||
+        roll_cd > tailsitter.transition_angle*100 ||
+        AP_HAL::millis() - transition_start_ms > 2000) {
+        return true;
+    }
+    // still waiting
+    return false;
+}
+
+
+/*
+  return true when we have completed enough of a transition to switch to VTOL control
+ */
+bool QuadPlane::tailsitter_transition_vtol_complete(void) const
+{
+    if (plane.fly_inverted()) {
+        // transition immediately
+        return true;
+    }
+    if (labs(plane.ahrs.pitch_sensor) > tailsitter.transition_angle*100 ||
+        labs(plane.ahrs.roll_sensor) > tailsitter.transition_angle*100 ||
         AP_HAL::millis() - transition_start_ms > 2000) {
         return true;
     }
@@ -150,3 +186,11 @@ void QuadPlane::tailsitter_check_input(void)
         plane.channel_rudder->set_control_in(-roll_in);
     }
 }
+
+/*
+  return true if we are a tailsitter transitioning to VTOL flight  
+ */
+bool QuadPlane::in_tailsitter_vtol_transition(void) const
+{
+    return is_tailsitter() && in_vtol_mode() && transition_state == TRANSITION_ANGLE_WAIT_VTOL;
+}