From ce5b81869d1b64d15e9df7cce0d9d98ee1572e58 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 18 Sep 2021 00:27:51 +0100 Subject: [PATCH] Plane: Tailsitter: add transtion class --- ArduPlane/tailsitter.cpp | 160 +++++++++++++++++++++++++++++++++++++-- ArduPlane/tailsitter.h | 59 ++++++++++++++- 2 files changed, 211 insertions(+), 8 deletions(-) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 2caf9383dd..89d8e0beaa 100644 --- a/ArduPlane/tailsitter.cpp +++ b/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 diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index 27b3466ce8..d6196cc74b 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -15,8 +15,11 @@ #pragma once #include +#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(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; + +}; \ No newline at end of file