Skip to content

Commit

Permalink
Plane: fixed tilt vectoring to cope with large tilt angles
Browse files Browse the repository at this point in the history 
This uses vectoring for both roll and yaw when tilted, and uses
differential thrust for yaw when tilted
  • Loading branch information
@tridge
tridge committed Dec 15, 2020
1 parent c504e2d commit ad14e15
Show file tree
Hide file tree
Showing 3 changed files with 88 additions and 105 deletions.
26 changes: 21 additions & 5 deletions ArduPlane/quadplane.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -735,6 +735,15 @@ bool QuadPlane::setup(void)
if (!motors->initialised_ok()) {
AP_BoardConfig::config_error("unknown Q_FRAME_TYPE %u", (unsigned)frame_type.get());
}

if (motors_var_info == AP_MotorsMatrix::var_info &&
tilt.tilt_mask != 0 &&
tilt.tilt_type == TILT_TYPE_VECTORED_YAW) {
// we will be using vectoring for yaw
motors->disable_yaw_torque();
}


motors->set_throttle_range(thr_min_pwm, thr_max_pwm);
motors->set_update_rate(rc_speed);
motors->set_interlock(true);
Expand Down Expand Up @@ -1728,11 +1737,18 @@ void QuadPlane::update_transition(void)
}
hold_hover(climb_rate_cms);

// set desired yaw to current yaw in both desired angle and
// rate request. This reduces wing twist in transition due to
// multicopter yaw demands
attitude_control->set_yaw_target_to_current_heading();
attitude_control->rate_bf_yaw_target(ahrs.get_gyro().z);
if (tilt.tilt_mask == 0 ||
tilt.tilt_type != TILT_TYPE_VECTORED_YAW ||
now - transition_start_ms < 100) {
// set desired yaw to current yaw in both desired angle
// and rate request. This reduces wing twist in transition
// due to multicopter yaw demands. This is disabled when
// using vectored yaw for tilt-rotors as the yaw control
// is needed to maintain good control in forward
// transitions
attitude_control->set_yaw_target_to_current_heading();
attitude_control->rate_bf_yaw_target(ahrs.get_gyro().z);
}

last_throttle = motors->get_throttle();

Expand Down
4 changes: 2 additions & 2 deletions ArduPlane/quadplane.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -480,6 +480,7 @@ class QuadPlane
float current_tilt;
float current_throttle;
bool motors_active:1;
float transition_yaw;
} tilt;

// bit 0 enables plane mode and bit 1 enables body-frame roll mode
Expand Down Expand Up @@ -542,8 +543,7 @@ class QuadPlane
void tiltrotor_binary_update(void);
void tiltrotor_vectored_yaw(void);
void tiltrotor_bicopter(void);
void tilt_compensate_up(float *thrust, uint8_t num_motors);
void tilt_compensate_down(float *thrust, uint8_t num_motors);
void tilt_compensate_angle(float *thrust, uint8_t num_motors, float non_tilted_mul, float tilted_mul);
void tilt_compensate(float *thrust, uint8_t num_motors);
bool is_motor_tilting(uint8_t motor) const {
return (((uint8_t)tilt.tilt_mask.get()) & (1U<<motor));
Expand Down
163 changes: 65 additions & 98 deletions ArduPlane/tiltrotor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -213,78 +213,66 @@ void QuadPlane::tiltrotor_update(void)
}
}


