Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Improve use of forward flight motors and tilting rotors in Q modes

This commit is contained in:
Paul Riseborough 2023-08-13 17:08:28 +10:00 committed by Andrew Tridgell
parent 01f94e9aa5
commit 2d3431a1ac
11 changed files with 151 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/mode_auto.cpp
View File

@ -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;
}

3
ArduPlane/mode_qacro.cpp
View File

@ -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();

3
ArduPlane/mode_qautotune.cpp
View File

@ -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

3
ArduPlane/mode_qhover.cpp
View File

@ -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);

3
ArduPlane/mode_qland.cpp
View File

@ -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();

5
ArduPlane/mode_qloiter.cpp
View File

@ -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();
}

6
ArduPlane/mode_qrtl.cpp
View File

@ -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();
}

5
ArduPlane/mode_qstabilize.cpp
View File

@ -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

78
ArduPlane/quadplane.cpp
View File

@ -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

14
ArduPlane/quadplane.h
View File

@ -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
*/

46
ArduPlane/tiltrotor.cpp
View File

@ -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);