Plane: Quadpalne: move tiltrotor functionality to own class
This commit is contained in:
Iampete1
Andrew Tridgell
parent
17f8b0b11a
commit
c4a1ae42e5
@ -139,6 +139,7 @@ public:
|
||||
friend class RC_Channel_Plane;
|
||||
friend class RC_Channels_Plane;
|
||||
friend class Tailsitter;
|
||||
friend class Tiltrotor;
|
||||
|
||||
friend class Mode;
|
||||
friend class ModeCircle;
|
||||
|
||||
@ -203,29 +203,9 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("RTL_MODE", 36, QuadPlane, rtl_mode, 0),
|
||||
|
||||
// @Param: TILT_MASK
|
||||
// @DisplayName: Tiltrotor mask
|
||||
// @Description: This is a bitmask of motors that are tiltable in a tiltrotor (or tiltwing). The mask is in terms of the standard motor order for the frame type.
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TILT_MASK", 37, QuadPlane, tilt.tilt_mask, 0),
|
||||
|
||||
// @Param: TILT_RATE_UP
|
||||
// @DisplayName: Tiltrotor upwards tilt rate
|
||||
// @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from forward flight to hover
|
||||
// @Units: deg/s
|
||||
// @Increment: 1
|
||||
// @Range: 10 300
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TILT_RATE_UP", 38, QuadPlane, tilt.max_rate_up_dps, 40),
|
||||
|
||||
// @Param: TILT_MAX
|
||||
// @DisplayName: Tiltrotor maximum VTOL angle
|
||||
// @Description: This is the maximum angle of the tiltable motors at which multicopter control will be enabled. Beyond this angle the plane will fly solely as a fixed wing aircraft and the motors will tilt to their maximum angle at the TILT_RATE
|
||||
// @Units: deg
|
||||
// @Increment: 1
|
||||
// @Range: 20 80
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TILT_MAX", 39, QuadPlane, tilt.max_angle_deg, 45),
|
||||
// 37: TILT_MASK
|
||||
// 38: TILT_RATE_UP
|
||||
// 39: TILT_MAX
|
||||
|
||||
// @Param: GUIDED_MODE
|
||||
// @DisplayName: Enable VTOL in GUIDED mode
|
||||
@ -268,36 +248,15 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist_angle, 30),
|
||||
|
||||
// @Param: TILT_TYPE
|
||||
// @DisplayName: Tiltrotor type
|
||||
// @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover, Bicopter tiltrottor must use the tailsitter frame class (10)
|
||||
// @Values: 0:Continuous,1:Binary,2:VectoredYaw,3:Bicopter
|
||||
AP_GROUPINFO("TILT_TYPE", 47, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS),
|
||||
|
||||
// 47: TILT_TYPE
|
||||
// 48: TAILSIT_ANGLE
|
||||
// 61: TAILSIT_ANG_VT
|
||||
|
||||
// @Param: TILT_RATE_DN
|
||||
// @DisplayName: Tiltrotor downwards tilt rate
|
||||
// @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from hover to forward flight. When this is zero the Q_TILT_RATE_UP value is used.
|
||||
// @Units: deg/s
|
||||
// @Increment: 1
|
||||
// @Range: 10 300
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TILT_RATE_DN", 49, QuadPlane, tilt.max_rate_down_dps, 0),
|
||||
|
||||
// 49: TILT_RATE_DN
|
||||
// 50: TAILSIT_INPUT
|
||||
// 51: TAILSIT_MASK
|
||||
// 52: TAILSIT_MASKCH
|
||||
// 53: TAILSIT_VFGAIN
|
||||
// 54: TAILSIT_VHGAIN
|
||||
|
||||
// @Param: TILT_YAW_ANGLE
|
||||
// @DisplayName: Tilt minimum angle for vectored yaw
|
||||
// @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output. This needs to be set for Q_TILT_TYPE=3 to enable vectored control for yaw of tricopter tilt quadplanes. This is also used to limit the forwards travel of bicopter tilts when in VTOL modes
|
||||
// @Range: 0 30
|
||||
AP_GROUPINFO("TILT_YAW_ANGLE", 55, QuadPlane, tilt.tilt_yaw_angle, 0),
|
||||
|
||||
// 56: TAILSIT_VHPOW
|
||||
|
||||
// @Param: MAV_TYPE
|
||||
@ -456,22 +415,8 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
|
||||
AP_GROUPINFO("FWD_MANTHR_MAX", 20, QuadPlane, fwd_thr_max, 0),
|
||||
|
||||
// 21: TAILSIT_DSKLD
|
||||
|
||||
// @Param: TILT_FIX_ANGLE
|
||||
// @DisplayName: Fixed wing tiltrotor angle
|
||||
// @Description: This is the angle the motors tilt down when at maximum output for forward flight. Set this to a non-zero value to enable vectoring for roll/pitch in forward flight on tilt-vectored aircraft
|
||||
// @Units: deg
|
||||
// @Range: 0 30
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TILT_FIX_ANGLE", 22, QuadPlane, tilt.fixed_angle, 0),
|
||||
|
||||
// @Param: TILT_FIX_GAIN
|
||||
// @DisplayName: Fixed wing tiltrotor gain
|
||||
// @Description: This is the gain for use of tilting motors in fixed wing flight for tilt vectored quadplanes
|
||||
// @Range: 0 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TILT_FIX_GAIN", 23, QuadPlane, tilt.fixed_gain, 0),
|
||||
|
||||
// 22: TILT_FIX_ANGLE
|
||||
// 23: TILT_FIX_GAIN
|
||||
// 24: TAILSIT_RAT_FW
|
||||
// 25: TAILSIT_RAT_VT
|
||||
|
||||
@ -479,6 +424,10 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
|
||||
// @Path: tailsitter.cpp
|
||||
AP_SUBGROUPINFO(tailsitter, "TAILSIT_", 26, QuadPlane, Tailsitter),
|
||||
|
||||
// @Group: TILT_
|
||||
// @Path: tiltrotor.cpp
|
||||
AP_SUBGROUPINFO(tiltrotor, "TILT_", 27, QuadPlane, Tiltrotor),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -548,6 +497,16 @@ const AP_Param::ConversionInfo q_conversion_table[] = {
|
||||
{ Parameters::k_param_quadplane, 1403, AP_PARAM_FLOAT, "Q_TAILSIT_DSKLD" },
|
||||
{ Parameters::k_param_quadplane, 1595, AP_PARAM_FLOAT, "Q_TAILSIT_RAT_FW" },
|
||||
{ Parameters::k_param_quadplane, 1659, AP_PARAM_FLOAT, "Q_TAILSIT_RAT_FW" },
|
||||
|
||||
// tiltrotor params have moved but retain the same names
|
||||
{ Parameters::k_param_quadplane, 37, AP_PARAM_INT16, "Q_TILT_MASK" },
|
||||
{ Parameters::k_param_quadplane, 38, AP_PARAM_INT16, "Q_TILT_RATE_UP" },
|
||||
{ Parameters::k_param_quadplane, 39, AP_PARAM_INT8, "Q_TILT_MAX" },
|
||||
{ Parameters::k_param_quadplane, 47, AP_PARAM_INT8, "Q_TILT_TYPE" },
|
||||
{ Parameters::k_param_quadplane, 49, AP_PARAM_INT16, "Q_TILT_RATE_DN" },
|
||||
{ Parameters::k_param_quadplane, 55, AP_PARAM_FLOAT, "Q_TILT_YAW_ANGLE" },
|
||||
{ Parameters::k_param_quadplane, 1467, AP_PARAM_FLOAT, "Q_TILT_FIX_ANGLE" },
|
||||
{ Parameters::k_param_quadplane, 1531, AP_PARAM_FLOAT, "Q_TILT_FIX_GAIN" },
|
||||
};
|
||||
|
||||
|
||||
@ -651,8 +610,8 @@ bool QuadPlane::setup(void)
|
||||
}
|
||||
|
||||
// Make sure not both a tailsiter and tiltrotor
|
||||
if (tailsitter.enabled() && (tilt.tilt_mask != 0)) {
|
||||
AP_BoardConfig::config_error("set TAILSIT_ENABLE 0 or TILT_MASK 0");
|
||||
if (tailsitter.enabled() && tiltrotor.enabled()) {
|
||||
AP_BoardConfig::config_error("set TAILSIT_ENABLE 0 or TILT_ENABLE 0");
|
||||
}
|
||||
|
||||
switch ((AP_Motors::motor_frame_class)frame_class) {
|
||||
@ -713,14 +672,6 @@ bool QuadPlane::setup(void)
|
||||
AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info);
|
||||
|
||||
motors->init(frame_class, frame_type);
|
||||
|
||||
tilt.is_vectored = tilt.tilt_mask != 0 && tilt.tilt_type == TILT_TYPE_VECTORED_YAW;
|
||||
|
||||
if (motors_var_info == AP_MotorsMatrix::var_info && tilt.is_vectored) {
|
||||
// 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);
|
||||
@ -736,19 +687,6 @@ bool QuadPlane::setup(void)
|
||||
|
||||
transition_state = TRANSITION_DONE;
|
||||
|
||||
if (tilt.tilt_mask != 0) {
|
||||
// setup tilt compensation
|
||||
motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&QuadPlane::tilt_compensate, void, float *, uint8_t));
|
||||
if (tilt.tilt_type == TILT_TYPE_VECTORED_YAW) {
|
||||
// setup tilt servos for vectored yaw
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorLeft, 1000);
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorRight, 1000);
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorRear, 1000);
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorRearLeft, 1000);
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorRearRight, 1000);
|
||||
}
|
||||
}
|
||||
|
||||
// default QAssist state as set with Q_OPTIONS
|
||||
if ((options & OPTION_Q_ASSIST_FORCE_ENABLE) != 0) {
|
||||
q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE;
|
||||
@ -760,6 +698,8 @@ bool QuadPlane::setup(void)
|
||||
|
||||
tailsitter.setup();
|
||||
|
||||
tiltrotor.setup();
|
||||
|
||||
// param count will have changed
|
||||
AP_Param::invalidate_count();
|
||||
|
||||
@ -809,39 +749,6 @@ void QuadPlane::run_esc_calibration(void)
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
when doing a forward transition of a tilt-vectored quadplane we use
|
||||
euler angle control to maintain good yaw. This updates the yaw
|
||||
target based on pilot input and target roll
|
||||
*/
|
||||
void QuadPlane::update_yaw_target(void)
|
||||
{
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - tilt.transition_yaw_set_ms > 100 ||
|
||||
!is_zero(get_pilot_input_yaw_rate_cds())) {
|
||||
// lock initial yaw when transition is started or when
|
||||
// pilot commands a yaw change. This allows us to track
|
||||
// straight in transitions for tilt-vectored planes, but
|
||||
// allows for turns when level transition is not wanted
|
||||
tilt.transition_yaw_cd = ahrs.yaw_sensor;
|
||||
}
|
||||
|
||||
/*
|
||||
now calculate the equivalent yaw rate for a coordinated turn for
|
||||
the desired bank angle given the airspeed
|
||||
*/
|
||||
float aspeed;
|
||||
bool have_airspeed = ahrs.airspeed_estimate(aspeed);
|
||||
if (have_airspeed && labs(plane.nav_roll_cd)>1000) {
|
||||
float dt = (now - tilt.transition_yaw_set_ms) * 0.001;
|
||||
// calculate the yaw rate to achieve the desired turn rate
|
||||
const float airspeed_min = MAX(plane.aparm.airspeed_min,5);
|
||||
const float yaw_rate_cds = fixedwing_turn_rate(plane.nav_roll_cd*0.01, MAX(aspeed,airspeed_min))*100;
|
||||
tilt.transition_yaw_cd += yaw_rate_cds * dt;
|
||||
}
|
||||
tilt.transition_yaw_set_ms = now;
|
||||
}
|
||||
|
||||
/*
|
||||
ask the multicopter attitude control to match the roll and pitch rates being demanded by the
|
||||
fixed wing controller if not in a pure VTOL mode
|
||||
@ -854,9 +761,9 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
|
||||
bool use_multicopter_eulers = false;
|
||||
|
||||
if (!use_multicopter_control &&
|
||||
tilt.is_vectored &&
|
||||
tiltrotor.is_vectored() &&
|
||||
transition_state <= TRANSITION_TIMER) {
|
||||
update_yaw_target();
|
||||
tiltrotor.update_yaw_target();
|
||||
use_multicopter_control = true;
|
||||
use_multicopter_eulers = true;
|
||||
}
|
||||
@ -912,7 +819,7 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
|
||||
if (use_multicopter_eulers) {
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd,
|
||||
plane.nav_pitch_cd,
|
||||
tilt.transition_yaw_cd,
|
||||
tiltrotor.transition_yaw_cd,
|
||||
true);
|
||||
} else {
|
||||
// use euler angle attitude control
|
||||
@ -1490,7 +1397,7 @@ void QuadPlane::update_transition(void)
|
||||
plane.control_mode == &plane.mode_acro ||
|
||||
plane.control_mode == &plane.mode_training) {
|
||||
// in manual modes quad motors are always off
|
||||
if (!tilt.motors_active && !tailsitter.enabled()) {
|
||||
if (!tiltrotor.motors_active() && !tailsitter.enabled()) {
|
||||
set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
||||
motors->output();
|
||||
}
|
||||
@ -1554,7 +1461,7 @@ void QuadPlane::update_transition(void)
|
||||
// if rotors are fully forward then we are not transitioning,
|
||||
// unless we are waiting for airspeed to increase (in which case
|
||||
// the tilt will decrease rapidly)
|
||||
if (tiltrotor_fully_fwd() && transition_state != TRANSITION_AIRSPEED_WAIT) {
|
||||
if (tiltrotor.fully_fwd() && transition_state != TRANSITION_AIRSPEED_WAIT) {
|
||||
if (transition_state == TRANSITION_TIMER) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done");
|
||||
}
|
||||
@ -1603,12 +1510,12 @@ void QuadPlane::update_transition(void)
|
||||
// otherwise the plane can end up in high-alpha flight with
|
||||
// low VTOL thrust and may not complete a transition
|
||||
float climb_rate_cms = assist_climb_rate_cms();
|
||||
if ((options & OPTION_LEVEL_TRANSITION) && tilt.tilt_mask == 0) {
|
||||
if ((options & OPTION_LEVEL_TRANSITION) && !tiltrotor.enabled()) {
|
||||
climb_rate_cms = MIN(climb_rate_cms, 0.0f);
|
||||
}
|
||||
hold_hover(climb_rate_cms);
|
||||
|
||||
if (!tilt.is_vectored) {
|
||||
if (!tiltrotor.is_vectored()) {
|
||||
// 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
|
||||
@ -1666,7 +1573,7 @@ void QuadPlane::update_transition(void)
|
||||
// control surfaces at this stage.
|
||||
// We disable this for vectored yaw tilt rotors as they do need active
|
||||
// yaw control throughout the transition
|
||||
if (!tilt.is_vectored) {
|
||||
if (!tiltrotor.is_vectored()) {
|
||||
attitude_control->reset_yaw_target_and_rate();
|
||||
attitude_control->rate_bf_yaw_target(ahrs.get_gyro().z);
|
||||
}
|
||||
@ -1694,7 +1601,7 @@ void QuadPlane::update_transition(void)
|
||||
return;
|
||||
|
||||
case TRANSITION_DONE:
|
||||
if (!tilt.motors_active && !tailsitter.enabled()) {
|
||||
if (!tiltrotor.motors_active() && !tailsitter.enabled()) {
|
||||
set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
||||
motors->output();
|
||||
}
|
||||
@ -1824,7 +1731,7 @@ void QuadPlane::update(void)
|
||||
throttle_wait = false;
|
||||
}
|
||||
|
||||
tiltrotor_update();
|
||||
tiltrotor.update();
|
||||
|
||||
// motors logging
|
||||
if (motors->armed()) {
|
||||
@ -2002,7 +1909,7 @@ void QuadPlane::motors_output(bool run_rate_controller)
|
||||
motors->output();
|
||||
|
||||
// remember when motors were last active for throttle suppression
|
||||
if (motors->get_throttle() > 0.01f || tilt.motors_active) {
|
||||
if (motors->get_throttle() > 0.01f || tiltrotor.motors_active()) {
|
||||
last_motors_active_ms = now;
|
||||
}
|
||||
|
||||
@ -2354,7 +2261,7 @@ void QuadPlane::vtol_position_controller(void)
|
||||
vel_forward.last_ms = now_ms;
|
||||
}
|
||||
|
||||
if (tilt.tilt_mask == 0 && !tailsitter.enabled()) {
|
||||
if (!tiltrotor.enabled() && !tailsitter.enabled()) {
|
||||
/*
|
||||
cope with fwd motor thrust loss during approach. We detect
|
||||
this by looking for the fwd throttle saturating. This only
|
||||
@ -3639,7 +3546,7 @@ bool QuadPlane::show_vtol_view() const
|
||||
return true;
|
||||
}
|
||||
}
|
||||
if (!show_vtol && tilt.is_vectored && transition_state <= TRANSITION_TIMER) {
|
||||
if (!show_vtol && tiltrotor.is_vectored() && transition_state <= TRANSITION_TIMER) {
|
||||
// we use multirotor controls during fwd transition for
|
||||
// vectored yaw vehicles
|
||||
return true;
|
||||
@ -3753,6 +3660,19 @@ bool QuadPlane::air_mode_active() const
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
return scaling factor for tilting rotors in forward flight throttle
|
||||
we want to scale back tilt angle for roll/pitch by throttle in forward flight
|
||||
*/
|
||||
float QuadPlane::FW_vector_throttle_scaling()
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
QuadPlane *QuadPlane::_singleton = nullptr;
|
||||
|
||||
#endif // HAL_QUADPLANE_ENABLED
|
||||
|
||||
@ -21,6 +21,7 @@
|
||||
#include "qautotune.h"
|
||||
#include "defines.h"
|
||||
#include "tailsitter.h"
|
||||
#include "tiltrotor.h"
|
||||
|
||||
/*
|
||||
QuadPlane specific functionality
|
||||
@ -37,6 +38,7 @@ public:
|
||||
friend class RC_Channel_Plane;
|
||||
friend class RC_Channel;
|
||||
friend class Tailsitter;
|
||||
friend class Tiltrotor;
|
||||
|
||||
friend class Mode;
|
||||
friend class ModeAuto;
|
||||
@ -231,9 +233,6 @@ private:
|
||||
// use multicopter rate controller
|
||||
void multicopter_attitude_rate_update(float yaw_rate_cds);
|
||||
|
||||
// update yaw target for tiltrotor transition
|
||||
void update_yaw_target();
|
||||
|
||||
void check_attitude_relax(void);
|
||||
float get_pilot_throttle(void);
|
||||
void control_hover(void);
|
||||
@ -470,31 +469,8 @@ private:
|
||||
// time of last QTUN log message
|
||||
uint32_t last_qtun_log_ms;
|
||||
|
||||
// types of tilt mechanisms
|
||||
enum {TILT_TYPE_CONTINUOUS =0,
|
||||
TILT_TYPE_BINARY =1,
|
||||
TILT_TYPE_VECTORED_YAW =2,
|
||||
TILT_TYPE_BICOPTER =3
|
||||
};
|
||||
|
||||
// tiltrotor control variables
|
||||
struct {
|
||||
AP_Int16 tilt_mask;
|
||||
AP_Int16 max_rate_up_dps;
|
||||
AP_Int16 max_rate_down_dps;
|
||||
AP_Int8 max_angle_deg;
|
||||
AP_Int8 tilt_type;
|
||||
AP_Float tilt_yaw_angle;
|
||||
AP_Float fixed_angle;
|
||||
AP_Float fixed_gain;
|
||||
float current_tilt;
|
||||
float current_throttle;
|
||||
bool motors_active:1;
|
||||
float transition_yaw_cd;
|
||||
uint32_t transition_yaw_set_ms;
|
||||
bool is_vectored;
|
||||
} tilt;
|
||||
|
||||
// Tiltrotor control
|
||||
Tiltrotor tiltrotor{*this, motors};
|
||||
|
||||
// tailsitter control
|
||||
Tailsitter tailsitter{*this, motors};
|
||||
@ -511,22 +487,9 @@ private:
|
||||
|
||||
// time when we were last in a vtol control mode
|
||||
uint32_t last_vtol_mode_ms;
|
||||
|
||||
void tiltrotor_slew(float tilt);
|
||||
void tiltrotor_binary_slew(bool forward);
|
||||
void tiltrotor_update(void);
|
||||
void tiltrotor_continuous_update(void);
|
||||
void tiltrotor_binary_update(void);
|
||||
void tiltrotor_vectoring(void);
|
||||
void tiltrotor_bicopter(void);
|
||||
float tilt_throttle_scaling(void);
|
||||
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));
|
||||
}
|
||||
bool tiltrotor_fully_fwd(void) const;
|
||||
float tilt_max_change(bool up) const;
|
||||
|
||||
// throttle scailing for vectored motors in FW flighy
|
||||
float FW_vector_throttle_scaling(void);
|
||||
|
||||
void afs_terminate(void);
|
||||
bool guided_mode_enabled(void);
|
||||
|
||||
@ -763,7 +763,7 @@ void Plane::force_flare(void)
|
||||
if (!control_mode->does_auto_throttle() && channel_throttle->in_trim_dz() && flare_mode != FlareMode::FLARE_DISABLED) {
|
||||
int32_t tilt = -SERVO_MAX; //this is tilts up for a normal tiltrotor
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
if (quadplane.tilt.tilt_type == QuadPlane::TILT_TYPE_BICOPTER) {
|
||||
if (quadplane.tiltrotor.enabled() && (quadplane.tiltrotor.type == Tiltrotor::TILT_TYPE_BICOPTER)) {
|
||||
tilt = 0; // this is tilts up for a Bicopter
|
||||
}
|
||||
if (quadplane.tailsitter.enabled()) {
|
||||
@ -940,7 +940,7 @@ void Plane::servos_output(void)
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
// cope with tailsitters and bicopters
|
||||
quadplane.tailsitter.output();
|
||||
quadplane.tiltrotor_bicopter();
|
||||
quadplane.tiltrotor.bicopter_output();
|
||||
#endif
|
||||
|
||||
// support forced flare option
|
||||
|
||||
@ -169,7 +169,7 @@ Tailsitter::Tailsitter(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors):qu
|
||||
void Tailsitter::setup()
|
||||
{
|
||||
// Set tailsitter enable flag based on old heuristics
|
||||
if (!enable.configured() && (((quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (motor_mask != 0)) && (quadplane.tilt.tilt_type != QuadPlane::TILT_TYPE_BICOPTER))) {
|
||||
if (!enable.configured() && (((quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (motor_mask != 0)) && (quadplane.tiltrotor.type != Tiltrotor::TILT_TYPE_BICOPTER))) {
|
||||
enable.set_and_save(1);
|
||||
}
|
||||
|
||||
@ -284,7 +284,7 @@ void Tailsitter::output(void)
|
||||
// in forward flight: set motor tilt servos and throttles using FW controller
|
||||
if (vectored_forward_gain > 0) {
|
||||
// remove scaling from surface speed scaling and apply throttle scaling
|
||||
const float scaler = plane.control_mode == &plane.mode_manual?1:(quadplane.tilt_throttle_scaling() / plane.get_speed_scaler());
|
||||
const float scaler = plane.control_mode == &plane.mode_manual?1:(quadplane.FW_vector_throttle_scaling() / plane.get_speed_scaler());
|
||||
// thrust vectoring in fixed wing flight
|
||||
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
|
||||
float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
|
||||
|
||||
@ -1,30 +1,139 @@
|
||||
#include "tiltrotor.h"
|
||||
#include "Plane.h"
|
||||
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
const AP_Param::GroupInfo Tiltrotor::var_info[] = {
|
||||
|
||||
// @Param: ENABLE
|
||||
// @DisplayName: Enable Tiltrotor functionality
|
||||
// @Values: 0:Disable, 1:Enable
|
||||
// @Description: This enables Tiltrotor functionality
|
||||
// @User: Standard
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, Tiltrotor, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: MASK
|
||||
// @DisplayName: Tiltrotor mask
|
||||
// @Description: This is a bitmask of motors that are tiltable in a tiltrotor (or tiltwing). The mask is in terms of the standard motor order for the frame type.
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("MASK", 2, Tiltrotor, tilt_mask, 0),
|
||||
|
||||
// @Param: RATE_UP
|
||||
// @DisplayName: Tiltrotor upwards tilt rate
|
||||
// @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from forward flight to hover
|
||||
// @Units: deg/s
|
||||
// @Increment: 1
|
||||
// @Range: 10 300
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("RATE_UP", 3, Tiltrotor, max_rate_up_dps, 40),
|
||||
|
||||
// @Param: MAX
|
||||
// @DisplayName: Tiltrotor maximum VTOL angle
|
||||
// @Description: This is the maximum angle of the tiltable motors at which multicopter control will be enabled. Beyond this angle the plane will fly solely as a fixed wing aircraft and the motors will tilt to their maximum angle at the TILT_RATE
|
||||
// @Units: deg
|
||||
// @Increment: 1
|
||||
// @Range: 20 80
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("MAX", 4, Tiltrotor, max_angle_deg, 45),
|
||||
|
||||
// @Param: TYPE
|
||||
// @DisplayName: Tiltrotor type
|
||||
// @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover, Bicopter tiltrottor must use the tailsitter frame class (10)
|
||||
// @Values: 0:Continuous,1:Binary,2:VectoredYaw,3:Bicopter
|
||||
AP_GROUPINFO("TYPE", 5, Tiltrotor, type, TILT_TYPE_CONTINUOUS),
|
||||
|
||||
// @Param: RATE_DN
|
||||
// @DisplayName: Tiltrotor downwards tilt rate
|
||||
// @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from hover to forward flight. When this is zero the Q_TILT_RATE_UP value is used.
|
||||
// @Units: deg/s
|
||||
// @Increment: 1
|
||||
// @Range: 10 300
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("RATE_DN", 6, Tiltrotor, max_rate_down_dps, 0),
|
||||
|
||||
// @Param: YAW_ANGLE
|
||||
// @DisplayName: Tilt minimum angle for vectored yaw
|
||||
// @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output. This needs to be set for Q_TILT_TYPE=3 to enable vectored control for yaw of tricopter tilt quadplanes. This is also used to limit the forwards travel of bicopter tilts when in VTOL modes
|
||||
// @Range: 0 30
|
||||
AP_GROUPINFO("YAW_ANGLE", 7, Tiltrotor, tilt_yaw_angle, 0),
|
||||
|
||||
// @Param: FIX_ANGLE
|
||||
// @DisplayName: Fixed wing tiltrotor angle
|
||||
// @Description: This is the angle the motors tilt down when at maximum output for forward flight. Set this to a non-zero value to enable vectoring for roll/pitch in forward flight on tilt-vectored aircraft
|
||||
// @Units: deg
|
||||
// @Range: 0 30
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("FIX_ANGLE", 8, Tiltrotor, fixed_angle, 0),
|
||||
|
||||
// @Param: FIX_GAIN
|
||||
// @DisplayName: Fixed wing tiltrotor gain
|
||||
// @Description: This is the gain for use of tilting motors in fixed wing flight for tilt vectored quadplanes
|
||||
// @Range: 0 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("FIX_GAIN", 9, Tiltrotor, fixed_gain, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
/*
|
||||
control code for tiltrotors and tiltwings. Enabled by setting
|
||||
Q_TILT_MASK to a non-zero value
|
||||
*/
|
||||
|
||||
Tiltrotor::Tiltrotor(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors):quadplane(_quadplane),motors(_motors)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
void Tiltrotor::setup()
|
||||
{
|
||||
|
||||
if (!enable.configured() && ((tilt_mask != 0) || (type == TILT_TYPE_BICOPTER))) {
|
||||
enable.set_and_save(1);
|
||||
}
|
||||
|
||||
if (!enabled()) {
|
||||
return;
|
||||
}
|
||||
|
||||
_is_vectored = tilt_mask != 0 && type == TILT_TYPE_VECTORED_YAW;
|
||||
|
||||
if (quadplane.motors_var_info == AP_MotorsMatrix::var_info && _is_vectored) {
|
||||
// we will be using vectoring for yaw
|
||||
motors->disable_yaw_torque();
|
||||
}
|
||||
|
||||
if (tilt_mask != 0) {
|
||||
// setup tilt compensation
|
||||
motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&Tiltrotor::tilt_compensate, void, float *, uint8_t));
|
||||
if (type == TILT_TYPE_VECTORED_YAW) {
|
||||
// setup tilt servos for vectored yaw
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorLeft, 1000);
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorRight, 1000);
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorRear, 1000);
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorRearLeft, 1000);
|
||||
SRV_Channels::set_range(SRV_Channel::k_tiltMotorRearRight, 1000);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
calculate maximum tilt change as a proportion from 0 to 1 of tilt
|
||||
*/
|
||||
float QuadPlane::tilt_max_change(bool up) const
|
||||
float Tiltrotor::tilt_max_change(bool up) const
|
||||
{
|
||||
float rate;
|
||||
if (up || tilt.max_rate_down_dps <= 0) {
|
||||
rate = tilt.max_rate_up_dps;
|
||||
if (up || max_rate_down_dps <= 0) {
|
||||
rate = max_rate_up_dps;
|
||||
} else {
|
||||
rate = tilt.max_rate_down_dps;
|
||||
rate = max_rate_down_dps;
|
||||
}
|
||||
if (tilt.tilt_type != TILT_TYPE_BINARY && !up) {
|
||||
if (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) {
|
||||
if (hal.util->get_soft_armed() && !quadplane.in_vtol_mode() && !quadplane.assisted_flight) {
|
||||
fast_tilt = true;
|
||||
}
|
||||
if (fast_tilt) {
|
||||
@ -39,61 +148,61 @@ float QuadPlane::tilt_max_change(bool up) const
|
||||
/*
|
||||
output a slew limited tiltrotor angle. tilt is from 0 to 1
|
||||
*/
|
||||
void QuadPlane::tiltrotor_slew(float newtilt)
|
||||
void 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);
|
||||
float max_change = tilt_max_change(newtilt<current_tilt);
|
||||
current_tilt = constrain_float(newtilt, current_tilt-max_change, current_tilt+max_change);
|
||||
|
||||
// translate to 0..1000 range and output
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * tilt.current_tilt);
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * current_tilt);
|
||||
}
|
||||
|
||||
/*
|
||||
update motor tilt for continuous tilt servos
|
||||
*/
|
||||
void QuadPlane::tiltrotor_continuous_update(void)
|
||||
void Tiltrotor::continuous_update(void)
|
||||
{
|
||||
// default to inactive
|
||||
tilt.motors_active = false;
|
||||
_motors_active = false;
|
||||
|
||||
// the maximum rate of throttle change
|
||||
float max_change;
|
||||
|
||||
if (!in_vtol_mode() && (!hal.util->get_soft_armed() || !assisted_flight)) {
|
||||
|
||||
if (!quadplane.in_vtol_mode() && (!hal.util->get_soft_armed() || !quadplane.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);
|
||||
slew(1);
|
||||
|
||||
max_change = tilt_max_change(false);
|
||||
|
||||
|
||||
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);
|
||||
if (current_tilt < 1) {
|
||||
current_throttle = constrain_float(new_throttle,
|
||||
current_throttle-max_change,
|
||||
current_throttle+max_change);
|
||||
} else {
|
||||
tilt.current_throttle = new_throttle;
|
||||
current_throttle = new_throttle;
|
||||
}
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
tilt.current_throttle = 0;
|
||||
current_throttle = 0;
|
||||
} else {
|
||||
// prevent motor shutdown
|
||||
tilt.motors_active = true;
|
||||
_motors_active = true;
|
||||
}
|
||||
if (!motor_test.running) {
|
||||
if (!quadplane.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);
|
||||
uint8_t mask = is_zero(current_throttle)?0:(uint8_t)tilt_mask.get();
|
||||
motors->output_motor_mask(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);
|
||||
max_change = tilt_max_change(motors_throttle<current_throttle);
|
||||
current_throttle = constrain_float(motors_throttle,
|
||||
current_throttle-max_change,
|
||||
current_throttle+max_change);
|
||||
|
||||
/*
|
||||
we are in a VTOL mode. We need to work out how much tilt is
|
||||
@ -117,39 +226,39 @@ void QuadPlane::tiltrotor_continuous_update(void)
|
||||
|
||||
#if QAUTOTUNE_ENABLED
|
||||
if (plane.control_mode == &plane.mode_qautotune) {
|
||||
tiltrotor_slew(0);
|
||||
slew(0);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
// if not in assisted flight and in QACRO, QSTABILIZE or QHOVER mode
|
||||
if (!assisted_flight &&
|
||||
if (!quadplane.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) {
|
||||
if (quadplane.rc_fwd_thr_ch == nullptr) {
|
||||
// no manual throttle control, set angle to zero
|
||||
tiltrotor_slew(0);
|
||||
slew(0);
|
||||
} else {
|
||||
// manual control of forward throttle
|
||||
float settilt = .01f * forward_throttle_pct();
|
||||
tiltrotor_slew(settilt);
|
||||
float settilt = .01f * quadplane.forward_throttle_pct();
|
||||
slew(settilt);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (assisted_flight &&
|
||||
transition_state >= TRANSITION_TIMER) {
|
||||
if (quadplane.assisted_flight &&
|
||||
quadplane.transition_state >= QuadPlane::TRANSITION_TIMER) {
|
||||
// we are transitioning to fixed wing - tilt the motors all
|
||||
// the way forward
|
||||
tiltrotor_slew(1);
|
||||
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);
|
||||
slew(settilt * max_angle_deg / 90.0f);
|
||||
}
|
||||
}
|
||||
|
||||
@ -157,7 +266,7 @@ void QuadPlane::tiltrotor_continuous_update(void)
|
||||
/*
|
||||
output a slew limited tiltrotor angle. tilt is 0 or 1
|
||||
*/
|
||||
void QuadPlane::tiltrotor_binary_slew(bool forward)
|
||||
void 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);
|
||||
@ -165,33 +274,33 @@ void QuadPlane::tiltrotor_binary_slew(bool forward)
|
||||
// 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);
|
||||
current_tilt = constrain_float(current_tilt+max_change, 0, 1);
|
||||
} else {
|
||||
tilt.current_tilt = constrain_float(tilt.current_tilt-max_change, 0, 1);
|
||||
current_tilt = constrain_float(current_tilt-max_change, 0, 1);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
update motor tilt for binary tilt servos
|
||||
*/
|
||||
void QuadPlane::tiltrotor_binary_update(void)
|
||||
void Tiltrotor::binary_update(void)
|
||||
{
|
||||
// motors always active
|
||||
tilt.motors_active = true;
|
||||
_motors_active = true;
|
||||
|
||||
if (!in_vtol_mode()) {
|
||||
if (!quadplane.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);
|
||||
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();
|
||||
if (current_tilt >= 1) {
|
||||
uint8_t mask = is_zero(new_throttle)?0:(uint8_t)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);
|
||||
binary_slew(false);
|
||||
}
|
||||
}
|
||||
|
||||
@ -199,21 +308,21 @@ void QuadPlane::tiltrotor_binary_update(void)
|
||||
/*
|
||||
update motor tilt
|
||||
*/
|
||||
void QuadPlane::tiltrotor_update(void)
|
||||
void Tiltrotor::update(void)
|
||||
{
|
||||
if (tilt.tilt_mask <= 0) {
|
||||
if (!enabled() || tilt_mask == 0) {
|
||||
// no motors to tilt
|
||||
return;
|
||||
}
|
||||
|
||||
if (tilt.tilt_type == TILT_TYPE_BINARY) {
|
||||
tiltrotor_binary_update();
|
||||
if (type == TILT_TYPE_BINARY) {
|
||||
binary_update();
|
||||
} else {
|
||||
tiltrotor_continuous_update();
|
||||
continuous_update();
|
||||
}
|
||||
|
||||
if (tilt.tilt_type == TILT_TYPE_VECTORED_YAW) {
|
||||
tiltrotor_vectoring();
|
||||
if (type == TILT_TYPE_VECTORED_YAW) {
|
||||
vectoring();
|
||||
}
|
||||
}
|
||||
|
||||
@ -237,7 +346,7 @@ void QuadPlane::tiltrotor_update(void)
|
||||
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)
|
||||
void Tiltrotor::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;
|
||||
@ -254,11 +363,11 @@ void QuadPlane::tilt_compensate_angle(float *thrust, uint8_t num_motors, float n
|
||||
}
|
||||
|
||||
float largest_tilted = 0;
|
||||
const float sin_tilt = sinf(radians(tilt.current_tilt*90));
|
||||
const float sin_tilt = sinf(radians(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 yaw_gain = sinf(radians(tilt_yaw_angle));
|
||||
const float avg_tilt_thrust = tilt_total / tilt_count;
|
||||
|
||||
for (uint8_t i=0; i<num_motors; i++) {
|
||||
@ -266,7 +375,7 @@ void QuadPlane::tilt_compensate_angle(float *thrust, uint8_t num_motors, float n
|
||||
// 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);
|
||||
thrust[i] = current_tilt * avg_tilt_thrust + thrust[i] * (1-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;
|
||||
@ -290,22 +399,22 @@ void QuadPlane::tilt_compensate_angle(float *thrust, uint8_t num_motors, float n
|
||||
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)
|
||||
void Tiltrotor::tilt_compensate(float *thrust, uint8_t num_motors)
|
||||
{
|
||||
if (tilt.current_tilt <= 0) {
|
||||
if (current_tilt <= 0) {
|
||||
// the motors are not tilted, no compensation needed
|
||||
return;
|
||||
}
|
||||
if (in_vtol_mode()) {
|
||||
if (quadplane.in_vtol_mode()) {
|
||||
// we are transitioning to VTOL flight
|
||||
const float tilt_factor = cosf(radians(tilt.current_tilt*90));
|
||||
const float tilt_factor = cosf(radians(current_tilt*90));
|
||||
tilt_compensate_angle(thrust, num_motors, tilt_factor, 1);
|
||||
} else {
|
||||
float inv_tilt_factor;
|
||||
if (tilt.current_tilt > 0.98f) {
|
||||
if (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));
|
||||
inv_tilt_factor = 1.0 / cosf(radians(current_tilt*90));
|
||||
}
|
||||
tilt_compensate_angle(thrust, num_motors, 1, inv_tilt_factor);
|
||||
}
|
||||
@ -314,50 +423,36 @@ void QuadPlane::tilt_compensate(float *thrust, uint8_t num_motors)
|
||||
/*
|
||||
return true if the rotors are fully tilted forward
|
||||
*/
|
||||
bool QuadPlane::tiltrotor_fully_fwd(void) const
|
||||
bool Tiltrotor::fully_fwd(void) const
|
||||
{
|
||||
if (tilt.tilt_mask <= 0) {
|
||||
if (!enabled() || (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);
|
||||
return (current_tilt >= 1);
|
||||
}
|
||||
|
||||
/*
|
||||
control vectoring for tilt multicopters
|
||||
*/
|
||||
void QuadPlane::tiltrotor_vectoring(void)
|
||||
void Tiltrotor::vectoring(void)
|
||||
{
|
||||
// total angle the tilt can go through
|
||||
const float total_angle = 90 + tilt.tilt_yaw_angle + tilt.fixed_angle;
|
||||
const float total_angle = 90 + tilt_yaw_angle + 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 zero_out = tilt_yaw_angle / total_angle;
|
||||
const float fixed_tilt_limit = 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));
|
||||
float base_output = zero_out + (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)) {
|
||||
if (!hal.util->get_soft_armed() && (plane.quadplane.options & QuadPlane::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()) {
|
||||
if (quadplane.in_vtol_mode()) {
|
||||
float yaw_out = plane.channel_rudder->get_control_in();
|
||||
yaw_out /= plane.channel_rudder->get_range();
|
||||
float yaw_range = zero_out;
|
||||
@ -369,7 +464,7 @@ void QuadPlane::tiltrotor_vectoring(void)
|
||||
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;
|
||||
const float gain = 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
|
||||
@ -387,13 +482,13 @@ void QuadPlane::tiltrotor_vectoring(void)
|
||||
return;
|
||||
}
|
||||
|
||||
float tilt_threshold = (tilt.max_angle_deg/90.0f);
|
||||
bool no_yaw = (tilt.current_tilt > tilt_threshold);
|
||||
float tilt_threshold = (max_angle_deg/90.0f);
|
||||
bool no_yaw = (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
|
||||
// fixed wing 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 scaler = plane.control_mode == &plane.mode_manual?1:(quadplane.FW_vector_throttle_scaling() / plane.get_speed_scaler());
|
||||
const float gain = 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;
|
||||
@ -408,7 +503,7 @@ void QuadPlane::tiltrotor_vectoring(void)
|
||||
float yaw_range = zero_out;
|
||||
|
||||
// now apply vectored thrust for yaw and roll.
|
||||
const float tilt_rad = radians(tilt.current_tilt*90);
|
||||
const float tilt_rad = radians(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
|
||||
@ -428,25 +523,25 @@ void QuadPlane::tiltrotor_vectoring(void)
|
||||
/*
|
||||
control bicopter tiltrotors
|
||||
*/
|
||||
void QuadPlane::tiltrotor_bicopter(void)
|
||||
void Tiltrotor::bicopter_output(void)
|
||||
{
|
||||
if (tilt.tilt_type != TILT_TYPE_BICOPTER || motor_test.running) {
|
||||
if (type != TILT_TYPE_BICOPTER || quadplane.motor_test.running) {
|
||||
// don't override motor test with motors_output
|
||||
return;
|
||||
}
|
||||
|
||||
if (!in_vtol_mode() && tiltrotor_fully_fwd()) {
|
||||
if (!quadplane.in_vtol_mode() && 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);
|
||||
if (quadplane.assisted_flight) {
|
||||
quadplane.hold_stabilize(throttle * 0.01f);
|
||||
quadplane.motors_output(true);
|
||||
} else {
|
||||
motors_output(false);
|
||||
quadplane.motors_output(false);
|
||||
}
|
||||
|
||||
// bicopter assumes that trim is up so we scale down so match
|
||||
@ -454,23 +549,55 @@ void QuadPlane::tiltrotor_bicopter(void)
|
||||
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;
|
||||
tilt_left *= tilt_yaw_angle / 90.0f;
|
||||
}
|
||||
if (is_negative(tilt_right)) {
|
||||
tilt_right *= tilt.tilt_yaw_angle / 90.0f;
|
||||
tilt_right *= tilt_yaw_angle / 90.0f;
|
||||
}
|
||||
|
||||
// reduce authority of bicopter as motors are tilted forwards
|
||||
const float scaling = cosf(tilt.current_tilt * M_PI_2);
|
||||
const float scaling = cosf(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);
|
||||
tilt_left = constrain_float(-(current_tilt * SERVO_MAX) + tilt_left, -SERVO_MAX, SERVO_MAX);
|
||||
tilt_right = constrain_float(-(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);
|
||||
}
|
||||
|
||||
/*
|
||||
when doing a forward transition of a tilt-vectored quadplane we use
|
||||
euler angle control to maintain good yaw. This updates the yaw
|
||||
target based on pilot input and target roll
|
||||
*/
|
||||
void Tiltrotor::update_yaw_target(void)
|
||||
{
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - transition_yaw_set_ms > 100 ||
|
||||
!is_zero(quadplane.get_pilot_input_yaw_rate_cds())) {
|
||||
// lock initial yaw when transition is started or when
|
||||
// pilot commands a yaw change. This allows us to track
|
||||
// straight in transitions for tilt-vectored planes, but
|
||||
// allows for turns when level transition is not wanted
|
||||
transition_yaw_cd = quadplane.ahrs.yaw_sensor;
|
||||
}
|
||||
|
||||
/*
|
||||
now calculate the equivalent yaw rate for a coordinated turn for
|
||||
the desired bank angle given the airspeed
|
||||
*/
|
||||
float aspeed;
|
||||
bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed);
|
||||
if (have_airspeed && labs(plane.nav_roll_cd)>1000) {
|
||||
float dt = (now - transition_yaw_set_ms) * 0.001;
|
||||
// calculate the yaw rate to achieve the desired turn rate
|
||||
const float airspeed_min = MAX(plane.aparm.airspeed_min,5);
|
||||
const float yaw_rate_cds = fixedwing_turn_rate(plane.nav_roll_cd*0.01, MAX(aspeed,airspeed_min))*100;
|
||||
transition_yaw_cd += yaw_rate_cds * dt;
|
||||
}
|
||||
transition_yaw_set_ms = now;
|
||||
}
|
||||
#endif // HAL_QUADPLANE_ENABLED
|
||||
|
||||
88
ArduPlane/tiltrotor.h
Normal file
88
ArduPlane/tiltrotor.h
Normal file
@ -0,0 +1,88 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
class QuadPlane;
|
||||
class AP_MotorsMulticopter;
|
||||
class Tiltrotor
|
||||
{
|
||||
friend class QuadPlane;
|
||||
friend class Plane;
|
||||
public:
|
||||
|
||||
Tiltrotor(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors);
|
||||
|
||||
bool enabled() const { return enable > 0;}
|
||||
|
||||
void setup();
|
||||
|
||||
void slew(float tilt);
|
||||
void binary_slew(bool forward);
|
||||
void update();
|
||||
void continuous_update();
|
||||
void binary_update();
|
||||
void vectoring();
|
||||
void bicopter_output();
|
||||
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_mask.get()) & (1U<<motor));
|
||||
}
|
||||
|
||||
bool fully_fwd() const;
|
||||
float tilt_max_change(bool up) const;
|
||||
|
||||
// update yaw target for tiltrotor transition
|
||||
void update_yaw_target();
|
||||
|
||||
bool is_vectored() const { return enabled() && _is_vectored; }
|
||||
|
||||
bool motors_active() const { return enabled() && _motors_active; }
|
||||
|
||||
AP_Int8 enable;
|
||||
AP_Int16 tilt_mask;
|
||||
AP_Int16 max_rate_up_dps;
|
||||
AP_Int16 max_rate_down_dps;
|
||||
AP_Int8 max_angle_deg;
|
||||
AP_Int8 type;
|
||||
AP_Float tilt_yaw_angle;
|
||||
AP_Float fixed_angle;
|
||||
AP_Float fixed_gain;
|
||||
|
||||
float current_tilt;
|
||||
float current_throttle;
|
||||
bool _motors_active:1;
|
||||
float transition_yaw_cd;
|
||||
uint32_t transition_yaw_set_ms;
|
||||
bool _is_vectored;
|
||||
|
||||
// types of tilt mechanisms
|
||||
enum {TILT_TYPE_CONTINUOUS =0,
|
||||
TILT_TYPE_BINARY =1,
|
||||
TILT_TYPE_VECTORED_YAW =2,
|
||||
TILT_TYPE_BICOPTER =3
|
||||
};
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
private:
|
||||
|
||||
// refences for convenience
|
||||
QuadPlane& quadplane;
|
||||
AP_MotorsMulticopter*& motors;
|
||||
|
||||
};
|
||||
Loading…
Reference in New Issue
Block a user