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: tailsitter add gain scaling options #11195

Closed
wants to merge 5 commits into from
Closed

Plane: tailsitter add gain scaling options #11195

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
32524fb
Plane: add tailsitter gain scaling interpolation
IamPete1 Apr 25, 2019
5616bfd
Plane: add tailsitter gain scaling options and logging
kd0aij Apr 25, 2019
9c4ec7d
AP_Motors: add copter tailsitter differential torque frames
kd0aij Aug 1, 2019
1b2d7cd
AC_AttitudeControl: fix argument order in tailsitter bodyframe roll i…
kd0aij May 21, 2019
7fcb444
Plane: change PID_Info.desired to .target
kd0aij Jul 25, 2019
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
78 changes: 64 additions & 14 deletions ArduPlane/quadplane.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -355,10 +355,10 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {

// @Param: TAILSIT_THSCMX
// @DisplayName: Maximum control throttle scaling value
// @Description: Maximum value of throttle scaling for tailsitter velocity scaling, reduce this value to remove low thorottle D ossilaitons
// @Description: Maximum value of throttle scaling for tailsitter velocity scaling, reduce this value to remove low throttle D oscillations
// @Range: 1 5
// @User: Standard
AP_GROUPINFO("TAILSIT_THSCMX", 3, QuadPlane, tailsitter.throttle_scale_max, 5),
AP_GROUPINFO("TAILSIT_THSCMX", 3, QuadPlane, tailsitter.throttle_scale_max, 2),

// @Param: TRIM_PITCH
// @DisplayName: Quadplane AHRS trim pitch
Expand Down Expand Up @@ -461,6 +461,36 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @User: Advanced
AP_GROUPINFO("TKOFF_ARSP_LIM", 15, QuadPlane, maximum_takeoff_airspeed, 0),

// @Param: TAILSIT_SPDMIN
// @DisplayName: Tailsitter minimum airspeed scaling
// @Description: bellow this airspeed tailsitter 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", 16, QuadPlane, tailsitter.scaling_speed_min, 10),

// @Param: TAILSIT_SPDMAX
// @DisplayName: Tailsitter maximum airspeed scaling
// @Description: above this airspeed tailsitter 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", 17, QuadPlane, tailsitter.scaling_speed_max, 20),

// @Param: TAILSIT_GSCMSK
// @DisplayName: Tailsitter gain scaling mask
// @Description: Bitmask of gain scaling methods to be applied: BOOST: boost gain at low throttle, ATT_THR: reduce gain at high throttle/tilt, INTERP: interpolate between fixed-wing and copter controls
// @User: Standard
// @Bitmask: 1:BOOST,2:ATT_THR,4:INTERP
AP_GROUPINFO("TAILSIT_GSCMSK", 18, QuadPlane, tailsitter.gain_scaling_mask, TAILSITTER_GSCL_BOOST),

// @Param: TAILSIT_GSCMIN
// @DisplayName: Minimum gain scaling based on throttle and attitude
// @Description: Minimum gain scaling at high throttle/tilt angle
// @Range: 0.1 1
// @User: Standard
AP_GROUPINFO("TAILSIT_GSCMIN", 19, QuadPlane, tailsitter.gain_scaling_min, 0.4),

AP_GROUPEND
};

Expand Down Expand Up @@ -802,21 +832,42 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
check_attitude_relax();

// tailsitter-only bodyframe roll control options
// Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw.
if (is_tailsitter()) {
const float euler_pitch = plane.nav_pitch_cd * .01f;

int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max);
// separate limit for tailsitter roll, if set
if (plane.quadplane.tailsitter.max_roll_angle > 0) {
roll_limit = plane.quadplane.tailsitter.max_roll_angle * 100.0f;
}
float roll_rate_limit_cds = plane.quadplane.yaw_rate_max * 100.0f;