/*
compensate for tilt in a set of motor outputs
Compensation is of two forms. The first is to apply _tilt_factor,
which is a compensation for the reduces vertical thrust when
tilted. This is supplied by set_motor_tilt_factor().
The second compensation is to use equal thrust on all tilted motors
when _tilt_equal_thrust is true. This is used when the motors are
tilted by a large angle to prevent the roll and yaw controllers from
causing instability. Typically this would be used when the motors
are tilted beyond 45 degrees. At this angle it is assumed that roll
control can be achieved using fixed wing control surfaces and yaw
control with the remaining multicopter motors (eg. tricopter tail).
By applying _tilt_equal_thrust the tilted motors effectively become
a single pitch control motor.
Note that we use a different strategy for when we are transitioning
into VTOL as compared to from VTOL flight. The reason for that is
we want to lean towards higher tilted motor throttle when
transitioning to fixed wing flight, in order to gain airspeed,
whereas when transitioning to VTOL flight we want to lean to towards
lower fwd throttle. So we raise the throttle on the tilted motors
when transitioning to fixed wing, and lower throttle on tilted
motors when transitioning to VTOL
*/
void QuadPlane::tilt_compensate_down(float *thrust, uint8_t num_motors)
{
float inv_tilt_factor;
if (tilt.current_tilt > 0.98f) {
inv_tilt_factor = 1.0 / cosf(radians(0.98f*90));
} else {
inv_tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90));
}
tilt compensation for angle of tilt. When the rotors are tilted the
roll effect of differential thrust on the tilted rotors is decreased
and the yaw effect increased
We have two factors we apply.
// when we got past Q_TILT_MAX we gang the tilted motors together
// to generate equal thrust. This makes them act as a single pitch
// control motor while preventing them trying to do roll and yaw
// control while angled over. This greatly improves the stability
// of the last phase of transitions
float tilt_threshold = (tilt.max_angle_deg/90.0f);
bool equal_thrust = (tilt.current_tilt > tilt_threshold);
1) when we are transitioning to fwd flight we scale the tilted rotors by 1/cos(angle). This pushes us towards more flight speed
2) when we are transitioning to hover we scale the non-tilted rotors by cos(angle). This pushes us towards lower fwd thrust
We also apply an equalisation to the tilted motors in proportion to
how much tilt we have. This smoothly reduces the impact of the roll
gains as we tilt further forward.
For yaw, we apply differential thrust in proportion to the demanded
yaw control and sin of the tilt angle
Finally we ensure no requested thrust is over 1 by scaling back all
motors so the largest thrust is at most 1.0
*/
void QuadPlane::tilt_compensate_angle(float *thrust, uint8_t num_motors, float non_tilted_mul, float tilted_mul)
{
float tilt_total = 0;
uint8_t tilt_count = 0;

// apply inv_tilt_factor first
// apply tilt_factors first
for (uint8_t i=0; i<num_motors; i++) {
if (is_motor_tilting(i)) {
thrust[i] *= inv_tilt_factor;
if (!is_motor_tilting(i)) {
thrust[i] *= non_tilted_mul;
} else {
thrust[i] *= tilted_mul;
tilt_total += thrust[i];
tilt_count++;
}
}

float largest_tilted = 0;
const float sin_tilt = sinf(radians(tilt.current_tilt*90));
// yaw_gain relates the amount of differential thrust we get from
// tilt, so that the scaling of the yaw control is the same at any
// tilt angle
const float yaw_gain = sinf(radians(tilt.tilt_yaw_angle));
const float avg_tilt_thrust = tilt_total / tilt_count;

// now constrain and apply _tilt_equal_thrust if enabled
for (uint8_t i=0; i<num_motors; i++) {
if (is_motor_tilting(i)) {
if (equal_thrust) {
thrust[i] = tilt_total / tilt_count;
}
// as we tilt we need to reduce the impact of the roll
// controller. This simple method keeps the same average,
// but moves us to no roll control as the angle increases
thrust[i] = tilt.current_tilt * avg_tilt_thrust + thrust[i] * (1-tilt.current_tilt);
// add in differential thrust for yaw control, scaled by tilt angle
const float diff_thrust = motors->get_roll_factor(i) * motors->get_yaw() * sin_tilt * yaw_gain;
thrust[i] += diff_thrust;
largest_tilted = MAX(largest_tilted, thrust[i]);
}
}

// if we are saturating one of the tilted motors then reduce all
// motors to keep them in proportion to the original thrust. This
// helps maintain stability when tilted at a large angle
// if we are saturating one of the motors then reduce all motors
// to keep them in proportion to the original thrust. This helps
// maintain stability when tilted at a large angle
if (largest_tilted > 1.0f) {
float scale = 1.0f / largest_tilted;
for (uint8_t i=0; i<num_motors; i++) {
Expand All @@ -293,45 +281,6 @@ void QuadPlane::tilt_compensate_down(float *thrust, uint8_t num_motors)
}
}


/*
tilt compensation when transitioning to VTOL flight
*/
void QuadPlane::tilt_compensate_up(float *thrust, uint8_t num_motors)
{
float tilt_factor = cosf(radians(tilt.current_tilt*90));

// when we got past Q_TILT_MAX we gang the tilted motors together
// to generate equal thrust. This makes them act as a single pitch
// control motor while preventing them trying to do roll and yaw
// control while angled over. This greatly improves the stability
// of the last phase of transitions
float tilt_threshold = (tilt.max_angle_deg/90.0f);
bool equal_thrust = (tilt.current_tilt > tilt_threshold);

float tilt_total = 0;
uint8_t tilt_count = 0;

// apply tilt_factor first
for (uint8_t i=0; i<num_motors; i++) {
if (!is_motor_tilting(i)) {
thrust[i] *= tilt_factor;
} else {
tilt_total += thrust[i];
tilt_count++;
}
}

// now constrain and apply _tilt_equal_thrust if enabled
for (uint8_t i=0; i<num_motors; i++) {
if (is_motor_tilting(i)) {
if (equal_thrust) {
thrust[i] = tilt_total / tilt_count;
}
}
}
}

/*
choose up or down tilt compensation based on flight mode When going
to a fixed wing mode we use tilt_compensate_down, when going to a
Expand All @@ -345,9 +294,16 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
}
if (in_vtol_mode()) {
// we are transitioning to VTOL flight
tilt_compensate_up(thrust, num_motors);
const float tilt_factor = cosf(radians(tilt.current_tilt*90));
tilt_compensate_angle(thrust, num_motors, tilt_factor, 1);
} else {
tilt_compensate_down(thrust, num_motors);
float inv_tilt_factor;
if (tilt.current_tilt > 0.98f) {
inv_tilt_factor = 1.0 / cosf(radians(0.98f*90));
} else {
inv_tilt_factor = 1.0 / cosf(radians(tilt.current_tilt*90));
}
tilt_compensate_angle(thrust, num_motors, 1, inv_tilt_factor);
}
}

Expand Down Expand Up @@ -386,8 +342,8 @@ void QuadPlane::tiltrotor_vectored_yaw(void)
yaw_out /= plane.channel_rudder->get_range();
float yaw_range = zero_out;

SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * (base_output + yaw_out * yaw_range));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * (base_output - yaw_out * yaw_range));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
}
return;
}
Expand All @@ -398,11 +354,22 @@ void QuadPlane::tiltrotor_vectored_yaw(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * base_output);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * base_output);
} else {
float yaw_out = motors->get_yaw();
const float yaw_out = motors->get_yaw();
const float roll_out = motors->get_roll();
float yaw_range = zero_out;

SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * (base_output + yaw_out * yaw_range));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * (base_output - yaw_out * yaw_range));
// now apply vectored thrust for yaw and roll.
const float tilt_rad = radians(tilt.current_tilt*90);
const float sin_tilt = sinf(tilt_rad);
const float cos_tilt = cosf(tilt_rad);
// the MotorsMatrix library normalises roll factor to 0.5, so
// we need to use the same factor here to keep the same roll
// gains when tilted as we have when not tilted
const float avg_roll_factor = 0.5;
const float tilt_offset = constrain_float(yaw_out * cos_tilt + avg_roll_factor * roll_out * sin_tilt, -1, 1);

SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * constrain_float(base_output + tilt_offset * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * constrain_float(base_output - tilt_offset * yaw_range,0,1));
}
}

Expand Down

0 comments on commit ad14e15

Please sign in to comment.