Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

plane: tail-sitter interpolate gains with airspeed and enable Q_Assist #9167

Closed
wants to merge 2 commits into from
Closed

plane: tail-sitter interpolate gains with airspeed and enable Q_Assist #9167

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8c481c5
plane: add params for gain interpolation and enable Qassist for tails…
IamPete1 Sep 25, 2018
6ae8484
plane: tail sitter add gain interpolation with airspeed
IamPete1 Sep 25, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 32 additions & 10 deletions ArduPlane/quadplane.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -409,6 +409,22 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @Bitmask: 0:Motor 1,1:Motor 2,2:Motor 3,3:Motor 4, 4:Motor 5,5:Motor 6,6:Motor 7,7:Motor 8
AP_GROUPINFO("TAILSIT_MOTMX", 9, QuadPlane, tailsitter.motor_mask, 0),

// @Param: TAILSIT_SPDMIN
// @DisplayName: Tailsitter minimum airspeed scaling
// @Description: bellow this airspeed tailsiter is controlled by copter gains, gains scaled linearly between TAILSIT_SPDMIN and TAILSIT_SPDMAX, this apply s in Q modes and during Q assist if enabled
// @Units: m/s
// @Range: 0 50
// @User: Standard
AP_GROUPINFO("TAILSIT_SPDMIN", 10, QuadPlane, tailsitter.scaling_speed_min, 10),

// @Param: TAILSIT_SPDMAX
// @DisplayName: Tailsitter maximum airspeed scaling
// @Description: above this airspeed tailsiter is controlled by plane gains, gains scaled linearly between TAILSIT_SPDMIN and TAILSIT_SPDMAX, this apply s in Q modes and during Q assist if enabled
// @Units: m/s
// @Range: 0 50
// @User: Standard
AP_GROUPINFO("TAILSIT_SPDMAX", 11, QuadPlane, tailsitter.scaling_speed_max, 20),

AP_GROUPEND
};

Expand Down Expand Up @@ -737,8 +753,8 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
{
check_attitude_relax();

if (in_vtol_mode() || is_tailsitter()) {
if (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL) {
if (in_vtol_mode()) {
if (is_tailsitter() && tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL) {
// Angle mode attitude control for pitch and body-frame roll, rate control for yaw.
// this version interprets the first argument as yaw rate and the third as roll angle
// because it is intended to be used with Q_TAILSIT_INPUT=1 where the roll and yaw sticks
Expand All @@ -757,7 +773,12 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
// use the fixed wing desired rates
float roll_rate = plane.rollController.get_pid_info().desired;
float pitch_rate = plane.pitchController.get_pid_info().desired;
attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds);
if (is_tailsitter()) {
// tailsitter roll and yaw swapped due to change in reference frame
attitude_control->input_rate_bf_roll_pitch_yaw_2(yaw_rate_cds, pitch_rate*100.0f, -roll_rate*100.0f);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds);
}
}
}

Expand Down Expand Up @@ -1391,18 +1412,19 @@ void QuadPlane::update_transition(void)
*/
if (have_airspeed &&
assistance_needed(aspeed) &&
!is_tailsitter() &&
hal.util->get_soft_armed() &&
((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
plane.get_throttle_input()>0 ||
plane.is_flying())) {
// the quad should provide some assistance to the plane
if (transition_state != TRANSITION_AIRSPEED_WAIT) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this change seems to have no effect, why change it?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

to enable assisted flight for tail sitters without entering airspeed wait transition state

gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed);
}
transition_state = TRANSITION_AIRSPEED_WAIT;
if (transition_start_ms == 0) {
transition_start_ms = now;
if (!is_tailsitter()) {
if (transition_state != TRANSITION_AIRSPEED_WAIT) {
gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed);
}
transition_state = TRANSITION_AIRSPEED_WAIT;
if (transition_start_ms == 0) {
transition_start_ms = now;
}
}
assisted_flight = true;
} else {
Expand Down
8 changes: 5 additions & 3 deletions ArduPlane/quadplane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,9 +111,9 @@ class QuadPlane
// check if we have completed transition to vtol
bool tailsitter_transition_vtol_complete(void) const;

// account for surface speed scaling in hover
void tailsitter_speed_scaling(void);
// calculate speed scaler of control surfaces in VTOL modes
float get_tailsitter_speed_scaling(void);

// user initiated takeoff for guided mode
bool do_user_takeoff(float takeoff_altitude);

Expand Down Expand Up @@ -434,6 +434,8 @@ class QuadPlane
AP_Float throttle_scale_max;
AP_Float max_roll_angle;
AP_Int16 motor_mask;
AP_Float scaling_speed_min;
AP_Float scaling_speed_max;
} tailsitter;

// the attitude view of the VTOL attitude controller
Expand Down
201 changes: 113 additions & 88 deletions ArduPlane/tailsitter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,27 +56,27 @@ void QuadPlane::tailsitter_output(void)
return;
}

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()) {
// 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);
// record plane outputs in case they are needed for interpolation
float fw_aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
float fw_elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
float fw_rudder = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder);

