diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 6251422841..0d3b34dbdc 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -270,14 +270,14 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_GROUPINFO("TILT_MASK", 37, QuadPlane, tilt.tilt_mask, 0), - // @Param: TILT_RATE - // @DisplayName: Tiltrotor tilt rate - // @Description: This is the maximum speed at which the motor angle will change for a tiltrotor + // @Param: TILT_RATE_UP + // @DisplayName: Tiltrotor upwards tilt rate + // @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from forward flight to hover // @Units: degrees/second // @Increment: 1 // @Range: 10 300 // @User: Standard - AP_GROUPINFO("TILT_RATE", 38, QuadPlane, tilt.max_rate_dps, 40), + AP_GROUPINFO("TILT_RATE_UP", 38, QuadPlane, tilt.max_rate_up_dps, 40), // @Param: TILT_MAX // @DisplayName: Tiltrotor maximum VTOL angle @@ -339,7 +339,16 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Description: This is the angle at which tailsitter aircraft will change from VTOL control to fixed wing control. // @Range: 5 80 AP_GROUPINFO("TAILSIT_ANGLE", 48, QuadPlane, tailsitter.transition_angle, 30), - + + // @Param: TILT_RATE_DN + // @DisplayName: Tiltrotor downwards tilt rate + // @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from hover to forward flight. When this is zero the Q_TILT_RATE_UP value is used. + // @Units: degrees/second + // @Increment: 1 + // @Range: 10 300 + // @User: Standard + AP_GROUPINFO("TILT_RATE_DN", 49, QuadPlane, tilt.max_rate_down_dps, 0), + AP_GROUPEND }; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 7599853209..b291d0b38f 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -319,7 +319,8 @@ private: // tiltrotor control variables struct { AP_Int16 tilt_mask; - AP_Int16 max_rate_dps; + AP_Int16 max_rate_up_dps; + AP_Int16 max_rate_down_dps; AP_Int8 max_angle_deg; AP_Int8 tilt_type; float current_tilt; @@ -345,6 +346,7 @@ private: void tiltrotor_binary_update(void); void tilt_compensate(float *thrust, uint8_t num_motors); bool tiltrotor_fully_fwd(void); + float tilt_max_change(bool up); void afs_terminate(void); bool guided_mode_enabled(void); diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index ca286c7f09..38b5c7efef 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -6,12 +6,30 @@ */ +/* + calculate maximum tilt change as a proportion from 0 to 1 of tilt + */ +float QuadPlane::tilt_max_change(bool up) +{ + float rate; + if (up || tilt.max_rate_down_dps <= 0) { + rate = tilt.max_rate_up_dps; + } else { + rate = tilt.max_rate_down_dps; + } + if (plane.control_mode == MANUAL && tilt.tilt_type != TILT_TYPE_BINARY) { + // allow a minimum of 90 DPS in manual, to give fast control + rate = MAX(rate, 90); + } + return rate * plane.G_Dt / 90.0f; +} + /* output a slew limited tiltrotor angle. tilt is from 0 to 1 */ void QuadPlane::tiltrotor_slew(float newtilt) { - float max_change = (tilt.max_rate_dps.get() * plane.G_Dt) / 90.0f; + float max_change = tilt_max_change(newtiltget_throttle(), + float motors_throttle = motors->get_throttle(); + max_change = tilt_max_change(motors_throttle