mirror of https://github.com/ArduPilot/ardupilot
Plane: remove tailsitter VTOL transition race condition
This commit is contained in:
parent
5f67f8130b
commit
2d80dcd937
|
@ -377,7 +377,8 @@ void Plane::stabilize()
|
||||||
}
|
}
|
||||||
float speed_scaler = get_speed_scaler();
|
float speed_scaler = get_speed_scaler();
|
||||||
|
|
||||||
if (quadplane.in_tailsitter_vtol_transition()) {
|
uint32_t now = AP_HAL::millis();
|
||||||
|
if (quadplane.in_tailsitter_vtol_transition(now)) {
|
||||||
/*
|
/*
|
||||||
during transition to vtol in a tailsitter try to raise the
|
during transition to vtol in a tailsitter try to raise the
|
||||||
nose rapidly while keeping the wings level
|
nose rapidly while keeping the wings level
|
||||||
|
@ -386,7 +387,6 @@ void Plane::stabilize()
|
||||||
nav_roll_cd = 0;
|
nav_roll_cd = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t now = AP_HAL::millis();
|
|
||||||
if (now - last_stabilize_ms > 2000) {
|
if (now - last_stabilize_ms > 2000) {
|
||||||
// if we haven't run the rate controllers for 2 seconds then
|
// if we haven't run the rate controllers for 2 seconds then
|
||||||
// reset the integrators
|
// reset the integrators
|
||||||
|
@ -411,7 +411,7 @@ void Plane::stabilize()
|
||||||
control_mode == &mode_qrtl ||
|
control_mode == &mode_qrtl ||
|
||||||
control_mode == &mode_qacro ||
|
control_mode == &mode_qacro ||
|
||||||
control_mode == &mode_qautotune) &&
|
control_mode == &mode_qautotune) &&
|
||||||
!quadplane.in_tailsitter_vtol_transition()) {
|
!quadplane.in_tailsitter_vtol_transition(now)) {
|
||||||
quadplane.control_run();
|
quadplane.control_run();
|
||||||
} else {
|
} else {
|
||||||
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != &mode_stabilize) {
|
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != &mode_stabilize) {
|
||||||
|
|
|
@ -83,8 +83,8 @@ public:
|
||||||
/*
|
/*
|
||||||
return true if we are a tailsitter transitioning to VTOL flight
|
return true if we are a tailsitter transitioning to VTOL flight
|
||||||
*/
|
*/
|
||||||
bool in_tailsitter_vtol_transition(void) const;
|
bool in_tailsitter_vtol_transition(uint32_t now = 0) const;
|
||||||
|
|
||||||
bool handle_do_vtol_transition(enum MAV_VTOL_STATE state);
|
bool handle_do_vtol_transition(enum MAV_VTOL_STATE state);
|
||||||
|
|
||||||
bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
|
bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
|
||||||
|
|
|
@ -262,9 +262,19 @@ void QuadPlane::tailsitter_check_input(void)
|
||||||
/*
|
/*
|
||||||
return true if we are a tailsitter transitioning to VTOL flight
|
return true if we are a tailsitter transitioning to VTOL flight
|
||||||
*/
|
*/
|
||||||
bool QuadPlane::in_tailsitter_vtol_transition(void) const
|
bool QuadPlane::in_tailsitter_vtol_transition(uint32_t now) const
|
||||||
{
|
{
|
||||||
return is_tailsitter() && in_vtol_mode() && transition_state == TRANSITION_ANGLE_WAIT_VTOL;
|
if (!is_tailsitter() || !in_vtol_mode()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (transition_state == TRANSITION_ANGLE_WAIT_VTOL) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
if ((now != 0) && ((now - last_vtol_mode_ms) > 1000)) {
|
||||||
|
// only just come out of forward flight
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
Loading…
Reference in New Issue