zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Plane: allow Qassit for fw to vtol transision

zr-v5.1
Peter Hall 5 years ago committed by Andrew Tridgell
parent
87cd9a4791
commit
7688430029
3 changed files with 57 additions and 41 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 29
      ArduPlane/quadplane.cpp
  2. 3
      ArduPlane/quadplane.h
  3. 66
      ArduPlane/tailsitter.cpp

29
ArduPlane/quadplane.cpp
Unescape View File

@ -1449,7 +1449,7 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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
Unescape View File

@ -191,6 +191,9 @@ private: @@ -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
Unescape View File

@ -70,49 +70,51 @@ void QuadPlane::tailsitter_output(void) @@ -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);
// handle forward flight modes and transition to VTOL modes
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);
// 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 (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);
}
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 {

Write Preview
Loading…
Cancel
Save