diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index cc3c632d9086c..216d88486bc22 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 83a3d8d7ec63a..286d742f24dfd 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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 diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 3d1c65b3f884c..66a2f41d0a46c 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -21,6 +21,7 @@ #include "qautotune.h" #include "defines.h" #include "tailsitter.h" +#include "tiltrotor.h" /* QuadPlane specific functionality @@ -37,6 +38,7 @@ class QuadPlane 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 @@ class QuadPlane // 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 @@ class QuadPlane // 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 @@ class QuadPlane // 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<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 diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index ebcdb94f57715..12fed63a6a037 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -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); diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index f566e038c9036..a2fe83f94e2e2 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -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(newtiltget_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= 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; iget_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 diff --git a/ArduPlane/tiltrotor.h b/ArduPlane/tiltrotor.h new file mode 100644 index 0000000000000..e1b2ef299d419 --- /dev/null +++ b/ArduPlane/tiltrotor.h @@ -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 . + */ +#pragma once + +#include +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<