Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-15 14:53:32 +08:00
Code Issues Packages Projects Releases Wiki Activity

Plane: update tailsitter speedscaling logic

Browse Source 
add tailsitter gain scaling option mask and logging
tailsitter qassist bugfix; thanks, @iampete
apply roll limit in tailsitter bodyframe roll control
add define for future exclusion of tailsitter gainscaling debug code/logging
tailsitter bodyframe roll and qacro input scaling bugfixes:
  respect Q_TAILSIT_RLL_MX and roll/yaw scale parameters in bodyframe roll modes
  fix unintended swap of Q_ACRO_RLL/YAW_RATE params in QACRO mode
don't scale motor tilt

Co-authored-by: IamPete1 <33176108+IamPete1@users.noreply.github.com>
This commit is contained in:
kd0aij
2019-12-18 08:33:29 +11:00
committed by Andrew Tridgell
co-authored by IamPete1
parent b1ccf5b9d5
commit 95da094bfe
3 changed files with 112 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduPlane/quadplane.cpp
+47 -14
View File
@@ -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 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
@@ -470,6 +470,20 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @User: Standard
AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist_alt, 0),
// @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", 17, 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", 18, QuadPlane, tailsitter.gain_scaling_min, 0.4),
AP_GROUPEND
};
@@ -811,21 +825,41 @@ 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;
}
}
@@ -1023,9 +1057,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;
ArduPlane/quadplane.h
+16 -3
View File
@@ -124,9 +124,9 @@ public:
// check if we have completed transition to vtol
bool tailsitter_transition_vtol_complete(void) const;
// account for surface speed scaling in hover
// account for control surface speed scaling in VTOL modes
void tailsitter_speed_scaling(void);
// user initiated takeoff for guided mode
bool do_user_takeoff(float takeoff_altitude);
@@ -455,7 +455,13 @@ private:
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;
@@ -466,10 +472,17 @@ private:
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;
ArduPlane/tailsitter.cpp
+49 -72
View File
@@ -18,6 +18,7 @@
to a configuration supported by AP_MotorsMatrix
*/
#include <math.h>
#include "Plane.h"
/*
@@ -103,7 +104,7 @@ void QuadPlane::tailsitter_output(void)
}
// handle VTOL modes
// the MultiCopter rate controller has already been run in an earlier call
// the MultiCopter rate controller has already been run in an earlier call
// to motors_output() from quadplane.update()
motors_output(false);
plane.pitchController.reset_I();
@@ -129,11 +130,12 @@ void QuadPlane::tailsitter_output(void)
power law. This allows the motors to point straight up for
takeoff without integrator windup
*/
int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5;
float des_pitch_cd = attitude_control->get_att_target_euler_cd().y;
int32_t pitch_error_cd = (des_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 = 0;
if (!is_zero(extra_pitch)) {
if (!is_zero(extra_pitch) && in_vtol_mode()) {
extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * SERVO_MAX;
}
tilt_left = extra_elevator + tilt_left * tailsitter.vectored_hover_gain;
@@ -215,8 +217,8 @@ bool QuadPlane::tailsitter_transition_vtol_complete(void) const
void QuadPlane::tailsitter_check_input(void)
{
if (tailsitter_active() &&
(tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P ||
tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M ||
(tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_M ||
tailsitter.input_type == TAILSITTER_INPUT_BF_ROLL_P ||
tailsitter.input_type == TAILSITTER_INPUT_PLANE)) {
// the user has asked for body frame controls when tailsitter
// is active. We switch around the control_in value for the
@@ -244,95 +246,70 @@ void QuadPlane::tailsitter_speed_scaling(void)
{
const float hover_throttle = motors->get_throttle_hover();
const float throttle = motors->get_throttle();
float spd_scaler = 1;
float spd_scaler = 1.0f;
// 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);
}
} else {
if (tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ATT_THR) {
// reduce gains when flying at high speed in Q modes:
// critical parameter: violent oscillations if too high
// sudden loss of attitude control if too low
constexpr float max_atten = 0.2f;
const float min_scale = tailsitter.gain_scaling_min;
float tthr = 1.25f * hover_throttle;
float aspeed;
bool airspeed_enabled = ahrs.airspeed_sensor_enabled();
// If there is an airspeed sensor use the measured airspeed
// The airspeed estimate based only on GPS and (estimated) wind is
// not sufficiently accurate for tailsitters.
// (based on tests in RealFlight 8 with 10kph wind)
if (airspeed_enabled && ahrs.airspeed_estimate(&aspeed)) {
// plane.get_speed_scaler() doesn't work well for copter tailsitters
// ramp down from 1 to max_atten as speed increases to airspeed_max
spd_scaler = constrain_float(1 - (aspeed / plane.aparm.airspeed_max), max_atten, 1.0f);
} else {
// if no airspeed sensor reduce control surface throws at large tilt
// angles (assuming high airspeed)
// ramp down from 1 to max_atten at tilt angles over trans_angle
// (angles here are represented by their cosines)
// reduce control surface throws at large tilt
// angles (assuming high airspeed)
// ramp down from 1 to max_atten at tilt angles over trans_angle
// (angles here are represented by their cosines)
// Note that the cosf call will be necessary if trans_angle becomes a parameter
// but the C language spec does not guarantee that trig functions can be used
// in constant expressions, even though gcc currently allows it.
constexpr float c_trans_angle = 0.9238795; // cosf(.125f * M_PI)
// Note that the cosf call will be necessary if trans_angle becomes a parameter
// but the C language spec does not guarantee that trig functions can be used
// in constant expressions, even though gcc currently allows it.
constexpr float c_trans_angle = 0.9238795; // cosf(.125f * M_PI)
// alpha = (1 - max_atten) / (c_trans_angle - cosf(radians(90)));
constexpr float alpha = (1 - max_atten) / c_trans_angle;
constexpr float beta = 1 - alpha * c_trans_angle;
// alpha = (1 - max_atten) / (c_trans_angle - cosf(radians(90)));
const float alpha = (1 - min_scale) / c_trans_angle;
const float beta = 1 - alpha * c_trans_angle;
const float c_tilt = ahrs_view->get_rotation_body_to_ned().c.z;
if (c_tilt < c_trans_angle) {
spd_scaler = constrain_float(beta + alpha * c_tilt, max_atten, 1.0f);
// reduce throttle attenuation threshold too
tthr = 0.5f * hover_throttle;
}
const float c_tilt = ahrs_view->get_rotation_body_to_ned().c.z;
if (c_tilt < c_trans_angle) {
spd_scaler = constrain_float(beta + alpha * c_tilt, min_scale, 1.0f);
// reduce throttle attenuation threshold too
tthr = 0.5f * hover_throttle;
}
// if throttle is above hover thrust, apply additional attenuation
if (throttle > tthr) {
const float throttle_atten = 1 - (throttle - tthr) / (1 - tthr);
spd_scaler *= throttle_atten;
spd_scaler = constrain_float(spd_scaler, max_atten, 1.0f);
spd_scaler = constrain_float(spd_scaler, min_scale, 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[5] = {
// limit positive and negative slew rates of applied speed scaling
constexpr float posTC = 2.0f; // seconds
constexpr float negTC = 1.0f; // seconds
const float posdelta = plane.G_Dt / posTC;
const float negdelta = plane.G_Dt / negTC;
spd_scaler = constrain_float(spd_scaler, last_spd_scaler - negdelta, last_spd_scaler + posdelta);
last_spd_scaler = spd_scaler;
}
// if gain attenuation isn't active and boost is enabled
if ((spd_scaler >= 1.0f) && (tailsitter.gain_scaling_mask & TAILSITTER_GSCL_BOOST)) {
// boost gains at low throttle
if (is_zero(throttle)) {
spd_scaler = tailsitter.throttle_scale_max;
} else {
spd_scaler = constrain_float(hover_throttle / throttle, 1.0f, tailsitter.throttle_scale_max);
}
}
const SRV_Channel::Aux_servo_function_t functions[] = {
SRV_Channel::Aux_servo_function_t::k_aileron,
SRV_Channel::Aux_servo_function_t::k_elevator,
SRV_Channel::Aux_servo_function_t::k_rudder,
SRV_Channel::Aux_servo_function_t::k_tiltMotorLeft,
SRV_Channel::Aux_servo_function_t::k_tiltMotorRight};
SRV_Channel::Aux_servo_function_t::k_rudder};
for (uint8_t i=0; i<ARRAY_SIZE(functions); i++) {
int32_t v = SRV_Channels::get_output_scaled(functions[i]);
v *= scale;
v *= spd_scaler;
v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
SRV_Channels::set_output_scaled(functions[i], v);
}
}