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:
parent
9709bca7b4
commit
3d20973ae2
@ -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) {
|
||||||
|
@ -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;
|
||||||
|
@ -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);
|
||||||
|
@ -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;
|
||||||
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user