mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 08:58:29 -04:00
Plane: Quadplane: add Q_OPTION to ignore forward flight angle limits in Q modes
This commit is contained in:
parent
e64a7b2884
commit
1d050a01ce
@ -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);
|
||||
|
||||
};
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
@ -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),
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user