Skip to content

Commit

Permalink
Plane: Quadpalne: move tiltrotor functionality to own class
Browse files Browse the repository at this point in the history
  • Loading branch information
@IamPete1
IamPete1 committed Sep 14, 2021
1 parent 3d34e06 commit e447308
Show file tree
Hide file tree
Showing 7 changed files with 388 additions and 289 deletions.
1 change: 1 addition & 0 deletions ArduPlane/Plane.h
View file
Expand Up @@ -139,6 +139,7 @@ class Plane : public AP_Vehicle {
friend class RC_Channel_Plane;
friend class RC_Channels_Plane;
friend class Tailsitter;
friend class Tiltrotor;

friend class Mode;
friend class ModeCircle;
Expand Down
182 changes: 51 additions & 131 deletions ArduPlane/quadplane.cpp
View file
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -456,29 +415,19 @@ 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

// @Group: TAILSIT_
// @Path: tailsitter.cpp
AP_SUBGROUPINFO(tailsitter, "TAILSIT_", 26, QuadPlane, Tailsitter),

// @Group: TILT_
// @Path: tiltrotor.cpp
AP_SUBGROUPINFO(tiltrotor, "TILT_", 27, QuadPlane, Tiltrotor),

AP_GROUPEND
};

Expand Down Expand Up @@ -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" },
};


Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand All @@ -760,6 +698,8 @@ bool QuadPlane::setup(void)

tailsitter.setup();

tiltrotor.setup();

// param count will have changed
AP_Param::invalidate_count();

Expand Down Expand Up @@ -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
Expand All @@ -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;
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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();
}
Expand Down Expand Up @@ -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");
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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();
}
Expand Down Expand Up @@ -1824,7 +1731,7 @@ void QuadPlane::update(void)
throttle_wait = false;
}

tiltrotor_update();
tiltrotor.update();

// motors logging
if (motors->armed()) {
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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

0 comments on commit e447308

Please sign in to comment.