float bf_yaw_cds = constrain_float(plane.nav_roll_cd, -roll_rate_limit_cds, roll_rate_limit_cds);
float bf_roll_cd = constrain_float(yaw_rate_cds, -roll_limit, roll_limit);
if (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M) {
// 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
// act in the tailsitter's body frame (i.e. roll is MC/earth frame yaw and
// yaw is MC/earth frame roll)
attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(plane.nav_roll_cd,
// If pitch is small (nose vertical) use roll rate limit for MC yaw rate and roll limit for MC roll angle
if (fabsf(euler_pitch) < 30.0f) {
float yaw_input_scale = roll_rate_limit_cds / roll_limit;
bf_yaw_cds = constrain_float(yaw_input_scale * plane.nav_roll_cd, -roll_rate_limit_cds, roll_rate_limit_cds);
bf_roll_cd = constrain_float(yaw_rate_cds, -roll_limit, roll_limit);
}
// multicopter style: rudder stick controls bodyframe roll when hovering
attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(bf_yaw_cds,
plane.nav_pitch_cd,
yaw_rate_cds);
bf_roll_cd);
return;

} else if (tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P) {
attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(plane.nav_roll_cd,
// If pitch is small (nose vertical) use roll rate limit for roll rate and roll limit for bf yaw angle
if (fabsf(euler_pitch) < 30.0f) {
bf_yaw_cds = constrain_float(plane.nav_roll_cd, -roll_limit, roll_limit);
bf_roll_cd = constrain_float(yaw_rate_cds, -roll_rate_limit_cds, roll_rate_limit_cds);
}
// plane style: rudder stick controls bodyframe yaw when hovering
attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(bf_yaw_cds,
plane.nav_pitch_cd,
yaw_rate_cds);
bf_roll_cd);
return;
}
}
Expand Down Expand Up @@ -1014,9 +1065,8 @@ void QuadPlane::control_qacro(void)
float target_yaw = 0;
if (is_tailsitter()) {
// Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw
// acro_roll_rate param applies to yaw in copter frame
target_roll = plane.channel_rudder->norm_input() * acro_roll_rate * 100.0f;
target_yaw = -plane.channel_roll->norm_input() * acro_yaw_rate * 100.0f;
target_roll = plane.channel_rudder->norm_input() * acro_yaw_rate * 100.0f;
target_yaw = -plane.channel_roll->norm_input() * acro_roll_rate * 100.0f;
} else {
target_roll = plane.channel_roll->norm_input() * acro_roll_rate * 100.0f;
target_yaw = plane.channel_rudder->norm_input() * acro_yaw_rate * 100.0;
Expand Down
22 changes: 18 additions & 4 deletions ArduPlane/quadplane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,7 @@ class QuadPlane

// create outputs for tailsitters
void tailsitter_output(void);
void scale_control_surfaces(float& fw_aileron, float& fw_elevator, float& fw_rudder, float& aileron, float& elevator, float& rudder);

// handle different tailsitter input types
void tailsitter_check_input(void);
Expand All @@ -124,9 +125,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_thr_att_gain_scaling(void);

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

Expand Down Expand Up @@ -450,7 +451,13 @@ class QuadPlane
TAILSITTER_MASK_THROTTLE = 4,
TAILSITTER_MASK_RUDDER = 8,
};


enum tailsitter_gscl_mask {
TAILSITTER_GSCL_BOOST = 1,
TAILSITTER_GSCL_ATT_THR = 2,
TAILSITTER_GSCL_INTERP = 4,
};

// tailsitter control variables
struct {
AP_Int8 transition_angle;
Expand All @@ -461,10 +468,17 @@ class QuadPlane
AP_Float vectored_hover_gain;
AP_Float vectored_hover_power;
AP_Float throttle_scale_max;
AP_Float gain_scaling_min;
AP_Float max_roll_angle;
AP_Int16 motor_mask;
AP_Float scaling_speed_min;
AP_Float scaling_speed_max;
AP_Int8 gain_scaling_mask;
} tailsitter;

// tailsitter speed scaler
float last_spd_scaler = 1.0f;

// the attitude view of the VTOL attitude controller
AP_AHRS_View *ahrs_view;

Expand Down
Loading