Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-05 21:23:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow Qassist for tailsitters

Browse Source
This commit is contained in:
Peter Hall 2020-01-02 19:07:09 +00:00 committed by Andrew Tridgell
parent 2467d7b5c7
commit 87cd9a4791
3 changed files with 84 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
ArduPlane/quadplane.cpp
View File

@ -827,52 +827,53 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
{
check_attitude_relax();
// tailsitter-only body-frame roll control options
// Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw.
if (is_tailsitter() &&
(tailsitter.input_type & TAILSITTER_INPUT_BF_ROLL)) {
if (!(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) {
// In multicopter input mode, the roll and yaw stick axes are independent of pitch
attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(false,
plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate_cds);
return;
} else {
// In plane input mode, the roll and yaw sticks are swapped
// and their effective axes rotate from yaw to roll and vice versa
// as pitch goes from zero to 90.
// So it is necessary to also rotate their scaling.
// Get the roll angle and yaw rate limits
int16_t roll_limit = aparm.angle_max;
// separate limit for tailsitter roll, if set
if (tailsitter.max_roll_angle > 0) {
roll_limit = tailsitter.max_roll_angle * 100.0f;
}
// Prevent a divide by zero
float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f;
float yaw2roll_scale = roll_limit / yaw_rate_limit;
// Rotate as a function of Euler pitch and swap roll/yaw
float euler_pitch = radians(.01f * plane.nav_pitch_cd);
float spitch = fabsf(sinf(euler_pitch));
float y2r_scale = linear_interpolate(1, yaw2roll_scale, spitch, 0, 1);
float p_yaw_rate = plane.nav_roll_cd / y2r_scale;
float p_roll_angle = -y2r_scale * yaw_rate_cds;
attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(true,
p_roll_angle,
plane.nav_pitch_cd,
p_yaw_rate);
return;
}
}
// normal control modes for VTOL and FW flight
if (in_vtol_mode() || is_tailsitter()) {
if (in_vtol_mode()) {
// tailsitter-only body-frame roll control options
// Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw.
if (is_tailsitter() &&
(tailsitter.input_type & TAILSITTER_INPUT_BF_ROLL)) {
if (!(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) {
// In multicopter input mode, the roll and yaw stick axes are independent of pitch
attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(false,
plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate_cds);
return;
} else {
// In plane input mode, the roll and yaw sticks are swapped
// and their effective axes rotate from yaw to roll and vice versa
// as pitch goes from zero to 90.
// So it is necessary to also rotate their scaling.
// Get the roll angle and yaw rate limits
int16_t roll_limit = aparm.angle_max;
// separate limit for tailsitter roll, if set
if (tailsitter.max_roll_angle > 0) {
roll_limit = tailsitter.max_roll_angle * 100.0f;
}
// Prevent a divide by zero
float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f;
float yaw2roll_scale = roll_limit / yaw_rate_limit;
// Rotate as a function of Euler pitch and swap roll/yaw
float euler_pitch = radians(.01f * plane.nav_pitch_cd);
float spitch = fabsf(sinf(euler_pitch));
float y2r_scale = linear_interpolate(1, yaw2roll_scale, spitch, 0, 1);
float p_yaw_rate = plane.nav_roll_cd / y2r_scale;
float p_roll_angle = -y2r_scale * yaw_rate_cds;
attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll(true,
p_roll_angle,
plane.nav_pitch_cd,
p_yaw_rate);
return;
}
}
// use euler angle attitude control
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
@ -881,7 +882,12 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
// use the fixed wing desired rates
float roll_rate = plane.rollController.get_pid_info().target;
float pitch_rate = plane.pitchController.get_pid_info().target;
attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds);
if (is_tailsitter()) {
// tailsitter roll and yaw swapped due to change in reference frame
attitude_control->input_rate_bf_roll_pitch_yaw_2(yaw_rate_cds,pitch_rate*100.0f, -roll_rate*100.0f);
} else {
attitude_control->input_rate_bf_roll_pitch_yaw_2(roll_rate*100.0f, pitch_rate*100.0f, yaw_rate_cds);
}
}
}
@ -900,7 +906,12 @@ void QuadPlane::hold_stabilize(float throttle_in)
}
} else {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
attitude_control->set_throttle_out(throttle_in, true, 0);
bool should_boost = true;
if (is_tailsitter() && assisted_flight) {
// tailsitters in forward flight should not use angle boost
should_boost = false;
}
attitude_control->set_throttle_out(throttle_in, should_boost, 0);
}
}
@ -1563,12 +1574,12 @@ void QuadPlane::update_transition(void)
/*
see if we should provide some assistance
*/
bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
plane.get_throttle_input()>0 ||
plane.is_flying());
const bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) ||
plane.get_throttle_input()>0 ||
plane.is_flying());
if (q_assist_safe &&
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE ||
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) {
(q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed) && !is_contol_surface_tailsitter()))) {
// the quad should provide some assistance to the plane
assisted_flight = true;
if (!is_tailsitter()) {

3
ArduPlane/quadplane.h
View File

@ -110,6 +110,9 @@ public:
// return true when tailsitter frame configured
bool is_tailsitter(void) const;
// return true when flying a control surface only tailsitter tailsitter
bool is_contol_surface_tailsitter(void) const;
// return true when flying a tailsitter in VTOL
bool tailsitter_active(void);

23
ArduPlane/tailsitter.cpp
View File

@ -26,11 +26,20 @@
*/
bool QuadPlane::is_tailsitter(void) const
{
return available()
return available()
&& ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (tailsitter.motor_mask != 0))
&& (tilt.tilt_type != TILT_TYPE_BICOPTER);
}
/*
return true when flying a control surface only tailsitter tailsitter
*/
bool QuadPlane::is_contol_surface_tailsitter(void) const
{
return frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER
&& ( is_zero(tailsitter.vectored_hover_gain) || !SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft));
}
/*
check if we are flying as a tailsitter
*/
@ -64,7 +73,7 @@ void QuadPlane::tailsitter_output(void)
uint16_t mask = tailsitter.motor_mask;
// handle forward flight modes and transition to VTOL modes
if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
if ((!tailsitter_active() || in_tailsitter_vtol_transition()) && !assisted_flight) {
// in forward flight: set motor tilt servos and throttles using FW controller
if (tailsitter.vectored_forward_gain > 0) {
// thrust vectoring in fixed wing flight
@ -102,8 +111,14 @@ void QuadPlane::tailsitter_output(void)
// handle VTOL modes
// the MultiCopter rate controller has already been run in an earlier call
// to motors_output() from quadplane.update()
motors_output(false);
// to motors_output() from quadplane.update(), unless we are in assisted flight
if (assisted_flight && tailsitter_transition_fw_complete()) {
hold_stabilize(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
motors_output(true);
} else {
motors_output(false);
}
plane.pitchController.reset_I();
plane.rollController.reset_I();