// thrust vectoring in fixed wing flight
float fw_tilt_left = 0;
float fw_tilt_right = 0;
if (tailsitter.vectored_forward_gain > 0) {
fw_tilt_left = (fw_elevator + fw_aileron) * tailsitter.vectored_forward_gain;
fw_tilt_right = (fw_elevator - fw_aileron) * tailsitter.vectored_forward_gain;
}

if ((!tailsitter_active() || in_tailsitter_vtol_transition()) && !assisted_flight) {
// output tilts for forward flight
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, fw_tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, fw_tilt_right);

// get FW controller throttle demand and mask of motors enabled during forward flight
// 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()) {
if (in_tailsitter_vtol_transition() && !throttle_wait && is_flying() && hal.util->get_soft_armed()) {
/*
during transitions to vtol mode set the throttle to
hover thrust, center the rudder and set the altitude controller
Expand All @@ -85,44 +85,49 @@ void QuadPlane::tailsitter_output(void)
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 (mask == 0) {
// 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 (mask != 0) {
if (tailsitter.motor_mask != 0) {
// set AP_MotorsMatrix throttles enabled for forward flight
motors->output_motor_mask(throttle * 0.01f, mask, plane.rudder_dt);
}
motors->output_motor_mask(throttle * 0.01f, tailsitter.motor_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()
motors_output(false);

if (assisted_flight) {
control_stabilize();
motors_output(true);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We're calling motors_output more than once in each pass through set_servos, right?
The option to not run the rate controller is to make the multiple calls less inefficient?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is for Q-assist, it we're in a normal vtol mode this is already done in the quadplane code, altough i'm not sure why I need to call control stabalise again

} else {
motors_output(false);
}

// if in Q assist still a good idea to use copter I term and zero plane I to prevent windup
plane.pitchController.reset_I();
plane.rollController.reset_I();

// pull in copter control outputs
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, (motors->get_yaw())*-SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, (motors->get_pitch())*SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, (motors->get_roll())*SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, (motors->get_throttle()) * 100);
float aileron = motors->get_yaw()*-SERVO_MAX;
float elevator = motors->get_pitch()*SERVO_MAX;
float rudder = motors->get_roll()*SERVO_MAX;
float throttle = motors->get_throttle() * 100;
float tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);
float tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight);

// scale surfaces for throttle
if (hal.util->get_soft_armed()) {
// scale surfaces for throttle
tailsitter_speed_scaling();
float scaling = get_tailsitter_speed_scaling();
rudder = constrain_float(rudder * scaling, -SERVO_MAX, SERVO_MAX);
aileron = constrain_float(aileron * scaling, -SERVO_MAX, SERVO_MAX);
elevator = constrain_float(elevator * scaling, -SERVO_MAX, SERVO_MAX);
tilt_left = constrain_float(tilt_left * scaling, -SERVO_MAX, SERVO_MAX);
tilt_right = constrain_float(tilt_right * scaling, -SERVO_MAX, SERVO_MAX);
}

if (tailsitter.vectored_hover_gain > 0) {
// thrust vectoring VTOL modes
tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);
tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight);
/*
apply extra elevator when at high pitch errors, using a
power law. This allows the motors to point straight up for
Expand All @@ -131,36 +136,78 @@ void QuadPlane::tailsitter_output(void)
int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5;
float extra_pitch = constrain_float(pitch_error_cd, -SERVO_MAX, SERVO_MAX) / SERVO_MAX;
float extra_sign = extra_pitch > 0?1:-1;
float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * SERVO_MAX;
// only apply extra elevator in Q modes
float extra_elevator = 0.0f;
if (!assisted_flight) {
extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * SERVO_MAX;
}
tilt_left = extra_elevator + tilt_left * tailsitter.vectored_hover_gain;
tilt_right = extra_elevator + tilt_right * tailsitter.vectored_hover_gain;
if (fabsf(tilt_left) >= SERVO_MAX || fabsf(tilt_right) >= SERVO_MAX) {
// prevent integrator windup
motors->limit.roll_pitch = 1;
motors->limit.yaw = 1;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
}

// apply speed scaling to interpolate between fixed wing and VTOL outputs based on airspeed
float aspeed;
bool have_airspeed = ahrs.airspeed_estimate(&aspeed);
const float scaling_range = tailsitter.scaling_speed_max - tailsitter.scaling_speed_min;
// only bother if it will change the output
if (aspeed > tailsitter.scaling_speed_min && have_airspeed && !is_zero(scaling_range)) {

// match the Q rates with plane controller
if (!assisted_flight) {
// no fixed wing yaw controller so cannot stabilize VTOL roll
const float pitch_rate = attitude_control->get_rate_pitch_pid().get_pid_info().desired * 100;
const float yaw_rate = attitude_control->get_rate_yaw_pid().get_pid_info().desired * 100;
const float speed_scaler = plane.get_speed_scaler();

// due to reference frame change roll and yaw are swapped, use roll as rudder input and output direct as with plane
fw_aileron = plane.rollController.get_rate_out(-yaw_rate, speed_scaler);
fw_elevator = plane.pitchController.get_rate_out(pitch_rate, speed_scaler);
fw_rudder = plane.channel_roll->get_control_in();
}

// calculate ratio of gains
float fw_ratio = (aspeed - tailsitter.scaling_speed_min) / scaling_range;
fw_ratio = constrain_float(fw_ratio, 0.0f, 1.0f);
const float VTOL_rato = 1.0f - fw_ratio;

// calculate interpolated outputs
aileron = aileron * VTOL_rato + fw_aileron * fw_ratio;
elevator = elevator * VTOL_rato + fw_elevator * fw_ratio;
rudder = rudder * VTOL_rato + fw_rudder * fw_ratio;
tilt_left = tilt_left * VTOL_rato + fw_tilt_left * fw_ratio;
tilt_right = tilt_right * VTOL_rato + fw_tilt_right * fw_ratio;
}

if (tailsitter.input_mask_chan > 0 &&
tailsitter.input_mask > 0 &&
RC_Channels::get_radio_in(tailsitter.input_mask_chan-1) > 1700) {
// the user is learning to prop-hang
if (tailsitter.input_mask & TAILSITTER_MASK_AILERON) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz());
aileron = plane.channel_roll->get_control_in_zero_dz();
}
if (tailsitter.input_mask & TAILSITTER_MASK_ELEVATOR) {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz());
elevator = plane.channel_pitch->get_control_in_zero_dz();
}
if (tailsitter.input_mask & TAILSITTER_MASK_THROTTLE) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
throttle = plane.get_throttle_input(true);
}
if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) {
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz());
rudder = plane.channel_rudder->get_control_in_zero_dz();
}
}

// set outputs
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
}


Expand Down Expand Up @@ -232,21 +279,23 @@ bool QuadPlane::in_tailsitter_vtol_transition(void) const
}

/*
account for speed scaling of control surfaces in VTOL modes
calculate speed scaler of control surfaces in VTOL modes
*/
void QuadPlane::tailsitter_speed_scaling(void)
float QuadPlane::get_tailsitter_speed_scaling(void)
{
const float hover_throttle = motors->get_throttle_hover();
const float throttle = motors->get_throttle();
float spd_scaler = 1;

// If throttle_scale_max is > 1, boost gains at low throttle
if (tailsitter.throttle_scale_max > 1) {
// If throttle_scale_max is => 1, boost gains at low throttle
if (tailsitter.throttle_scale_max >= 1) {
if (is_zero(throttle)) {
spd_scaler = tailsitter.throttle_scale_max;
} else {
spd_scaler = constrain_float(hover_throttle / throttle, 0, tailsitter.throttle_scale_max);
}
return spd_scaler;

} else {
// reduce gains when flying at high speed in Q modes:

Expand Down Expand Up @@ -287,39 +336,15 @@ void QuadPlane::tailsitter_speed_scaling(void)
spd_scaler *= throttle_atten;
spd_scaler = constrain_float(spd_scaler, max_atten, 1.0f);
}
}
// limit positive and negative slew rates of applied speed scaling
constexpr float posTC = 5.0f; // seconds
constexpr float negTC = 2.0f; // seconds
const float posdelta = plane.G_Dt / posTC;
const float negdelta = plane.G_Dt / negTC;
static float last_scale = 0;
static float scale = 0;
if ((spd_scaler - last_scale) > 0) {
if ((spd_scaler - last_scale) > posdelta) {
scale += posdelta;
} else {
scale = spd_scaler;
}
} else {
if ((spd_scaler - last_scale) < -negdelta) {
scale -= negdelta;
} else {
scale = spd_scaler;
}
}
last_scale = scale;

const SRV_Channel::Aux_servo_function_t functions[4] = {
SRV_Channel::Aux_servo_function_t::k_aileron,
SRV_Channel::Aux_servo_function_t::k_elevator,
SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft,
SRV_Channel::Aux_servo_function_t::k_tiltMotorRight};
for (uint8_t i=0; i<ARRAY_SIZE(functions); i++) {
int32_t v = SRV_Channels::get_output_scaled(functions[i]);
v *= scale;
v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
SRV_Channels::set_output_scaled(functions[i], v);

// limit positive and negative slew rates of applied speed scaling
constexpr float posTC = 5.0f; // seconds
constexpr float negTC = 2.0f; // seconds
const float posdelta = plane.G_Dt / posTC;
const float negdelta = plane.G_Dt / negTC;
static float last_scale = 0;
const float scale = constrain_float(spd_scaler, last_scale - negdelta, last_scale + posdelta);
last_scale = scale;
return scale;
}
}