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

Plane: use rull euler control for fwd transition of tilt vectored planes 

this gives strong yaw control and wind handling for tilt vectored
planes in forward transitions. It relaxes the yaw if the user either
demands yaw with stick input or plane navigation is demanding a roll
angle for a turn

When navigation is demanded we setup yaw rate for a coordinated turn
This commit is contained in:
Andrew Tridgell 2020-12-15 15:03:42 +11:00
parent abd5f8351d
commit e94cf561d0
3 changed files with 70 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -166,7 +166,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const
return;
}
const QuadPlane &quadplane = plane.quadplane;
if (quadplane.in_vtol_mode()) {
if (quadplane.show_vtol_view()) {
const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
bool wp_nav_valid = quadplane.using_wp_nav();

70
ArduPlane/quadplane.cpp
View File

@ -840,6 +840,38 @@ void QuadPlane::init_stabilize(void)
throttle_wait = false;
}
/*
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) {
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 = degrees(GRAVITY_MSS/MAX(aspeed,airspeed_min) * sinf(radians(plane.nav_roll_cd*0.01)))*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
@ -849,9 +881,20 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
{
check_attitude_relax();
bool use_multicopter_control = in_vtol_mode() && !in_tailsitter_vtol_transition();
bool use_multicopter_eulers = false;
if (!use_multicopter_control &&
tilt.is_vectored &&
transition_state <= TRANSITION_TIMER) {
update_yaw_target();
use_multicopter_control = true;
use_multicopter_eulers = true;
}
// normal control modes for VTOL and FW flight
// tailsitter in transition to VTOL flight is not really in a VTOL mode yet
if (in_vtol_mode() && !in_tailsitter_vtol_transition()) {
if (use_multicopter_control) {
// tailsitter-only body-frame roll control options
// Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw.
@ -897,10 +940,17 @@ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
}
}
// use euler angle attitude control
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
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,
true);
} else {
// use euler angle attitude control
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate_cds);
}
} else {
// use the fixed wing desired rates
float roll_rate = plane.rollController.get_pid_info().target;
@ -1689,6 +1739,9 @@ void QuadPlane::update_transition(void)
// 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 (transition_state == TRANSITION_TIMER) {
gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done");
}
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
transition_low_airspeed_ms = 0;
@ -1737,7 +1790,7 @@ void QuadPlane::update_transition(void)
}
hold_hover(climb_rate_cms);
if (!tilt.is_vectored || now - transition_start_ms < 100) {
if (!tilt.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
@ -3472,6 +3525,11 @@ bool QuadPlane::show_vtol_view() const
return true;
}
}
if (!show_vtol && tilt.is_vectored && transition_state <= TRANSITION_TIMER) {
// we use multirotor controls during fwd transition for
// vectored yaw vehicles
return true;
}
return show_vtol;
}

6
ArduPlane/quadplane.h
View File

@ -236,6 +236,9 @@ private:
// use multicopter rate controller
void multicopter_attitude_rate_update(float yaw_rate_cds);
// update yaw target for tiltrotor transition
void update_yaw_target();
// main entry points for VTOL flight modes
void init_stabilize(void);
@ -480,7 +483,8 @@ private:
float current_tilt;
float current_throttle;
bool motors_active:1;
float transition_yaw;
float transition_yaw_cd;
uint32_t transition_yaw_set_ms;
bool is_vectored;
} tilt;