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

Plane: allow Qassit for fw to vtol transision

This commit is contained in:
Peter Hall 2020-02-18 00:17:50 +00:00 committed by Andrew Tridgell
parent 87cd9a4791
commit 7688430029
3 changed files with 57 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ArduPlane/quadplane.cpp
View File

@ -1449,7 +1449,7 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const
*/
bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed)
{
if (assist_speed <= 0) {
if (assist_speed <= 0 || is_contol_surface_tailsitter()) {
// assistance disabled
in_angle_assist = false;
angle_error_start_ms = 0;
@ -1530,6 +1530,14 @@ bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed)
return ret;
}
// return true if it is safe to provide assistance
bool QuadPlane::assistance_safe()
{
return hal.util->get_soft_armed() && ( (plane.auto_throttle_mode && !plane.throttle_suppressed)
|| plane.get_throttle_input()>0
|| plane.is_flying() );
}
/*
update for transition from quadplane to fixed wing mode
*/
@ -1574,12 +1582,8 @@ void QuadPlane::update_transition(void)
/*
see if we should provide some assistance
*/
const bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
plane.get_throttle_input()>0 ||
plane.is_flying());
if (q_assist_safe &&
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE ||
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed) && !is_contol_surface_tailsitter()))) {
if (assistance_safe() && (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE ||
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) {
// the quad should provide some assistance to the plane
assisted_flight = true;
if (!is_tailsitter()) {
@ -1802,7 +1806,7 @@ void QuadPlane::update(void)
const uint32_t now = AP_HAL::millis();
assisted_flight = false;
// output to motors
motors_output();
@ -1817,6 +1821,13 @@ void QuadPlane::update(void)
transition_start_ms = now;
} else if (is_tailsitter() &&
transition_state == TRANSITION_ANGLE_WAIT_VTOL) {
float aspeed;
bool have_airspeed = ahrs.airspeed_estimate(aspeed);
// provide asistance in forward flight portion of tailsitter transision
if (assistance_safe() && (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE ||
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) {
assisted_flight = true;
}
if (tailsitter_transition_vtol_complete()) {
/*
we have completed transition to VTOL as a tailsitter,
@ -1982,7 +1993,7 @@ void QuadPlane::motors_output(bool run_rate_controller)
return;
}
if (in_tailsitter_vtol_transition()) {
if (in_tailsitter_vtol_transition() && !assisted_flight) {
/*
don't run the motor outputs while in tailsitter->vtol
transition. That is taken care of by the fixed wing

3
ArduPlane/quadplane.h
View File

@ -191,6 +191,9 @@ private:
// check for quadplane assistance needed
bool assistance_needed(float aspeed, bool have_airspeed);
// check if it is safe to provide assistance
bool assistance_safe();
// update transition handling
void update_transition(void);

66
ArduPlane/tailsitter.cpp
View File

@ -70,49 +70,51 @@ void QuadPlane::tailsitter_output(void)
float tilt_left = 0.0f;
float tilt_right = 0.0f;
uint16_t mask = tailsitter.motor_mask;
// handle forward flight modes and transition to VTOL modes
if ((!tailsitter_active() || in_tailsitter_vtol_transition()) && !assisted_flight) {
// in forward flight: set motor tilt servos and throttles using FW controller
if (tailsitter.vectored_forward_gain > 0) {
// thrust vectoring in fixed wing flight
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain;
tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
// get FW controller throttle demand and mask of motors enabled during forward flight
float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
if (hal.util->get_soft_armed()) {
if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying()) {
/*
during transitions to vtol mode set the throttle to
hover thrust, center the rudder and set the altitude controller
integrator to the same throttle level
*/
throttle = motors->get_throttle_hover() * 100;
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
pos_control->get_accel_z_pid().set_integrator(throttle*10);
if (hal.util->get_soft_armed() && in_tailsitter_vtol_transition() && !throttle_wait && is_flying()) {
/*
during transitions to vtol mode set the throttle to
hover thrust, center the rudder and set the altitude controller
integrator to the same throttle level
*/
throttle = motors->get_throttle_hover() * 100;
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
pos_control->get_accel_z_pid().set_integrator(throttle*10);
// override AP_MotorsTailsitter throttles during back transition
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);
}
// override AP_MotorsTailsitter throttles during back transition
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);
}
if (!assisted_flight) {
// set AP_MotorsMatrix throttles for forward flight
motors->output_motor_mask(throttle * 0.01f, tailsitter.motor_mask, plane.rudder_dt);
// in forward flight: set motor tilt servos and throttles using FW controller
if (tailsitter.vectored_forward_gain > 0) {
// thrust vectoring in fixed wing flight
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain;
tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
return;
}
// set AP_MotorsMatrix throttles for forward flight
motors->output_motor_mask(throttle * 0.01f, mask, plane.rudder_dt);
return;
}
// handle VTOL modes
// the MultiCopter rate controller has already been run in an earlier call
// to motors_output() from quadplane.update(), unless we are in assisted flight
if (assisted_flight && tailsitter_transition_fw_complete()) {
if (assisted_flight) {
hold_stabilize(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
motors_output(true);
} else {