5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00

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

View File

@ -364,15 +364,25 @@ void Plane::stabilize()
} }
float speed_scaler = get_speed_scaler(); 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) { if (control_mode == TRAINING) {
stabilize_training(speed_scaler); stabilize_training(speed_scaler);
} else if (control_mode == ACRO) { } else if (control_mode == ACRO) {
stabilize_acro(speed_scaler); stabilize_acro(speed_scaler);
} else if (control_mode == QSTABILIZE || } else if ((control_mode == QSTABILIZE ||
control_mode == QHOVER || control_mode == QHOVER ||
control_mode == QLOITER || control_mode == QLOITER ||
control_mode == QLAND || control_mode == QLAND ||
control_mode == QRTL) { control_mode == QRTL) &&
!quadplane.in_tailsitter_vtol_transition()) {
quadplane.control_run(); quadplane.control_run();
} else { } else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {

View File

@ -1242,9 +1242,9 @@ void QuadPlane::update_transition(void)
} }
if (is_tailsitter()) { if (is_tailsitter()) {
if (transition_state == TRANSITION_ANGLE_WAIT && if (transition_state == TRANSITION_ANGLE_WAIT_FW &&
tailsitter_transition_complete()) { tailsitter_transition_fw_complete()) {
gcs().send_text(MAV_SEVERITY_INFO, "Transition done"); gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done");
transition_state = TRANSITION_DONE; transition_state = TRANSITION_DONE;
} }
} }
@ -1332,19 +1332,25 @@ void QuadPlane::update_transition(void)
break; break;
} }
case TRANSITION_ANGLE_WAIT: { case TRANSITION_ANGLE_WAIT_FW: {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
assisted_flight = true; assisted_flight = true;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, plane.nav_pitch_cd = constrain_float(-(tailsitter.transition_angle+5)*100, -8500, -2500);
-(tailsitter.transition_angle+15)*100, plane.nav_roll_cd = 0;
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
0, 0,
smoothing_gain); 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(); run_rate_controller();
motors_output(); motors_output();
break; break;
} }
case TRANSITION_ANGLE_WAIT_VTOL:
// nothing to do, this is handled in the fw attitude controller
break;
case TRANSITION_DONE: case TRANSITION_DONE:
if (!tilt.motors_active && !is_tailsitter()) { if (!tilt.motors_active && !is_tailsitter()) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
@ -1396,6 +1402,8 @@ void QuadPlane::update(void)
if (!in_vtol_mode()) { if (!in_vtol_mode()) {
update_transition(); update_transition();
} else { } else {
uint32_t now = AP_HAL::millis();
assisted_flight = false; assisted_flight = false;
// give full authority to attitude control // give full authority to attitude control
@ -1406,16 +1414,50 @@ void QuadPlane::update(void)
// output to motors // output to motors
motors_output(); motors_output();
transition_start_ms = 0;
if (throttle_wait && !plane.is_flying()) { if (now - last_vtol_mode_ms > 1000 && is_tailsitter()) {
transition_state = TRANSITION_DONE; /*
} else if (is_tailsitter()) { we are just entering a VTOL mode as a tailsitter, set
transition_state = TRANSITION_ANGLE_WAIT; our transition state so the fixed wing controller brings
transition_start_ms = AP_HAL::millis(); 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 { } 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% // disable throttle_wait when throttle rises above 10%
@ -1496,6 +1538,15 @@ void QuadPlane::motors_output(void)
return; 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 // see if motors should be shut down
check_throttle_suppression(); 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? are we in a VTOL auto state?
*/ */
bool QuadPlane::in_vtol_auto(void) bool QuadPlane::in_vtol_auto(void) const
{ {
if (!enable) { if (!enable) {
return false; return false;
@ -1661,7 +1712,7 @@ bool QuadPlane::in_vtol_auto(void)
/* /*
are we in a VTOL mode? are we in a VTOL mode?
*/ */
bool QuadPlane::in_vtol_mode(void) bool QuadPlane::in_vtol_mode(void) const
{ {
if (!enable) { if (!enable) {
return false; return false;

View File

@ -54,6 +54,11 @@ public:
return true if we are in a transition to fwd flight from hover return true if we are in a transition to fwd flight from hover
*/ */
bool in_transition(void) const; 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 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 do_vtol_land(const AP_Mission::Mission_Command& cmd);
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
bool verify_vtol_land(void); bool verify_vtol_land(void);
bool in_vtol_auto(void); bool in_vtol_auto(void) const;
bool in_vtol_mode(void); bool in_vtol_mode(void) const;
// vtol help for is_flying() // vtol help for is_flying()
bool is_flying(void); bool is_flying(void);
@ -80,7 +85,7 @@ public:
bool is_flying_vtol(void); bool is_flying_vtol(void);
// return true when tailsitter frame configured // return true when tailsitter frame configured
bool is_tailsitter(void); bool is_tailsitter(void) const;
// return true when flying a tailsitter in VTOL // return true when flying a tailsitter in VTOL
bool tailsitter_active(void); bool tailsitter_active(void);
@ -91,9 +96,12 @@ public:
// handle different tailsitter input types // handle different tailsitter input types
void tailsitter_check_input(void); void tailsitter_check_input(void);
// check if we have completed transition // check if we have completed transition to fixed wing
bool tailsitter_transition_complete(void); 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 // user initiated takeoff for guided mode
bool do_user_takeoff(float takeoff_altitude); bool do_user_takeoff(float takeoff_altitude);
@ -294,7 +302,8 @@ private:
enum { enum {
TRANSITION_AIRSPEED_WAIT, TRANSITION_AIRSPEED_WAIT,
TRANSITION_TIMER, TRANSITION_TIMER,
TRANSITION_ANGLE_WAIT, TRANSITION_ANGLE_WAIT_FW,
TRANSITION_ANGLE_WAIT_VTOL,
TRANSITION_DONE TRANSITION_DONE
} transition_state; } transition_state;
@ -399,6 +408,9 @@ private:
// time when we last ran the vertical accel controller // time when we last ran the vertical accel controller
uint32_t last_pidz_active_ms; uint32_t last_pidz_active_ms;
uint32_t last_pidz_init_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_slew(float tilt);
void tiltrotor_binary_slew(bool forward); void tiltrotor_binary_slew(bool forward);

View File

@ -21,7 +21,7 @@
/* /*
return true when flying a tailsitter 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; return available() && frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER;
} }
@ -37,8 +37,8 @@ bool QuadPlane::tailsitter_active(void)
if (in_vtol_mode()) { if (in_vtol_mode()) {
return true; return true;
} }
// check if we are in ANGLE_WAIT transition // check if we are in ANGLE_WAIT fixed wing transition
if (transition_state == TRANSITION_ANGLE_WAIT) { if (transition_state == TRANSITION_ANGLE_WAIT_FW) {
return true; return true;
} }
return false; return false;
@ -52,7 +52,7 @@ void QuadPlane::tailsitter_output(void)
if (!is_tailsitter()) { if (!is_tailsitter()) {
return; return;
} }
if (!tailsitter_active()) { if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
if (tailsitter.vectored_forward_gain > 0) { if (tailsitter.vectored_forward_gain > 0) {
// thrust vectoring in fixed wing flight // thrust vectoring in fixed wing flight
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); 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_tiltMotorLeft, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 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; 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 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()) { if (plane.fly_inverted()) {
// transition immediately // transition immediately
return true; 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 || 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) { AP_HAL::millis() - transition_start_ms > 2000) {
return true; return true;
} }
@ -150,3 +186,11 @@ void QuadPlane::tailsitter_check_input(void)
plane.channel_rudder->set_control_in(-roll_in); 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;
}