Plane: Tailsitter: add transtion class

ArduPlane/tailsitter.cpp

@@ -211,6 +211,13 @@ void Tailsitter::setup()
         quadplane.options.set(quadplane.options.get() | QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO);
     }
 
+    transition = new Tailsitter_Transition(quadplane, motors, *this);
+    if (!transition) {
+        AP_BoardConfig::config_error("Unable to allocate tailsitter transition");
+    }
+    quadplane.transition = transition;
+
+    setup_complete = true;
 }
 
 /*
@@ -234,7 +241,7 @@ bool Tailsitter::active(void)
         return true;
     }
     // check if we are in ANGLE_WAIT fixed wing transition
-    if (quadplane.transition_state == QuadPlane::TRANSITION_ANGLE_WAIT_FW) {
+    if (transition->transition_state == Tailsitter_Transition::TRANSITION_ANGLE_WAIT_FW) {
         return true;
     }
     return false;
@@ -317,7 +324,7 @@ void Tailsitter::output(void)
     // the MultiCopter rate controller has already been run in an earlier call
     // to motors_output() from quadplane.update(), unless we are in assisted flight
     // tailsitter in TRANSITION_ANGLE_WAIT_FW is not really in assisted flight, its still in a VTOL mode
-    if (quadplane.assisted_flight && (quadplane.transition_state != QuadPlane::TRANSITION_ANGLE_WAIT_FW)) {
+    if (quadplane.assisted_flight && (transition->transition_state != Tailsitter_Transition::TRANSITION_ANGLE_WAIT_FW)) {
         quadplane.hold_stabilize(throttle);
         quadplane.motors_output(true);
 
@@ -426,7 +433,7 @@ bool Tailsitter::transition_fw_complete(void)
         gcs().send_text(MAV_SEVERITY_WARNING, "Transition FW done, roll error");
         return true;
     }
-    if (AP_HAL::millis() - quadplane.transition_start_ms > ((transition_angle_fw+(quadplane.transition_initial_pitch*0.01f))/transition_rate_fw)*1500) {
+    if (AP_HAL::millis() - transition->transition_start_ms > ((transition_angle_fw+(transition->transition_initial_pitch*0.01f))/transition_rate_fw)*1500) {
         gcs().send_text(MAV_SEVERITY_WARNING, "Transition FW done, timeout");
         return true;
     }
@@ -466,7 +473,7 @@ bool Tailsitter::transition_vtol_complete(void) const
         gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, roll error");
         return true;
     }
-    if (AP_HAL::millis() - quadplane.transition_start_ms >  ((trans_angle-(quadplane.transition_initial_pitch*0.01f))/transition_rate_vtol)*1500) {
+    if (AP_HAL::millis() - transition->transition_start_ms >  ((trans_angle-(transition->transition_initial_pitch*0.01f))/transition_rate_vtol)*1500) {
         gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, timeout");
         return true;
     }
@@ -496,10 +503,10 @@ bool Tailsitter::in_vtol_transition(uint32_t now) const
     if (!enabled() || !quadplane.in_vtol_mode()) {
         return false;
     }
-    if (quadplane.transition_state == QuadPlane::TRANSITION_ANGLE_WAIT_VTOL) {
+    if (transition->transition_state == Tailsitter_Transition::TRANSITION_ANGLE_WAIT_VTOL) {
         return true;
     }
-    if ((now != 0) && ((now - quadplane.last_vtol_mode_ms) > 1000)) {
+    if ((now != 0) && ((now - transition->last_vtol_mode_ms) > 1000)) {
         // only just come out of forward flight
         return true;
     }
@@ -511,7 +518,7 @@ bool Tailsitter::in_vtol_transition(uint32_t now) const
  */
 bool Tailsitter::is_in_fw_flight(void) const
 {
-    return enabled() && !quadplane.in_vtol_mode() && quadplane.transition_state == QuadPlane::TRANSITION_DONE;
+    return enabled() && !quadplane.in_vtol_mode() && transition->transition_state == Tailsitter_Transition::TRANSITION_DONE;
 }
 
 /*
@@ -660,4 +667,143 @@ void Tailsitter::speed_scaling(void)
     }
 }
 
+/*
+  update for transition from quadplane to fixed wing mode
+ */
+void Tailsitter_Transition::update()
+{
+    const uint32_t now = millis();
+
+    float aspeed;
+    bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed);
+
+    /*
+      see if we should provide some assistance
+     */
+    quadplane.assisted_flight = quadplane.should_assist(aspeed, have_airspeed);
+
+    if (transition_state == TRANSITION_ANGLE_WAIT_FW &&
+        quadplane.tailsitter.transition_fw_complete()) {
+        transition_state = TRANSITION_DONE;
+        transition_start_ms = 0;
+    }
+
+    if (transition_state < TRANSITION_DONE) {
+        // during transition we ask TECS to use a synthetic
+        // airspeed. Otherwise the pitch limits will throw off the
+        // throttle calculation which is driven by pitch
+        plane.TECS_controller.use_synthetic_airspeed();
+    }
+
+    switch (transition_state) {
+
+    case TRANSITION_ANGLE_WAIT_FW: {
+        quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+        quadplane.assisted_flight = true;
+        uint32_t dt = now - transition_start_ms;
+        // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
+        plane.nav_pitch_cd = constrain_float(transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500);
+        plane.nav_roll_cd = 0;
+        quadplane.check_attitude_relax();
+        quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
+                                                                      plane.nav_pitch_cd,
+                                                                      0);
+        // set throttle at either hover throttle or current throttle, whichever is higher, through the transition
+        quadplane.attitude_control->set_throttle_out(MAX(motors->get_throttle_hover(),quadplane.attitude_control->get_throttle_in()), true, 0);
+        break;
+    }
+
+    case TRANSITION_ANGLE_WAIT_VTOL:
+        // nothing to do, this is handled in the fixed wing attitude controller
+        return;
+
+    case TRANSITION_DONE:
+        return;
+    }
+
+    quadplane.motors_output();
+}
+
+void Tailsitter_Transition::VTOL_update()
+{
+    const uint32_t now = millis();
+
+    if (quadplane.tailsitter.enabled() && (now - last_vtol_mode_ms) > 1000) {
+        /*
+          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;
+        transition_initial_pitch = constrain_float(quadplane.ahrs.pitch_sensor,-8500,8500);
+    }
+    if (quadplane.tailsitter.enabled() &&
+               transition_state == TRANSITION_ANGLE_WAIT_VTOL) {
+        float aspeed;
+        bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed);
+        // provide asistance in forward flight portion of tailsitter transision
+        if (quadplane.should_assist(aspeed, have_airspeed)) {
+            quadplane.assisted_flight = true;
+        }
+        if (quadplane.tailsitter.transition_vtol_complete()) {
+            /*
+              we have completed transition to VTOL as a tailsitter,
+              setup for the back transition when needed
+            */
+            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 (quadplane.throttle_wait && !plane.is_flying()) {
+            transition_state = TRANSITION_DONE;
+        } else {
+            /*
+              setup for the transition back to fixed wing for later
+             */
+            transition_state = TRANSITION_ANGLE_WAIT_FW;
+            transition_start_ms = now;
+            transition_initial_pitch = constrain_float(quadplane.ahrs_view->pitch_sensor,-8500,8500);
+        }
+    }
+    last_vtol_mode_ms = now;
+}
+
+// return true if we should show VTOL view
+bool Tailsitter_Transition::show_vtol_view() const
+{
+    bool show_vtol = quadplane.in_vtol_mode();
+
+    if (show_vtol && (transition_state == TRANSITION_ANGLE_WAIT_VTOL)) {
+        // in a vtol mode but still transitioning from forward flight
+        return false;
+    }
+    if (!show_vtol && (transition_state == TRANSITION_ANGLE_WAIT_FW)) {
+        // not in VTOL mode but still transitioning from VTOL
+        return true;
+    }
+    return show_vtol;
+}
+
+void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing)
+{
+    uint32_t now = AP_HAL::millis();
+    if (tailsitter.in_vtol_transition(now)) {
+        /*
+          during transition to vtol in a tailsitter try to raise the
+          nose while keeping the wings level
+         */
+        uint32_t dt = now - transition_start_ms;
+        // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
+        nav_pitch_cd = constrain_float(transition_initial_pitch + (tailsitter.transition_rate_vtol * dt) * 0.1f, -8500, 8500);
+        nav_roll_cd = 0;
+        allow_stick_mixing = false;
+    }
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/tailsitter.h

@@ -15,8 +15,11 @@
 #pragma once
 
 #include <AP_Param/AP_Param.h>
+#include "transition.h"
+
 class QuadPlane;
 class AP_MotorsMulticopter;
+class Tailsitter_Transition;
 class Tailsitter
 {
 friend class QuadPlane;
@@ -25,7 +28,7 @@ public:
 
     Tailsitter(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors);
 
-    bool enabled() const { return enable > 0;}
+    bool enabled() const { return (enable > 0) && setup_complete;}
 
     void setup();
 
@@ -103,8 +106,62 @@ public:
 
 private:
 
+    bool setup_complete;
+
     // refences for convenience
     QuadPlane& quadplane;
     AP_MotorsMulticopter*& motors;
 
+    // transition logic
+    Tailsitter_Transition* transition;
+
 };
+
+
+// Transition for tailsitters
+class Tailsitter_Transition : public Transition
+{
+friend class Tailsitter;
+public:
+
+    Tailsitter_Transition(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors, Tailsitter& _tailsitter):Transition(_quadplane, _motors), tailsitter(_tailsitter) {};
+
+    void update() override;
+
+    void VTOL_update() override;
+
+    void force_transistion_complete() override {
+        transition_state = TRANSITION_DONE; 
+        transition_start_ms = 0;
+    };
+
+    bool complete() const override { return transition_state == TRANSITION_DONE; }
+
+    void restart() override { transition_state = TRANSITION_ANGLE_WAIT_FW; };
+
+    uint8_t get_log_transision_state() const override { return static_cast<uint8_t>(transition_state); }
+
+    bool active() const override { return transition_state != TRANSITION_DONE; }
+
+    bool show_vtol_view() const override;
+
+    void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override;
+
+private:
+
+    enum {
+        TRANSITION_ANGLE_WAIT_FW,
+        TRANSITION_ANGLE_WAIT_VTOL,
+        TRANSITION_DONE
+    } transition_state;
+
+    // timer start for transition
+    uint32_t transition_start_ms;
+    float transition_initial_pitch;
+
+    // time when we were last in a vtol control mode
+    uint32_t last_vtol_mode_ms;
+
+    Tailsitter& tailsitter;
+
+};