diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 6ebf3546ef..ff880e33bd 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -17,6 +17,8 @@ bool ModeAuto::_enter() if (plane.quadplane.available() && plane.quadplane.enable == 2) { plane.auto_state.vtol_mode = true; + // always zero forward throttle demand on entry into VTOL modes + quadplane.q_fwd_throttle = 0.0f; } else { plane.auto_state.vtol_mode = false; } diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index b74a434384..13b16a066e 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -5,6 +5,9 @@ bool ModeQAcro::_enter() { + // always zero forward throttle demand on entry into VTOL modes + quadplane.q_fwd_throttle = 0.0f; + quadplane.throttle_wait = false; quadplane.transition->force_transition_complete(); attitude_control->relax_attitude_controllers(); diff --git a/ArduPlane/mode_qautotune.cpp b/ArduPlane/mode_qautotune.cpp index 28bf977128..5d50f8b7c5 100644 --- a/ArduPlane/mode_qautotune.cpp +++ b/ArduPlane/mode_qautotune.cpp @@ -7,6 +7,9 @@ bool ModeQAutotune::_enter() { + // always zero forward throttle demand on entry into VTOL modes + quadplane.q_fwd_throttle = 0.0f; + #if QAUTOTUNE_ENABLED return quadplane.qautotune.init(); #else diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp index 1fe4848296..0553c8dfae 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -5,6 +5,9 @@ bool ModeQHover::_enter() { + // always zero forward throttle demand on entry into VTOL modes + quadplane.q_fwd_throttle = 0.0f; + // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index e646c80746..11b2bcbeea 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -5,6 +5,9 @@ bool ModeQLand::_enter() { + // always zero forward throttle demand on entry into VTOL modes + quadplane.q_fwd_throttle = 0.0f; + plane.mode_qloiter._enter(); quadplane.throttle_wait = false; quadplane.setup_target_position(); diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index e39ad63559..fe3a936d30 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -5,6 +5,9 @@ bool ModeQLoiter::_enter() { + // always zero forward throttle demand on entry into VTOL modes + quadplane.q_fwd_throttle = 0.0f; + // initialise loiter loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); @@ -83,6 +86,8 @@ void ModeQLoiter::run() plane.nav_roll_cd = loiter_nav->get_roll(); plane.nav_pitch_cd = loiter_nav->get_pitch(); + plane.quadplane.assign_tilt_to_fwd_thr(); + if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 16e805a40d..e4545c01d8 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -5,6 +5,9 @@ bool ModeQRTL::_enter() { + // always zero forward throttle demand on entry into VTOL modes + plane.quadplane.q_fwd_throttle = 0.0f; + // treat QRTL as QLAND if we are in guided wait takeoff state, to cope // with failsafes during GUIDED->AUTO takeoff sequence if (plane.quadplane.guided_wait_takeoff_on_mode_enter) { @@ -103,6 +106,9 @@ void ModeQRTL::run() // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + + plane.quadplane.assign_tilt_to_fwd_thr(); + if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 39f168fd73..61c79582d2 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -5,6 +5,9 @@ bool ModeQStabilize::_enter() { + // always zero forward throttle demand on entry into VTOL modes + quadplane.q_fwd_throttle = 0.0f; + quadplane.throttle_wait = false; return true; } @@ -35,6 +38,8 @@ void ModeQStabilize::update() plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max; plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max; } + + plane.quadplane.assign_tilt_to_fwd_thr(); } // quadplane stabilize mode diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1c89dd9bcd..f896c7fa89 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -167,7 +167,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: VFWD_GAIN // @DisplayName: Forward velocity hold gain - // @Description: Controls use of forward motor in vtol modes. If this is zero then the forward motor will not be used for position control in VTOL modes. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor. + // @Description: The use of this parameter is no longer recommended and has been superseded by a method that works in all VTOL modes with the exception of autotune which is controlled by the Q_FWD_THR_GAIN parameter. This Q_VFD_GAIN parameter controls use of the forward motor in VTOL modes that use the velocity controller. Set to 0 to disable this function. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor. // @Range: 0 0.5 // @Increment: 0.01 // @User: Standard @@ -505,6 +505,22 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("RTL_ALT_MIN", 34, QuadPlane, qrtl_alt_min, 10), + // @Param: FWD_THR_GAIN + // @DisplayName: Q mode fwd throttle gain + // @Description: Gain from forward accel/tilt to forward throttle that is used in all VTOL modes except autotune. Vehicles using separate forward thrust motors, eg quadplanes, should set this parameter to (all up weight) / (maximum combined thrust of forward motors). Vehicles that tilt lifting rotors to provide forward thrust should set this parameter to (all up weight) / (weight lifted by tilting rotors) which for most aircraft can be approximnated as (total number of lifting rotors) / (number of lifting rotors that tilt). When using this method of forward throttle control, the forward tilt angle limit is controlled by the Q_FWD_PIT_LIM parameter and the Q_VFWD_GAIN parameter should be set to 0 to disable the alternative method that works through the velocity controller. Set Q_FWD_THR_GAIN to 0 to disable this function. Do not use this parameter with tail sitters. + // @Range: 0.0 5.0 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("FWD_THR_GAIN", 35, QuadPlane, q_fwd_thr_gain, 2.0f), + + // @Param: FWD_PIT_LIM + // @DisplayName: Q mode forward pitch limit + // @Description: When forward throttle is being controlled by the Q_FWD_THR_GAIN parameter in Q modes, the vehicle forward (nose down) pitch rotation will be limited to the value specified by this parameter and the any additional forward acceleration required will be produced by use of the forward thrust motor(s) or tilting of moveable rotors. Larger values allow the vehicle to pitch more nose down. Set initially to the amount of nose down pitch required to remove wing lift. + // @Units: degrees + // @Range: 0.0 5.0 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("FWD_PIT_LIM", 36, QuadPlane, q_fwd_tilt_lim, 3.0f), AP_GROUPEND }; @@ -638,6 +654,9 @@ bool QuadPlane::setup(void) return false; } + q_fwd_throttle = 0.0f; + q_fwd_nav_pitch_lim_cd = -aparm.angle_max; + /* cope with upgrade from old AP_Motors values for frame_class */ @@ -2069,6 +2088,8 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) const gcs().send_text(MAV_SEVERITY_NOTICE, "Entered VTOL mode"); } plane.auto_state.vtol_mode = true; + // always zero forward throttle demand on entry into VTOL modes + plane.quadplane.q_fwd_throttle = 0.0f; return true; case MAV_VTOL_STATE_FW: @@ -2539,7 +2560,6 @@ void QuadPlane::vtol_position_controller(void) } } - if (poscontrol.get_state() == QPOS_APPROACH) { poscontrol_init_approach(); } @@ -2676,6 +2696,8 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + assign_tilt_to_fwd_thr(); + if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } @@ -2729,6 +2751,8 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + assign_tilt_to_fwd_thr(); + if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } @@ -2773,6 +2797,8 @@ void QuadPlane::vtol_position_controller(void) plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + assign_tilt_to_fwd_thr(); + // call attitude controller set_pilot_yaw_rate_time_constant(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, @@ -2896,6 +2922,39 @@ void QuadPlane::vtol_position_controller(void) } } +void QuadPlane::assign_tilt_to_fwd_thr(void) { +#if QAUTOTUNE_ENABLED + if (plane.control_mode == &plane.mode_qautotune) { + // limiting forward pitch and using forward throttle or rotor tilt is incompatible with auto-tune + q_fwd_throttle = 0.0f; + q_fwd_nav_pitch_lim_cd = -aparm.angle_max; + return; + } +#endif + if (is_positive(q_fwd_thr_gain) && is_positive(q_fwd_tilt_lim)) { + // Handle the case where we are limiting the forward pitch angle to prevent negative wing lift + // and are using the forward thrust motor or tilting rotors to provide the forward acceleration + float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f)); + q_fwd_nav_pitch_lim_cd = (int32_t)MIN(-100.0f * q_fwd_tilt_lim, 0.0f); + plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, q_fwd_nav_pitch_lim_cd); + q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f); + + // To prevent forward motor prop strike, reduce throttle to zero when close to ground + // When we are doing horizontal positioning in a VTOL land + // we always allow the fwd motor to run. Otherwise a bad + // lidar could cause the aircraft not to be able to + // approach the landing point when landing below the takeoff point + if (!in_vtol_land_approach()) { + float alt_cutoff = MAX(0,vel_forward_alt_cutoff); + float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); + float fwd_thr_scaler = linear_interpolate(0.0f, 1.0f, height_above_ground, alt_cutoff, alt_cutoff+2); + q_fwd_throttle *= fwd_thr_scaler; + } + + } else { + q_fwd_throttle = 0.0f; + } +} /* we want to limit WP speed to a lower speed when more than 20 degrees @@ -3021,6 +3080,8 @@ void QuadPlane::takeoff_controller(void) // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); + + assign_tilt_to_fwd_thr(); } run_xy_controller(); @@ -3078,6 +3139,8 @@ void QuadPlane::waypoint_controller(void) plane.nav_roll_cd = wp_nav->get_roll(); plane.nav_pitch_cd = wp_nav->get_pitch(); + assign_tilt_to_fwd_thr(); + if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) { pos_control->set_externally_limited_xy(); } @@ -3536,6 +3599,17 @@ void QuadPlane::Log_Write_QControl_Tuning() */ float QuadPlane::forward_throttle_pct() { + // handle special case where forward thrust motor is used instead of forward pitch. + // but not in autotune as it will upset the results +#if QAUTOTUNE_ENABLED + if (is_positive(q_fwd_thr_gain) && is_positive(q_fwd_tilt_lim) && plane.control_mode != &plane.mode_qautotune) { +#else + if (is_positive(q_fwd_thr_gain) && is_positive(q_fwd_thr_gain)) { +#endif + // handle special case where we are using forward throttle instead of forward tilt in Q modes + return 100.0f * q_fwd_throttle; + } + /* Unless an RC channel is assigned for manual forward throttle control, we don't use forward throttle in QHOVER or QSTABILIZE as they are the primary diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 33d4254134..e20cc33dc9 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -395,6 +395,12 @@ private: AP_Float acro_pitch_rate; AP_Float acro_yaw_rate; + // gain from forward acceleration to forward throttle + AP_Float q_fwd_thr_gain; + + // limit applied to forward pitch to prevent wing producing negative lift + AP_Float q_fwd_tilt_lim; + // time we last got an EKF yaw reset uint32_t ekfYawReset_ms; @@ -411,6 +417,9 @@ private: Location last_auto_target; + float q_fwd_throttle; // forward throttle used in q modes + int32_t q_fwd_nav_pitch_lim_cd; // forward tilt limit used in q modes in centi-degrees + // when did we last run the attitude controller? uint32_t last_att_control_ms; @@ -683,6 +692,11 @@ private: */ void set_desired_spool_state(AP_Motors::DesiredSpoolState state); + /* + limit forward pitch demand if using rotor tilt or forward flight motor to provide forward acceleration. + */ + void assign_tilt_to_fwd_thr(void); + /* get a scaled Q_WP_SPEED based on direction of movement */ diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 0c40e47d1b..b033c46dbf 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -258,21 +258,25 @@ void Tiltrotor::continuous_update(void) /* we are in a VTOL mode. We need to work out how much tilt is - needed. There are 4 strategies we will use: + needed. There are 5 strategies we will use: - 1) without manual forward throttle control, the angle will be set to zero - in QAUTOTUNE QACRO, QSTABILIZE and QHOVER. This - enables these modes to be used as a safe recovery mode. + 1) With use of a forward throttle controlled by Q_FWD_THR_GAIN in all + VTOL modes except Q_AUTOTUNE, we set the angle based on a calculated + forward throttle. - 2) with manual forward throttle control we will set the angle based on - the demanded forward throttle via RC input. + 2) With manual forward throttle control we set the angle based on the + RC input demanded forward throttle for QACRO, QSTABILIZE and QHOVER. - 3) in fixed wing assisted flight or velocity controlled modes we - will set the angle based on the demanded forward throttle, - with a maximum tilt given by Q_TILT_MAX. This relies on - Q_VFWD_GAIN being set. + 3) Without a RC input or calculated forward throttle value, the angle + will be set to zero in QAUTOTUNE, QACRO, QSTABILIZE and QHOVER. + This enables these modes to be used as a safe recovery mode. - 4) if we are in TRANSITION_TIMER mode then we are transitioning + 4) In fixed wing assisted flight or velocity controlled modes we will + set the angle based on the demanded forward throttle, with a maximum + tilt given by Q_TILT_MAX. This relies on Q_FWD_THR_GAIN or Q_VFWD_GAIN + being set. + + 5) if we are in TRANSITION_TIMER mode then we are transitioning to forward flight and should put the rotors all the way forward */ @@ -283,11 +287,23 @@ void Tiltrotor::continuous_update(void) } #endif - // if not in assisted flight and in QACRO, QSTABILIZE or QHOVER mode if (!quadplane.assisted_flight && - (plane.control_mode == &plane.mode_qacro || - plane.control_mode == &plane.mode_qstabilize || - plane.control_mode == &plane.mode_qhover)) { + is_positive(plane.quadplane.q_fwd_thr_gain) && + quadplane.is_flying_vtol()) + { + // We are using the rotor tilt functionality controlled by Q_FWD_THR_GAIN which can + // operate in all VTOL modes except Q_AUTOTUNE. Forward rotor tilt is used to produce + // forward thrust equivalent to what would have been produced by a forward thrust motor + // set to quadplane.forward_throttle_pct() + const float fwd_g_demand = 0.01f * quadplane.forward_throttle_pct() / plane.quadplane.q_fwd_thr_gain; + const float fwd_tilt_deg = MIN(degrees(atanf(fwd_g_demand)), (float)max_angle_deg); + slew(MIN(fwd_tilt_deg * (1/90.0), get_forward_flight_tilt())); + return; + } else if (!quadplane.assisted_flight && + (plane.control_mode == &plane.mode_qacro || + plane.control_mode == &plane.mode_qstabilize || + plane.control_mode == &plane.mode_qhover)) + { if (quadplane.rc_fwd_thr_ch == nullptr) { // no manual throttle control, set angle to zero slew(0);