Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: tailsitter: update I reset and limit handling

Browse Source
This commit is contained in:
Iampete1 2023-01-13 00:09:21 +00:00 committed by Andrew Tridgell
parent 29f124543b
commit 02ca9c855a
2 changed files with 27 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
ArduPlane/tailsitter.cpp
View File

@ -223,6 +223,8 @@ void Tailsitter::setup()
_have_elevator = SRV_Channels::function_assigned(SRV_Channel::k_elevator); _have_elevator = SRV_Channels::function_assigned(SRV_Channel::k_elevator);
_have_aileron = SRV_Channels::function_assigned(SRV_Channel::k_aileron); _have_aileron = SRV_Channels::function_assigned(SRV_Channel::k_aileron);
_have_rudder = SRV_Channels::function_assigned(SRV_Channel::k_rudder); _have_rudder = SRV_Channels::function_assigned(SRV_Channel::k_rudder);
_have_elevon = SRV_Channels::function_assigned(SRV_Channel::k_elevon_left) || SRV_Channels::function_assigned(SRV_Channel::k_elevon_right);
_have_v_tail = SRV_Channels::function_assigned(SRV_Channel::k_vtail_left) || SRV_Channels::function_assigned(SRV_Channel::k_vtail_right);
// set defaults for dual/single motor tailsitter // set defaults for dual/single motor tailsitter
if (quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) { if (quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) {
@ -362,9 +364,24 @@ void Tailsitter::output(void)
quadplane.motors_output(true); quadplane.motors_output(true);
if (quadplane.option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) { if (quadplane.option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) {
// only use motors for Q assist, control surfaces remain under plane control // only use motors for Q assist, control surfaces remain under plane control. Zero copter I terms and use plane.
// zero copter I terms and use plane // Smoothly relax to zero so there is no step change in output, must also set limit flags so integrator cannot build faster than the relax.
quadplane.attitude_control->reset_rate_controller_I_terms(); // Assume there is always roll and pitch control surfaces, otherwise motors only assist should not be set.
const float dt = quadplane.attitude_control->get_dt();
quadplane.attitude_control->get_rate_pitch_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC);
motors->limit.pitch = true;
quadplane.attitude_control->get_rate_yaw_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC);
motors->limit.yaw = true;
if (_have_rudder || _have_v_tail) {
// there are yaw control surfaces, zero motor I term
quadplane.attitude_control->get_rate_roll_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC);
motors->limit.roll = true;
} else {
// no yaw control surfaces, zero plane I terms and use motors
// We skip the outputing to surfaces for this axis from the copter controller but there are none setup
plane.yawController.reset_I();
}
// output tilt motors // output tilt motors
tilt_left = 0.0f; tilt_left = 0.0f;
@ -447,18 +464,18 @@ void Tailsitter::output(void)
if (is_positive(headroom)) { if (is_positive(headroom)) {
if (fabsf(aileron_mix) > headroom) { if (fabsf(aileron_mix) > headroom) {
aileron_mix *= headroom / fabsf(aileron_mix); aileron_mix *= headroom / fabsf(aileron_mix);
yaw_lim = true; yaw_lim |= _have_elevon;
} }
if (fabsf(rudder_mix) > headroom) { if (fabsf(rudder_mix) > headroom) {
rudder_mix *= headroom / fabsf(rudder_mix); rudder_mix *= headroom / fabsf(rudder_mix);
roll_lim = true; roll_lim |= _have_v_tail;
} }
} else { } else {
aileron_mix = 0.0; aileron_mix = 0.0;
rudder_mix = 0.0; rudder_mix = 0.0;
roll_lim = true; yaw_lim |= _have_elevon;
pitch_lim = true; pitch_lim |= _have_elevon || _have_v_tail;
yaw_lim = true; roll_lim |= _have_v_tail;
} }
SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, elevator_mix - aileron_mix); SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, elevator_mix - aileron_mix);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, elevator_mix + aileron_mix); SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, elevator_mix + aileron_mix);

2
ArduPlane/tailsitter.h
View File

@ -121,6 +121,8 @@ private:
bool _have_elevator; bool _have_elevator;
bool _have_aileron; bool _have_aileron;
bool _have_rudder; bool _have_rudder;
bool _have_elevon;
bool _have_v_tail;
// refences for convenience // refences for convenience
QuadPlane& quadplane; QuadPlane& quadplane;