Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: add high-speed Qmode tailsitter gain scaling with support for airspeed sensor

Browse Source
This commit is contained in:
Mark Whitehorn 2019-02-25 21:14:04 -07:00 committed by Andrew Tridgell
parent 487ad7a90f
commit 61aff344d3
1 changed files with 72 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
ArduPlane/tailsitter.cpp
View File

@ -196,7 +196,7 @@ void QuadPlane::tailsitter_check_input(void)
}
/*
return true if we are a tailsitter transitioning to VTOL flight
return true if we are a tailsitter transitioning to VTOL flight
*/
bool QuadPlane::in_tailsitter_vtol_transition(void) const
{
@ -204,19 +204,83 @@ bool QuadPlane::in_tailsitter_vtol_transition(void) const
}
/*
account for speed scaling of control surfaces in hover
account for speed scaling of control surfaces in VTOL modes
*/
void QuadPlane::tailsitter_speed_scaling(void)
{
const float hover_throttle = motors->get_throttle_hover();
const float throttle = motors->get_throttle();
float scaling;
float spd_scaler = 1;
if (is_zero(throttle)) {
scaling = tailsitter.throttle_scale_max;
// 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 {
scaling = constrain_float(hover_throttle / throttle, 0, tailsitter.throttle_scale_max);
// 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;
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)
constexpr float c_trans_angle = cosf(.125f * M_PI);
constexpr float alpha = (1 - max_atten) / (c_trans_angle - cosf(radians(90)));
constexpr 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;
}
}
// 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);
}
}
// 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,
@ -225,9 +289,9 @@ void QuadPlane::tailsitter_speed_scaling(void)
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 *= scaling;
v *= scale;
v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
SRV_Channels::set_output_scaled(functions[i], v);
}
}