diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 661cf23816..77db49d0f5 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -444,6 +444,10 @@ public: bool _enter() override; protected: +private: + + void set_tailsitter_roll_pitch(const float roll_input, const float pitch_input); + void set_limited_roll_pitch(const float roll_input, const float pitch_input); }; diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 7fb513aa6e..703fc5b3f7 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -15,29 +15,50 @@ bool ModeQStabilize::_enter() void ModeQStabilize::update() { // set nav_roll and nav_pitch using sticks - int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); - float pitch_input = plane.channel_pitch->norm_input(); - // Scale from normalized input [-1,1] to centidegrees - if (plane.quadplane.tailsitter_active()) { - // separate limit for tailsitter roll, if set - if (plane.quadplane.tailsitter.max_roll_angle > 0) { - roll_limit = plane.quadplane.tailsitter.max_roll_angle * 100.0f; - } + // Scale from normalized input [-1,1] to target angles in centidegrees + const float roll_input = plane.channel_roll->norm_input(); + const float pitch_input = plane.channel_pitch->norm_input(); - // angle max for tailsitter pitch - plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max; - } else { - // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose - // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX - if (pitch_input > 0) { - plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max); - } else { - plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max); - } - plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + if (plane.quadplane.tailsitter_active()) { + // tailsitters are different + set_tailsitter_roll_pitch(roll_input, pitch_input); + return; } - plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit; - plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit); + if ((plane.quadplane.options & QuadPlane::OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES) == 0) { + // by default angles are also constrained by forward flight limits + set_limited_roll_pitch(roll_input, pitch_input); + } else { + // use angle max for both roll and pitch + plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max; + plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max; + } } +// set the desired roll and pitch for a tailsitter +void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const float pitch_input) +{ + // separate limit for roll, if set + if (plane.quadplane.tailsitter.max_roll_angle > 0) { + // roll param is in degrees not centidegrees + plane.nav_roll_cd = plane.quadplane.tailsitter.max_roll_angle * 100.0f * roll_input; + } else { + plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max; + } + + // angle max for tailsitter pitch + plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max; +} + +// set the desired roll and pitch for normal quadplanes, also limited by forward flight limtis +void ModeQStabilize::set_limited_roll_pitch(const float roll_input, const float pitch_input) +{ + plane.nav_roll_cd = roll_input * MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); + // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose + // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX + if (pitch_input > 0) { + plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max); + } else { + plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max); + } +} diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3d29b8b4f6..e9a3190911 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -330,7 +330,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: OPTIONS // @DisplayName: quadplane options // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Use FW Approach:Use a fixed wing approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming. - // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,9:Airmode_On_Arm,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation + // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,9:Airmode_On_Arm,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 29e9c97459..4998cdc133 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -597,6 +597,7 @@ private: OPTION_DELAY_ARMING=(1<<11), OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12), OPTION_DISABLE_GROUND_EFFECT_COMP=(1<<13), + OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES=(1<<14), }; AP_Float takeoff_failure_scalar;