Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Tailsitter: add transtion class

Browse Source
This commit is contained in:
Iampete1 2021-09-18 00:27:51 +01:00 committed by Andrew Tridgell
parent 7af1b40fb8
commit ce5b81869d
2 changed files with 211 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
ArduPlane/tailsitter.cpp
View File

@ -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

59
ArduPlane/tailsitter.h
View File

@ -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;
};