From e94cf561d0cfd4f926e1adfa65ae536f78217e29 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Dec 2020 15:03:42 +1100 Subject: [PATCH] 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 --- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/quadplane.cpp | 70 +++++++++++++++++++++++++++++++++++---- ArduPlane/quadplane.h | 6 +++- 3 files changed, 70 insertions(+), 8 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 73631cce6d..ade326f0b2 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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(); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 452582808d..e8937efe04 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 91b2cc3113..fa471cf00b 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -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;