ardupilot/ArduPlane/tiltrotor.cpp

471 lines
19 KiB
C++
Raw Normal View History

#include "Plane.h"
/*
control code for tiltrotors and tiltwings. Enabled by setting
Q_TILT_MASK to a non-zero value
*/
/*
calculate maximum tilt change as a proportion from 0 to 1 of tilt
*/
float QuadPlane::tilt_max_change(bool up) const
{
float rate;
if (up || tilt.max_rate_down_dps <= 0) {
rate = tilt.max_rate_up_dps;
} else {
rate = tilt.max_rate_down_dps;
}
if (tilt.tilt_type != TILT_TYPE_BINARY && !up) {
bool fast_tilt = false;
if (plane.control_mode == &plane.mode_manual) {
fast_tilt = true;
}
if (hal.util->get_soft_armed() && !in_vtol_mode() && !assisted_flight) {
fast_tilt = true;
}
if (fast_tilt) {
// allow a minimum of 90 DPS in manual or if we are not
// stabilising, to give fast control
rate = MAX(rate, 90);
}
}
return rate * plane.G_Dt / 90.0f;
}
/*
output a slew limited tiltrotor angle. tilt is from 0 to 1
*/
void QuadPlane::tiltrotor_slew(float newtilt)
{
float max_change = tilt_max_change(newtilt<tilt.current_tilt);
tilt.current_tilt = constrain_float(newtilt, tilt.current_tilt-max_change, tilt.current_tilt+max_change);
// translate to 0..1000 range and output
2016-10-22 07:27:57 -03:00
SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * tilt.current_tilt);
}
/*
update motor tilt for continuous tilt servos
*/
void QuadPlane::tiltrotor_continuous_update(void)
{
// default to inactive
tilt.motors_active = false;
// the maximum rate of throttle change
float max_change;
if (!in_vtol_mode() && (!hal.util->get_soft_armed() || !assisted_flight)) {
// we are in pure fixed wing mode. Move the tiltable motors all the way forward and run them as
// a forward motor
tiltrotor_slew(1);
max_change = tilt_max_change(false);
2016-10-22 07:27:57 -03:00
float new_throttle = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01, 0, 1);
if (tilt.current_tilt < 1) {
tilt.current_throttle = constrain_float(new_throttle,
tilt.current_throttle-max_change,
tilt.current_throttle+max_change);
} else {
tilt.current_throttle = new_throttle;
}
if (!hal.util->get_soft_armed()) {
tilt.current_throttle = 0;
} else {
2020-01-19 12:46:36 -04:00
// prevent motor shutdown
tilt.motors_active = true;
}
if (!motor_test.running) {
// the motors are all the way forward, start using them for fwd thrust
uint8_t mask = is_zero(tilt.current_throttle)?0:(uint8_t)tilt.tilt_mask.get();
motors->output_motor_mask(tilt.current_throttle, mask, plane.rudder_dt);
}
return;
}
// remember the throttle level we're using for VTOL flight
float motors_throttle = motors->get_throttle();
max_change = tilt_max_change(motors_throttle<tilt.current_throttle);
tilt.current_throttle = constrain_float(motors_throttle,
tilt.current_throttle-max_change,
tilt.current_throttle+max_change);
/*
we are in a VTOL mode. We need to work out how much tilt is
needed. There are 4 strategies we will use:
1) without manual forward throttle control, the angle will be set to zero
in QAUTOTUNE QACRO, QSTABILIZE and QHOVER. This
enables these modes to be used as a safe recovery mode.
2) with manual forward throttle control we will set the angle based on
the demanded forward throttle via RC input.
3) in fixed wing assisted flight or velocity controlled modes we
will set the angle based on the demanded forward throttle,
with a maximum tilt given by Q_TILT_MAX. This relies on
Q_VFWD_GAIN being set.
4) if we are in TRANSITION_TIMER mode then we are transitioning
to forward flight and should put the rotors all the way forward
*/
if (plane.control_mode == &plane.mode_qautotune) {
tiltrotor_slew(0);
return;
}
// if not in assisted flight and in QACRO, QSTABILIZE or QHOVER mode
if (!assisted_flight &&
(plane.control_mode == &plane.mode_qacro ||
plane.control_mode == &plane.mode_qstabilize ||
plane.control_mode == &plane.mode_qhover)) {
if (rc_fwd_thr_ch == nullptr) {
// no manual throttle control, set angle to zero
tiltrotor_slew(0);
} else {
// manual control of forward throttle
float settilt = .01f * forward_throttle_pct();
tiltrotor_slew(settilt);
}
return;
}
if (assisted_flight &&
transition_state >= TRANSITION_TIMER) {
// we are transitioning to fixed wing - tilt the motors all
// the way forward
tiltrotor_slew(1);
} else {
// until we have completed the transition we limit the tilt to
// Q_TILT_MAX. Anything above 50% throttle gets
// Q_TILT_MAX. Below 50% throttle we decrease linearly. This
// relies heavily on Q_VFWD_GAIN being set appropriately.
float settilt = constrain_float((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)-MAX(plane.aparm.throttle_min.get(),0)) / 50.0f, 0, 1);
tiltrotor_slew(settilt * tilt.max_angle_deg / 90.0f);
}
}
2016-05-08 20:11:11 -03:00
/*
output a slew limited tiltrotor angle. tilt is 0 or 1
*/
void QuadPlane::tiltrotor_binary_slew(bool forward)
{
// The servo output is binary, not slew rate limited
SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, forward?1000:0);
// rate limiting current_tilt has the effect of delaying throttle in tiltrotor_binary_update
float max_change = tilt_max_change(!forward);
if (forward) {
tilt.current_tilt = constrain_float(tilt.current_tilt+max_change, 0, 1);
} else {
tilt.current_tilt = constrain_float(tilt.current_tilt-max_change, 0, 1);
}
}
/*
update motor tilt for binary tilt servos
*/
void QuadPlane::tiltrotor_binary_update(void)
{
// motors always active
tilt.motors_active = true;
if (!in_vtol_mode()) {
// we are in pure fixed wing mode. Move the tiltable motors
// all the way forward and run them as a forward motor
tiltrotor_binary_slew(true);
float new_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01f;
if (tilt.current_tilt >= 1) {
uint8_t mask = is_zero(new_throttle)?0:(uint8_t)tilt.tilt_mask.get();
// the motors are all the way forward, start using them for fwd thrust
motors->output_motor_mask(new_throttle, mask, plane.rudder_dt);
}
} else {
tiltrotor_binary_slew(false);
}
}
/*
update motor tilt
*/
void QuadPlane::tiltrotor_update(void)
{
if (tilt.tilt_mask <= 0) {
// no motors to tilt
return;
}
if (tilt.tilt_type == TILT_TYPE_BINARY) {
tiltrotor_binary_update();
} else {
tiltrotor_continuous_update();
}
if (tilt.tilt_type == TILT_TYPE_VECTORED_YAW) {
tiltrotor_vectoring();
}
}
2016-05-08 20:11:11 -03:00
/*
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.
2016-05-08 20:11:11 -03:00
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.
2016-05-08 20:11:11 -03:00
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)
{
2016-05-08 20:11:11 -03:00
float tilt_total = 0;
uint8_t tilt_count = 0;
// apply tilt_factors first
2016-05-08 20:11:11 -03:00
for (uint8_t i=0; i<num_motors; i++) {
if (!is_motor_tilting(i)) {
thrust[i] *= non_tilted_mul;
} else {
thrust[i] *= tilted_mul;
2016-05-08 20:11:11 -03:00
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;
2016-05-08 20:11:11 -03:00
for (uint8_t i=0; i<num_motors; i++) {
if (is_motor_tilting(i)) {
// 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;
2016-05-08 20:11:11 -03:00
largest_tilted = MAX(largest_tilted, thrust[i]);
}
}
// 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
2016-05-08 20:11:11 -03:00
if (largest_tilted > 1.0f) {
float scale = 1.0f / largest_tilted;
for (uint8_t i=0; i<num_motors; i++) {
thrust[i] *= scale;
}
}
}
/*
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
VTOL mode we use tilt_compensate_up
*/
void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
{
if (tilt.current_tilt <= 0) {
// the motors are not tilted, no compensation needed
return;
}
if (in_vtol_mode()) {
// we are transitioning to VTOL flight
const float tilt_factor = cosf(radians(tilt.current_tilt*90));
tilt_compensate_angle(thrust, num_motors, tilt_factor, 1);
} else {
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);
}
}
/*
return true if the rotors are fully tilted forward
*/
bool QuadPlane::tiltrotor_fully_fwd(void) const
{
if (tilt.tilt_mask <= 0) {
return false;
}
return (tilt.current_tilt >= 1);
}
/*
return scaling factor for tilt rotors by throttle
we want to scale back tilt angle for roll/pitch by throttle in
forward flight
*/
float QuadPlane::tilt_throttle_scaling(void)
{
const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01;
// scale relative to a fixed 0.5 mid throttle so that changes in TRIM_THROTTLE in missions don't change
// the scaling of tilt
const float mid_throttle = 0.5;
return mid_throttle / constrain_float(throttle, 0.1, 1.0);
}
/*
control vectoring for tilt multicopters
*/
void QuadPlane::tiltrotor_vectoring(void)
{
// total angle the tilt can go through
const float total_angle = 90 + tilt.tilt_yaw_angle + tilt.fixed_angle;
// output value (0 to 1) to get motors pointed straight up
const float zero_out = tilt.tilt_yaw_angle / total_angle;
const float fixed_tilt_limit = tilt.fixed_angle / total_angle;
const float level_out = 1.0 - fixed_tilt_limit;
// calculate the basic tilt amount from current_tilt
float base_output = zero_out + (tilt.current_tilt * (level_out - zero_out));
// for testing when disarmed, apply vectored yaw in proportion to rudder stick
// Wait TILT_DELAY_MS after disarming to allow props to spin down first.
constexpr uint32_t TILT_DELAY_MS = 3000;
uint32_t now = AP_HAL::millis();
if (!hal.util->get_soft_armed() && (plane.quadplane.options & OPTION_DISARMED_TILT)) {
// this test is subject to wrapping at ~49 days, but the consequences are insignificant
if ((now - hal.util->get_last_armed_change()) > TILT_DELAY_MS) {
if (in_vtol_mode()) {
float yaw_out = plane.channel_rudder->get_control_in();
yaw_out /= plane.channel_rudder->get_range();
float yaw_range = zero_out;
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));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
} else {
// fixed wing tilt
const float gain = tilt.fixed_gain * fixed_tilt_limit;
// base the tilt on elevon mixing, which means it
// takes account of the MIXING_GAIN. The rear tilt is
// based on elevator
const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) / 4500.0;
const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) / 4500.0;
const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0;
// front tilt is effective canards, so need to swap and use negative. Rear motors are treated live elevons.
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft,1000 * constrain_float(base_output + left,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight,1000 * constrain_float(base_output + right,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output + mid,0,1));
}
}
return;
}
float tilt_threshold = (tilt.max_angle_deg/90.0f);
bool no_yaw = (tilt.current_tilt > tilt_threshold);
if (no_yaw) {
// fixed wing tilt. We need to apply inverse scaling with throttle, and remove the surface speed scaling as
// we don't want tilt impacted by airspeed
const float scaler = plane.control_mode == &plane.mode_manual?1:(tilt_throttle_scaling() / plane.get_speed_scaler());
const float gain = tilt.fixed_gain * fixed_tilt_limit * scaler;
const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) / 4500.0;
const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) / 4500.0;
const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0;
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft,1000 * constrain_float(base_output + left,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight,1000 * constrain_float(base_output + right,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output + mid,0,1));
} else {
const float yaw_out = motors->get_yaw();
const float roll_out = motors->get_roll();
float yaw_range = zero_out;
// 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));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft, 1000 * constrain_float(base_output + tilt_offset * yaw_range,0,1));
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight, 1000 * constrain_float(base_output - tilt_offset * yaw_range,0,1));
}
}
2019-03-18 21:00:25 -03:00
/*
control bicopter tiltrotors
*/
void QuadPlane::tiltrotor_bicopter(void)
{
2020-01-19 12:46:36 -04:00
if (tilt.tilt_type != TILT_TYPE_BICOPTER || motor_test.running) {
// don't override motor test with motors_output
2019-03-18 21:00:25 -03:00
return;
}
if (!in_vtol_mode() && tiltrotor_fully_fwd()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, -SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, -SERVO_MAX);
return;
}
float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
if (assisted_flight) {
hold_stabilize(throttle * 0.01f);
motors_output(true);
} else {
motors_output(false);
}
// bicopter assumes that trim is up so we scale down so match
float tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);
float tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight);
if (is_negative(tilt_left)) {
tilt_left *= tilt.tilt_yaw_angle / 90.0f;
}
if (is_negative(tilt_right)) {
tilt_right *= tilt.tilt_yaw_angle / 90.0f;
}
// reduce authority of bicopter as motors are tilted forwards
const float scaling = cosf(tilt.current_tilt * M_PI_2);
tilt_left *= scaling;
tilt_right *= scaling;
// add current tilt and constrain
tilt_left = constrain_float(-(tilt.current_tilt * SERVO_MAX) + tilt_left, -SERVO_MAX, SERVO_MAX);
tilt_right = constrain_float(-(tilt.current_tilt * SERVO_MAX) + tilt_right, -SERVO_MAX, SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
}