diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index e1f7f28ff0..35b2ead353 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -651,12 +651,16 @@ void Plane::update_flight_mode(void) case QRTL: { // set nav_roll and nav_pitch using sticks int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max); - nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit; - nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); float pitch_input = channel_pitch->norm_input(); + // Scale from normalized input [-1,1] to centidegrees if (quadplane.tailsitter_active()) { - // For tailsitters, the pitch range is symmetrical: [-Q_ANGLE_MAX,Q_ANGLE_MAX] + // separate limit for tailsitter roll, if set + if (quadplane.tailsitter.max_roll_angle > 0) { + roll_limit = quadplane.tailsitter.max_roll_angle * 100.0f; + } + + // angle max for tailsitter pitch nav_pitch_cd = pitch_input * quadplane.aparm.angle_max; } else { // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose @@ -668,6 +672,10 @@ void Plane::update_flight_mode(void) } nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); } + + nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit; + nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); + break; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 774a5dcea4..5ad99f1d1a 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -369,6 +369,14 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @RebootRequired: True AP_GROUPINFO("TRIM_PITCH", 4, QuadPlane, ahrs_trim_pitch, 0), + // @Param: TAILSIT_RLL_MX + // @DisplayName: Maximum Roll angle + // @Description: Maximum Allowed roll angle for tailsitters. If this is zero then Q_ANGLE_MAX is used. + // @Units: deg + // @Range: 0 80 + // @User: Standard + AP_GROUPINFO("TAILSIT_RLL_MX", 5, QuadPlane, tailsitter.max_roll_angle, 0), + AP_GROUPEND }; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 45bd17ec9b..5cf5009302 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -413,6 +413,7 @@ private: AP_Float vectored_hover_gain; AP_Float vectored_hover_power; AP_Float throttle_scale_max; + AP_Float max_roll_angle; } tailsitter; // the attitude view of the VTOL attitude controller