Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: smoother tailsitter transitions 

this smooths out tailsitter transitions in both directions, adding a
new FW to VTOL transition state
This commit is contained in:
Andrew Tridgell 2017-10-30 15:19:38 +11:00
parent 9709bca7b4
commit 3d20973ae2
4 changed files with 152 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/Attitude.cpp
View File

@ -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 ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL) {
} else if ((control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL) &&
!quadplane.in_tailsitter_vtol_transition()) {
quadplane.control_run();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {

87
ArduPlane/quadplane.cpp
View File

@ -1242,9 +1242,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;
}
}
@ -1332,19 +1332,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);
@ -1396,6 +1402,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
@ -1406,16 +1414,50 @@ void QuadPlane::update(void)
// output to motors
motors_output();
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();
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 {
transition_state = TRANSITION_AIRSPEED_WAIT;
/*
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()) {
/*
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_throttle = motors->get_throttle();
last_vtol_mode_ms = now;
}
// disable throttle_wait when throttle rises above 10%
@ -1496,6 +1538,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();
@ -1628,7 +1679,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;
@ -1661,7 +1712,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;

24
ArduPlane/quadplane.h
View File

@ -54,6 +54,11 @@ public:
return true if we are in a transition to fwd flight from hover
*/
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);
@ -61,8 +66,8 @@ public:
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,9 +96,12 @@ 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;
@ -399,6 +408,9 @@ private:
// time when we last ran the vertical accel controller
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);

56
ArduPlane/tailsitter.cpp
View File

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