From 6116eaeb3246c4d741b0f394109d2d21a3f56a8e Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 22 Nov 2021 02:18:19 +0000 Subject: [PATCH] Plane: quadplane: enhance tiltrotor transtion to better deal with tiltrotors with all motors tilting --- ArduPlane/quadplane.cpp | 14 ++++++++++++++ ArduPlane/tiltrotor.cpp | 16 ++++++++++++++++ ArduPlane/tiltrotor.h | 10 ++++++++++ ArduPlane/transition.h | 3 +++ 4 files changed, 43 insertions(+) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index d073221fdf..8b88319192 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1477,6 +1477,7 @@ void SLT_Transition::update() transition_low_airspeed_ms = now; if (have_airspeed && aspeed > plane.aparm.airspeed_min && !quadplane.assisted_flight) { transition_state = TRANSITION_TIMER; + airspeed_reached_tilt = quadplane.tiltrotor.current_tilt; gcs().send_text(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", (double)aspeed); } quadplane.assisted_flight = true; @@ -1502,6 +1503,11 @@ void SLT_Transition::update() quadplane.attitude_control->reset_yaw_target_and_rate(); quadplane.attitude_control->rate_bf_yaw_target(quadplane.ahrs.get_gyro().z); } + if (quadplane.tiltrotor.enabled() && !quadplane.tiltrotor.has_fw_motor()) { + // tilt rotors without decidated fw motors do not have forward throttle output in this stage + // prevent throttle I wind up + plane.TECS_controller.reset_throttle_I(); + } last_throttle = motors->get_throttle(); @@ -1541,6 +1547,14 @@ void SLT_Transition::update() // will stop stabilizing throttle_scaled = 0.01; } + if (quadplane.tiltrotor.enabled() && !quadplane.tiltrotor.has_vtol_motor() && !quadplane.tiltrotor.has_fw_motor()) { + // All motors tilting, Use a combination of vertical and forward throttle based on curent tilt angle + // scale from all VTOL throttle at airspeed_reached_tilt to all forward throttle at fully forward tilt + // this removes a step change in throttle once assistance is stoped + const float ratio = (constrain_float(quadplane.tiltrotor.current_tilt, airspeed_reached_tilt, quadplane.tiltrotor.get_fully_forward_tilt()) - airspeed_reached_tilt) / (quadplane.tiltrotor.get_fully_forward_tilt() - airspeed_reached_tilt); + const float fw_throttle = MAX(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),0) * 0.01; + throttle_scaled = constrain_float(throttle_scaled * (1.0-ratio) + fw_throttle * ratio, 0.0, 1.0); + } quadplane.assisted_flight = true; quadplane.hold_stabilize(throttle_scaled); diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index d204813254..c9bb89dc92 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -107,6 +107,22 @@ void Tiltrotor::setup() _is_vectored = tilt_mask != 0 && type == TILT_TYPE_VECTORED_YAW; + // true if a fixed forward motor is configured, either throttle, throttle left or throttle right. + // bicopter tiltrotors use throttle left and right as tilting motors, so they don't count in that case. + _have_fw_motor = SRV_Channels::function_assigned(SRV_Channel::k_throttle) || + ((SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) || SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) + && (type != TILT_TYPE_BICOPTER)); + + + // check if there are any perminant VTOL motors + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) { + if (motors->is_motor_enabled(i) && ((tilt_mask & (1U<<1)) == 0)) { + // enabled motor not set in tilt mask + _have_vtol_motor = true; + break; + } + } + if (quadplane.motors_var_info == AP_MotorsMatrix::var_info && _is_vectored) { // we will be using vectoring for yaw motors->disable_yaw_torque(); diff --git a/ArduPlane/tiltrotor.h b/ArduPlane/tiltrotor.h index 150c6b5d16..3d63367e4c 100644 --- a/ArduPlane/tiltrotor.h +++ b/ArduPlane/tiltrotor.h @@ -57,6 +57,10 @@ public: bool is_vectored() const { return enabled() && _is_vectored; } + bool has_fw_motor() const { return _have_fw_motor; } + + bool has_vtol_motor() const { return _have_vtol_motor; } + bool motors_active() const { return enabled() && _motors_active; } AP_Int8 enable; @@ -90,6 +94,12 @@ private: bool setup_complete; + // true if a fixed forward motor is setup + bool _have_fw_motor; + + // true if all motors tilt with no fixed VTOL motor + bool _have_vtol_motor; + // refences for convenience QuadPlane& quadplane; AP_MotorsMulticopter*& motors; diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index ea15b9a749..8b00fd6088 100644 --- a/ArduPlane/transition.h +++ b/ArduPlane/transition.h @@ -110,5 +110,8 @@ protected: uint32_t last_fw_mode_ms; int32_t last_fw_nav_pitch_cd; + // tiltrotor tilt angle when airspeed wait transition stage completes + float airspeed_reached_tilt; + };