Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Quadplane: add Q_OPTION to ignore forward flight angle limits in Q modes

Browse Source
This commit is contained in:
Iampete1 2021-02-19 18:30:26 +00:00 committed by Andrew Tridgell
parent e64a7b2884
commit 1d050a01ce
4 changed files with 48 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/mode.h
View File

@ -444,6 +444,10 @@ public:
bool _enter() override; bool _enter() override;
protected: 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);
}; };

63
ArduPlane/mode_qstabilize.cpp
View File

@ -15,29 +15,50 @@ bool ModeQStabilize::_enter()
void ModeQStabilize::update() void ModeQStabilize::update()
{ {
// set nav_roll and nav_pitch using sticks // set nav_roll and nav_pitch using sticks
int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); // Scale from normalized input [-1,1] to target angles in centidegrees
float pitch_input = plane.channel_pitch->norm_input(); const float roll_input = plane.channel_roll->norm_input();
// Scale from normalized input [-1,1] to centidegrees const float pitch_input = plane.channel_pitch->norm_input();
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;
}
// angle max for tailsitter pitch if (plane.quadplane.tailsitter_active()) {
plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max; // tailsitters are different
} else { set_tailsitter_roll_pitch(roll_input, pitch_input);
// pitch is further constrained by LIM_PITCH_MIN/MAX which may impose return;
// 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());
} }
plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit; if ((plane.quadplane.options & QuadPlane::OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES) == 0) {
plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit); // 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);
}
}

2
ArduPlane/quadplane.cpp
View File

@ -330,7 +330,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Param: OPTIONS // @Param: OPTIONS
// @DisplayName: quadplane 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. // @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_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),

1
ArduPlane/quadplane.h
View File

@ -597,6 +597,7 @@ private:
OPTION_DELAY_ARMING=(1<<11), OPTION_DELAY_ARMING=(1<<11),
OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12), OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12),
OPTION_DISABLE_GROUND_EFFECT_COMP=(1<<13), OPTION_DISABLE_GROUND_EFFECT_COMP=(1<<13),
OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES=(1<<14),
}; };
AP_Float takeoff_failure_scalar; AP_Float takeoff_failure_scalar;