mirror of https://github.com/ArduPilot/ardupilot
Plane: QuadPlane: move tailsitter to class
This commit is contained in:
parent
9047b19e59
commit
2bc2b9533b
|
@ -271,19 +271,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
|||
// @Values: 0:Continuous,1:Binary,2:VectoredYaw,3:Bicopter
|
||||
AP_GROUPINFO("TILT_TYPE", 47, QuadPlane, tilt.tilt_type, TILT_TYPE_CONTINUOUS),
|
||||
|
||||
// @Param: TAILSIT_ANGLE
|
||||
// @DisplayName: Tailsitter fixed wing transition angle
|
||||
// @Description: This is the pitch angle at which tailsitter aircraft will change from VTOL control to fixed wing control.
|
||||
// @Units: deg
|
||||
// @Range: 5 80
|
||||
AP_GROUPINFO("TAILSIT_ANGLE", 48, QuadPlane, tailsitter.transition_angle_fw, 45),
|
||||
|
||||
// @Param: TAILSIT_ANG_VT
|
||||
// @DisplayName: Tailsitter VTOL transition angle
|
||||
// @Description: This is the pitch angle at which tailsitter aircraft will change from fixed wing control to VTOL control, if zero Q_TAILSIT_ANGLE will be used
|
||||
// @Units: deg
|
||||
// @Range: 5 80
|
||||
AP_GROUPINFO("TAILSIT_ANG_VT", 61, QuadPlane, tailsitter.transition_angle_vtol, 0),
|
||||
// 48: TAILSIT_ANGLE
|
||||
// 61: TAILSIT_ANG_VT
|
||||
|
||||
// @Param: TILT_RATE_DN
|
||||
// @DisplayName: Tiltrotor downwards tilt rate
|
||||
|
@ -293,38 +282,12 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
|||
// @Range: 10 300
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TILT_RATE_DN", 49, QuadPlane, tilt.max_rate_down_dps, 0),
|
||||
|
||||
// @Param: TAILSIT_INPUT
|
||||
// @DisplayName: Tailsitter input type bitmask
|
||||
// @Description: This controls whether stick input when hovering as a tailsitter follows the conventions for fixed wing hovering or multicopter hovering. When PlaneMode is not enabled (bit0 = 0) the roll stick will roll the aircraft in earth frame and yaw stick will yaw in earth frame. When PlaneMode input is enabled, the roll and yaw sticks are swapped so that the roll stick controls earth-frame yaw and rudder controls earth-frame roll. When body-frame roll is enabled (bit1 = 1), the yaw stick controls earth-frame yaw rate and the roll stick controls roll in the tailsitter's body frame when flying level.
|
||||
// @Bitmask: 0:PlaneMode,1:BodyFrameRoll
|
||||
AP_GROUPINFO("TAILSIT_INPUT", 50, QuadPlane, tailsitter.input_type, 0),
|
||||
|
||||
// @Param: TAILSIT_MASK
|
||||
// @DisplayName: Tailsitter input mask
|
||||
// @Description: This controls what channels have full manual control when hovering as a tailsitter and the Q_TAILSIT_MASKCH channel in high. This can be used to teach yourself to prop-hang a 3D plane by learning one or more channels at a time.
|
||||
// @Bitmask: 0:Aileron,1:Elevator,2:Throttle,3:Rudder
|
||||
AP_GROUPINFO("TAILSIT_MASK", 51, QuadPlane, tailsitter.input_mask, 0),
|
||||
|
||||
// @Param: TAILSIT_MASKCH
|
||||
// @DisplayName: Tailsitter input mask channel
|
||||
// @Description: This controls what input channel will activate the Q_TAILSIT_MASK mask. When this channel goes above 1700 then the pilot will have direct manual control of the output channels specified in Q_TAILSIT_MASK. Set to zero to disable.
|
||||
// @Values: 0:Disabled,1:Channel1,2:Channel2,3:Channel3,4:Channel4,5:Channel5,6:Channel6,7:Channel7,8:Channel8
|
||||
AP_GROUPINFO("TAILSIT_MASKCH", 52, QuadPlane, tailsitter.input_mask_chan, 0),
|
||||
|
||||
// @Param: TAILSIT_VFGAIN
|
||||
// @DisplayName: Tailsitter vector thrust gain in forward flight
|
||||
// @Description: This controls the amount of vectored thrust control used in forward flight for a vectored tailsitter
|
||||
// @Range: 0 1
|
||||
// @Increment: 0.01
|
||||
AP_GROUPINFO("TAILSIT_VFGAIN", 53, QuadPlane, tailsitter.vectored_forward_gain, 0),
|
||||
|
||||
// @Param: TAILSIT_VHGAIN
|
||||
// @DisplayName: Tailsitter vector thrust gain in hover
|
||||
// @Description: This controls the amount of vectored thrust control used in hover for a vectored tailsitter
|
||||
// @Range: 0 1
|
||||
// @Increment: 0.01
|
||||
AP_GROUPINFO("TAILSIT_VHGAIN", 54, QuadPlane, tailsitter.vectored_hover_gain, 0.5),
|
||||
// 50: TAILSIT_INPUT
|
||||
// 51: TAILSIT_MASK
|
||||
// 52: TAILSIT_MASKCH
|
||||
// 53: TAILSIT_VFGAIN
|
||||
// 54: TAILSIT_VHGAIN
|
||||
|
||||
// @Param: TILT_YAW_ANGLE
|
||||
// @DisplayName: Tilt minimum angle for vectored yaw
|
||||
|
@ -332,12 +295,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
|||
// @Range: 0 30
|
||||
AP_GROUPINFO("TILT_YAW_ANGLE", 55, QuadPlane, tilt.tilt_yaw_angle, 0),
|
||||
|
||||
// @Param: TAILSIT_VHPOW
|
||||
// @DisplayName: Tailsitter vector thrust gain power
|
||||
// @Description: This controls the amount of extra pitch given to the vectored control when at high pitch errors
|
||||
// @Range: 0 4
|
||||
// @Increment: 0.1
|
||||
AP_GROUPINFO("TAILSIT_VHPOW", 56, QuadPlane, tailsitter.vectored_hover_power, 2.5),
|
||||
// 56: TAILSIT_VHPOW
|
||||
|
||||
// @Param: MAV_TYPE
|
||||
// @DisplayName: MAVLink type identifier
|
||||
|
@ -354,7 +312,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
|||
AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
|
||||
|
||||
// 60 is used above for VELZ_MAX_DN
|
||||
// 61 is used above for TS_ANGLE_VTOL
|
||||
// 61 was used above for TAILSIT_ANG_VT
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
@ -375,12 +333,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
|
|||
// @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
|
||||
AP_SUBGROUPPTR(loiter_nav, "LOIT_", 2, QuadPlane, AC_Loiter),
|
||||
|
||||
// @Param: TAILSIT_GSCMAX
|
||||
// @DisplayName: Maximum tailsitter gain scaling
|
||||
// @Description: Maximum gain scaling for tailsitter Q_TAILSIT_GSCMSK options
|
||||
// @Range: 1 5
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TAILSIT_GSCMAX", 3, QuadPlane, tailsitter.throttle_scale_max, 2),
|
||||
// 3: TAILSIT_GSCMAX
|
||||
|
||||
// @Param: TRIM_PITCH
|
||||
// @DisplayName: Quadplane AHRS trim pitch
|
||||
|
@ -392,13 +345,7 @@ 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),
|
||||
// 5: TAILSIT_RLL_MX
|
||||
|
||||
#if QAUTOTUNE_ENABLED
|
||||
// @Group: AUTOTUNE_
|
||||
|
@ -424,12 +371,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
|
|||
// @User: Advanced
|
||||
AP_GROUPINFO("TRANS_FAIL", 8, QuadPlane, transition_failure, 0),
|
||||
|
||||
// @Param: TAILSIT_MOTMX
|
||||
// @DisplayName: Tailsitter motor mask
|
||||
// @Description: Bitmask of motors to remain active in forward flight for a 'Copter' tailsitter. Non-zero indicates airframe is a Copter tailsitter and uses copter style motor layouts determined by Q_FRAME_CLASS and Q_FRAME_TYPE. This should be zero for non-Copter tailsitters.
|
||||
// @User: Standard
|
||||
// @Bitmask: 0:Motor 1,1:Motor 2,2:Motor 3,3:Motor 4, 4:Motor 5,5:Motor 6,6:Motor 7,7:Motor 8
|
||||
AP_GROUPINFO("TAILSIT_MOTMX", 9, QuadPlane, tailsitter.motor_mask, 0),
|
||||
// 9: TAILSIT_MOTMX
|
||||
|
||||
// @Param: THROTTLE_EXPO
|
||||
// @DisplayName: Throttle expo strength
|
||||
|
@ -492,19 +434,8 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist_alt, 0),
|
||||
|
||||
// @Param: TAILSIT_GSCMSK
|
||||
// @DisplayName: Tailsitter gain scaling mask
|
||||
// @Description: Bitmask of gain scaling methods to be applied: Throttle: scale gains with throttle, ATT_THR: reduce gain at high throttle/tilt, 2:Disk theory velocity calculation, requires Q_TAILSIT_DSKLD to be set, ATT_THR must not be set, 3:Altitude correction, scale with air density
|
||||
// @User: Standard
|
||||
// @Bitmask: 0:Throttle,1:ATT_THR,2:Disk Theory,3:Altitude correction
|
||||
AP_GROUPINFO("TAILSIT_GSCMSK", 17, QuadPlane, tailsitter.gain_scaling_mask, TAILSITTER_GSCL_THROTTLE),
|
||||
|
||||
// @Param: TAILSIT_GSCMIN
|
||||
// @DisplayName: Minimum tailsitter gain scaling
|
||||
// @Description: Minimum gain scaling for tailsitter Q_TAILSIT_GSCMSK options
|
||||
// @Range: 0.1 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TAILSIT_GSCMIN", 18, QuadPlane, tailsitter.gain_scaling_min, 0.4),
|
||||
// 17: TAILSIT_GSCMSK
|
||||
// 18: TAILSIT_GSCMIN
|
||||
|
||||
// @Param: ASSIST_DELAY
|
||||
// @DisplayName: Quadplane assistance delay
|
||||
|
@ -522,13 +453,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
|
|||
// @RebootRequired: False
|
||||
AP_GROUPINFO("FWD_MANTHR_MAX", 20, QuadPlane, fwd_thr_max, 0),
|
||||
|
||||
// @Param: TAILSIT_DSKLD
|
||||
// @DisplayName: Tailsitter disk loading
|
||||
// @Description: This is the vehicle weight in kg divided by the total disk area of all propellers in m^2. Only used with Q_TAILSIT_GSCMSK = 4
|
||||
// @Units: kg/m/m
|
||||
// @Range: 0 50
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("TAILSIT_DSKLD", 21, QuadPlane, tailsitter.disk_loading, 0),
|
||||
// 21: TAILSIT_DSKLD
|
||||
|
||||
// @Param: TILT_FIX_ANGLE
|
||||
// @DisplayName: Fixed wing tiltrotor angle
|
||||
|
@ -545,19 +470,12 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("TILT_FIX_GAIN", 23, QuadPlane, tilt.fixed_gain, 0),
|
||||
|
||||
// @Param: TAILSIT_RAT_FW
|
||||
// @DisplayName: Tailsitter VTOL to forward flight transition rate
|
||||
// @Description: The pitch rate at which tailsitter aircraft will pitch down in the transition from VTOL to forward flight
|
||||
// @Units: deg/s
|
||||
// @Range: 10 500
|
||||
AP_GROUPINFO("TAILSIT_RAT_FW", 24, QuadPlane, tailsitter.transition_rate_fw, 50),
|
||||
// 24: TAILSIT_RAT_FW
|
||||
// 25: TAILSIT_RAT_VT
|
||||
|
||||
// @Param: TAILSIT_RAT_VT
|
||||
// @DisplayName: Tailsitter forward flight to VTOL transition rate
|
||||
// @Description: The pitch rate at which tailsitter aircraft will pitch up in the transition from forward flight to VTOL
|
||||
// @Units: deg/s
|
||||
// @Range: 10 500
|
||||
AP_GROUPINFO("TAILSIT_RAT_VT", 25, QuadPlane, tailsitter.transition_rate_vtol, 50),
|
||||
// @Group: TAILSIT_
|
||||
// @Path: tailsitter.cpp
|
||||
AP_SUBGROUPINFO(tailsitter, "TAILSIT_", 26, QuadPlane, Tailsitter),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
|
|
@ -12,6 +12,7 @@
|
|||
#include <AP_Proximity/AP_Proximity.h>
|
||||
#include "qautotune.h"
|
||||
#include "defines.h"
|
||||
#include "tailsitter.h"
|
||||
|
||||
/*
|
||||
QuadPlane specific functionality
|
||||
|
@ -27,6 +28,7 @@ public:
|
|||
friend class AP_Arming_Plane;
|
||||
friend class RC_Channel_Plane;
|
||||
friend class RC_Channel;
|
||||
friend class Tailsitter;
|
||||
|
||||
friend class Mode;
|
||||
friend class ModeAuto;
|
||||
|
@ -85,11 +87,6 @@ public:
|
|||
*/
|
||||
bool in_transition(void) const;
|
||||
|
||||
/*
|
||||
return true if we are a tailsitter transitioning to VTOL flight
|
||||
*/
|
||||
bool in_tailsitter_vtol_transition(uint32_t now = 0) const;
|
||||
|
||||
bool handle_do_vtol_transition(enum MAV_VTOL_STATE state) const;
|
||||
|
||||
bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
|
||||
|
@ -117,36 +114,6 @@ public:
|
|||
// see if we are flying from vtol point of view
|
||||
bool is_flying_vtol(void) const;
|
||||
|
||||
// return true when tailsitter frame configured
|
||||
bool is_tailsitter(void) const;
|
||||
|
||||
// return true when flying a control surface only tailsitter
|
||||
bool is_control_surface_tailsitter(void) const;
|
||||
|
||||
// true when flying a tilt-vectored tailsitter
|
||||
bool _is_vectored;
|
||||
|
||||
// return true when flying a tailsitter in VTOL
|
||||
bool tailsitter_active(void);
|
||||
|
||||
// create outputs for tailsitters
|
||||
void tailsitter_output(void);
|
||||
|
||||
// handle different tailsitter input types
|
||||
void tailsitter_check_input(void);
|
||||
|
||||
// check if we have completed transition to fixed wing
|
||||
bool tailsitter_transition_fw_complete(void);
|
||||
|
||||
// return true if we are a tailsitter in FW flight
|
||||
bool is_tailsitter_in_fw_flight(void) const;
|
||||
|
||||
// check if we have completed transition to vtol
|
||||
bool tailsitter_transition_vtol_complete(void) const;
|
||||
|
||||
// account for control surface speed scaling in VTOL modes
|
||||
void tailsitter_speed_scaling(void);
|
||||
|
||||
// user initiated takeoff for guided mode
|
||||
bool do_user_takeoff(float takeoff_altitude);
|
||||
|
||||
|
@ -193,10 +160,11 @@ private:
|
|||
|
||||
AP_Enum<AP_Motors::motor_frame_class> frame_class;
|
||||
AP_Enum<AP_Motors::motor_frame_type> frame_type;
|
||||
|
||||
AP_MotorsMulticopter *motors;
|
||||
|
||||
// Initialise motors to allow passing it to tailsitter in its constructor
|
||||
AP_MotorsMulticopter *motors = nullptr;
|
||||
const struct AP_Param::GroupInfo *motors_var_info;
|
||||
|
||||
|
||||
AC_AttitudeControl_Multi *attitude_control;
|
||||
AC_PosControl *pos_control;
|
||||
AC_WPNav *wp_nav;
|
||||
|
@ -532,56 +500,9 @@ private:
|
|||
bool is_vectored;
|
||||
} tilt;
|
||||
|
||||
// bit 0 enables plane mode and bit 1 enables body-frame roll mode
|
||||
enum tailsitter_input {
|
||||
TAILSITTER_INPUT_PLANE = (1U<<0),
|
||||
TAILSITTER_INPUT_BF_ROLL = (1U<<1)
|
||||
};
|
||||
|
||||
enum tailsitter_mask {
|
||||
TAILSITTER_MASK_AILERON = (1U<<0),
|
||||
TAILSITTER_MASK_ELEVATOR = (1U<<1),
|
||||
TAILSITTER_MASK_THROTTLE = (1U<<2),
|
||||
TAILSITTER_MASK_RUDDER = (1U<<3),
|
||||
};
|
||||
|
||||
enum tailsitter_gscl_mask {
|
||||
TAILSITTER_GSCL_THROTTLE = (1U<<0),
|
||||
TAILSITTER_GSCL_ATT_THR = (1U<<1),
|
||||
TAILSITTER_GSCL_DISK_THEORY = (1U<<2),
|
||||
TAILSITTER_GSCL_ALTITUDE = (1U<<3),
|
||||
};
|
||||
|
||||
// tailsitter control variables
|
||||
struct {
|
||||
// transition from VTOL to forward
|
||||
AP_Int8 transition_angle_fw;
|
||||
AP_Float transition_rate_fw;
|
||||
// transition from forward to VTOL
|
||||
AP_Int8 transition_angle_vtol;
|
||||
AP_Float transition_rate_vtol;
|
||||
AP_Int8 input_type;
|
||||
AP_Int8 input_mask;
|
||||
AP_Int8 input_mask_chan;
|
||||
AP_Float vectored_forward_gain;
|
||||
AP_Float vectored_hover_gain;
|
||||
AP_Float vectored_hover_power;
|
||||
AP_Float throttle_scale_max;
|
||||
AP_Float gain_scaling_min;
|
||||
AP_Float max_roll_angle;
|
||||
AP_Int16 motor_mask;
|
||||
AP_Float scaling_speed_min;
|
||||
AP_Float scaling_speed_max;
|
||||
AP_Int16 gain_scaling_mask;
|
||||
AP_Float disk_loading;
|
||||
} tailsitter;
|
||||
|
||||
// return the transition_angle_vtol value
|
||||
int8_t get_tailsitter_transition_angle_vtol(void) const;
|
||||
|
||||
// tailsitter speed scaler
|
||||
float last_spd_scaler = 1.0f; // used to slew rate limiting with TAILSITTER_GSCL_ATT_THR option
|
||||
float log_spd_scaler; // for QTUN log
|
||||
// tailsitter control
|
||||
Tailsitter tailsitter{*this, motors};
|
||||
|
||||
// the attitude view of the VTOL attitude controller
|
||||
AP_AHRS_View *ahrs_view;
|
||||
|
|
|
@ -17,42 +17,160 @@
|
|||
or by setting Q_TAILSIT_MOTMX nonzero and Q_FRAME_CLASS and Q_FRAME_TYPE
|
||||
to a configuration supported by AP_MotorsMatrix
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include "tailsitter.h"
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
return true when flying a tailsitter
|
||||
*/
|
||||
bool QuadPlane::is_tailsitter(void) const
|
||||
const AP_Param::GroupInfo Tailsitter::var_info[] = {
|
||||
|
||||
// @Param: ENABLE
|
||||
// @DisplayName: Enable Tailsitter
|
||||
// @Description: This enables Tailsitter functionality
|
||||
// @Values: 0:Disable,1:Enable
|
||||
// @User: Standard
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, Tailsitter, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: ANGLE
|
||||
// @DisplayName: Tailsitter fixed wing transition angle
|
||||
// @Description: This is the pitch angle at which tailsitter aircraft will change from VTOL control to fixed wing control.
|
||||
// @Units: deg
|
||||
// @Range: 5 80
|
||||
AP_GROUPINFO("ANGLE", 2, Tailsitter, transition_angle_fw, 45),
|
||||
|
||||
// @Param: ANG_VT
|
||||
// @DisplayName: Tailsitter VTOL transition angle
|
||||
// @Description: This is the pitch angle at which tailsitter aircraft will change from fixed wing control to VTOL control, if zero Q_TAILSIT_ANGLE will be used
|
||||
// @Units: deg
|
||||
// @Range: 5 80
|
||||
AP_GROUPINFO("ANG_VT", 3, Tailsitter, transition_angle_vtol, 0),
|
||||
|
||||
// @Param: INPUT
|
||||
// @DisplayName: Tailsitter input type bitmask
|
||||
// @Description: This controls whether stick input when hovering as a tailsitter follows the conventions for fixed wing hovering or multicopter hovering. When PlaneMode is not enabled (bit0 = 0) the roll stick will roll the aircraft in earth frame and yaw stick will yaw in earth frame. When PlaneMode input is enabled, the roll and yaw sticks are swapped so that the roll stick controls earth-frame yaw and rudder controls earth-frame roll. When body-frame roll is enabled (bit1 = 1), the yaw stick controls earth-frame yaw rate and the roll stick controls roll in the tailsitter's body frame when flying level.
|
||||
// @Bitmask: 0:PlaneMode,1:BodyFrameRoll
|
||||
AP_GROUPINFO("INPUT", 4, Tailsitter, input_type, 0),
|
||||
|
||||
// @Param: MASK
|
||||
// @DisplayName: Tailsitter input mask
|
||||
// @Description: This controls what channels have full manual control when hovering as a tailsitter and the Q_TAILSIT_MASKCH channel in high. This can be used to teach yourself to prop-hang a 3D plane by learning one or more channels at a time.
|
||||
// @Bitmask: 0:Aileron,1:Elevator,2:Throttle,3:Rudder
|
||||
AP_GROUPINFO("MASK", 5, Tailsitter, input_mask, 0),
|
||||
|
||||
// @Param: MASKCH
|
||||
// @DisplayName: Tailsitter input mask channel
|
||||
// @Description: This controls what input channel will activate the Q_TAILSIT_MASK mask. When this channel goes above 1700 then the pilot will have direct manual control of the output channels specified in Q_TAILSIT_MASK. Set to zero to disable.
|
||||
// @Values: 0:Disabled,1:Channel1,2:Channel2,3:Channel3,4:Channel4,5:Channel5,6:Channel6,7:Channel7,8:Channel8
|
||||
AP_GROUPINFO("MASKCH", 6, Tailsitter, input_mask_chan, 0),
|
||||
|
||||
// @Param: VFGAIN
|
||||
// @DisplayName: Tailsitter vector thrust gain in forward flight
|
||||
// @Description: This controls the amount of vectored thrust control used in forward flight for a vectored tailsitter
|
||||
// @Range: 0 1
|
||||
// @Increment: 0.01
|
||||
AP_GROUPINFO("VFGAIN", 7, Tailsitter, vectored_forward_gain, 0),
|
||||
|
||||
// @Param: VHGAIN
|
||||
// @DisplayName: Tailsitter vector thrust gain in hover
|
||||
// @Description: This controls the amount of vectored thrust control used in hover for a vectored tailsitter
|
||||
// @Range: 0 1
|
||||
// @Increment: 0.01
|
||||
AP_GROUPINFO("VHGAIN", 8, Tailsitter, vectored_hover_gain, 0.5),
|
||||
|
||||
// @Param: VHPOW
|
||||
// @DisplayName: Tailsitter vector thrust gain power
|
||||
// @Description: This controls the amount of extra pitch given to the vectored control when at high pitch errors
|
||||
// @Range: 0 4
|
||||
// @Increment: 0.1
|
||||
AP_GROUPINFO("VHPOW", 9, Tailsitter, vectored_hover_power, 2.5),
|
||||
|
||||
// @Param: GSCMAX
|
||||
// @DisplayName: Maximum tailsitter gain scaling
|
||||
// @Description: Maximum gain scaling for tailsitter Q_TAILSIT_GSCMSK options
|
||||
// @Range: 1 5
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("GSCMAX", 10, Tailsitter, throttle_scale_max, 2),
|
||||
|
||||
// @Param: 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("RLL_MX", 11, Tailsitter, max_roll_angle, 0),
|
||||
|
||||
// @Param: MOTMX
|
||||
// @DisplayName: Tailsitter motor mask
|
||||
// @Description: Bitmask of motors to remain active in forward flight for a 'Copter' tailsitter. Non-zero indicates airframe is a Copter tailsitter and uses copter style motor layouts determined by Q_FRAME_CLASS and Q_FRAME_TYPE. This should be zero for non-Copter tailsitters.
|
||||
// @User: Standard
|
||||
// @Bitmask: 0:Motor 1,1:Motor 2,2:Motor 3,3:Motor 4, 4:Motor 5,5:Motor 6,6:Motor 7,7:Motor 8
|
||||
AP_GROUPINFO("MOTMX", 12, Tailsitter, motor_mask, 0),
|
||||
|
||||
// @Param: GSCMSK
|
||||
// @DisplayName: Tailsitter gain scaling mask
|
||||
// @Description: Bitmask of gain scaling methods to be applied: Throttle: scale gains with throttle, ATT_THR: reduce gain at high throttle/tilt, 2:Disk theory velocity calculation, requires Q_TAILSIT_DSKLD to be set, ATT_THR must not be set, 3:Altitude correction, scale with air density
|
||||
// @User: Standard
|
||||
// @Bitmask: 0:Throttle,1:ATT_THR,2:Disk Theory,3:Altitude correction
|
||||
AP_GROUPINFO("GSCMSK", 13, Tailsitter, gain_scaling_mask, TAILSITTER_GSCL_THROTTLE),
|
||||
|
||||
// @Param: GSCMIN
|
||||
// @DisplayName: Minimum tailsitter gain scaling
|
||||
// @Description: Minimum gain scaling for tailsitter Q_TAILSIT_GSCMSK options
|
||||
// @Range: 0.1 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("GSCMIN", 14, Tailsitter, gain_scaling_min, 0.4),
|
||||
|
||||
// @Param: DSKLD
|
||||
// @DisplayName: Tailsitter disk loading
|
||||
// @Description: This is the vehicle weight in kg divided by the total disk area of all propellers in m^2. Only used with Q_TAILSIT_GSCMSK = 4
|
||||
// @Units: kg/m/m
|
||||
// @Range: 0 50
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("DSKLD", 15, Tailsitter, disk_loading, 0),
|
||||
|
||||
// @Param: RAT_FW
|
||||
// @DisplayName: Tailsitter VTOL to forward flight transition rate
|
||||
// @Description: The pitch rate at which tailsitter aircraft will pitch down in the transition from VTOL to forward flight
|
||||
// @Units: deg/s
|
||||
// @Range: 10 500
|
||||
AP_GROUPINFO("RAT_FW", 16, Tailsitter, transition_rate_fw, 50),
|
||||
|
||||
// @Param: RAT_VT
|
||||
// @DisplayName: Tailsitter forward flight to VTOL transition rate
|
||||
// @Description: The pitch rate at which tailsitter aircraft will pitch up in the transition from forward flight to VTOL
|
||||
// @Units: deg/s
|
||||
// @Range: 10 500
|
||||
AP_GROUPINFO("RAT_VT", 17, Tailsitter, transition_rate_vtol, 50),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
Tailsitter::Tailsitter(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors):quadplane(_quadplane),motors(_motors)
|
||||
{
|
||||
return available()
|
||||
&& ((frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (tailsitter.motor_mask != 0))
|
||||
&& (tilt.tilt_type != TILT_TYPE_BICOPTER);
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
/*
|
||||
return true when flying a control surface only tailsitter
|
||||
*/
|
||||
bool QuadPlane::is_control_surface_tailsitter(void) const
|
||||
bool Tailsitter::is_control_surface_tailsitter(void) const
|
||||
{
|
||||
return frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER
|
||||
&& ( is_zero(tailsitter.vectored_hover_gain) || !SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft));
|
||||
return quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER
|
||||
&& ( is_zero(vectored_hover_gain) || !SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft));
|
||||
}
|
||||
|
||||
/*
|
||||
check if we are flying as a tailsitter
|
||||
*/
|
||||
bool QuadPlane::tailsitter_active(void)
|
||||
bool Tailsitter::active(void)
|
||||
{
|
||||
if (!is_tailsitter()) {
|
||||
if (!enabled()) {
|
||||
return false;
|
||||
}
|
||||
if (in_vtol_mode()) {
|
||||
if (quadplane.in_vtol_mode()) {
|
||||
return true;
|
||||
}
|
||||
// check if we are in ANGLE_WAIT fixed wing transition
|
||||
if (transition_state == TRANSITION_ANGLE_WAIT_FW) {
|
||||
if (quadplane.transition_state == QuadPlane::TRANSITION_ANGLE_WAIT_FW) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -61,9 +179,9 @@ bool QuadPlane::tailsitter_active(void)
|
|||
/*
|
||||
run output for tailsitters
|
||||
*/
|
||||
void QuadPlane::tailsitter_output(void)
|
||||
void Tailsitter::output(void)
|
||||
{
|
||||
if (!is_tailsitter() || motor_test.running) {
|
||||
if (!enabled() || quadplane.motor_test.running || !quadplane.initialised) {
|
||||
// if motor test is running we don't want to overwrite it with output_motor_mask or motors_output
|
||||
return;
|
||||
}
|
||||
|
@ -74,10 +192,10 @@ void QuadPlane::tailsitter_output(void)
|
|||
|
||||
|
||||
// handle forward flight modes and transition to VTOL modes
|
||||
if (!tailsitter_active() || in_tailsitter_vtol_transition()) {
|
||||
if (!active() || in_vtol_transition()) {
|
||||
// get FW controller throttle demand and mask of motors enabled during forward flight
|
||||
float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
|
||||
if (hal.util->get_soft_armed() && in_tailsitter_vtol_transition() && !throttle_wait && is_flying()) {
|
||||
if (hal.util->get_soft_armed() && in_vtol_transition() && !quadplane.throttle_wait && quadplane.is_flying()) {
|
||||
/*
|
||||
during transitions to vtol mode set the throttle to
|
||||
hover thrust, center the rudder and set the altitude controller
|
||||
|
@ -89,7 +207,7 @@ void QuadPlane::tailsitter_output(void)
|
|||
throttle = MAX(throttle,plane.aparm.throttle_cruise.get());
|
||||
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
|
||||
pos_control->get_accel_z_pid().set_integrator(throttle*10);
|
||||
quadplane.pos_control->get_accel_z_pid().set_integrator(throttle*10);
|
||||
|
||||
// override AP_MotorsTailsitter throttles during back transition
|
||||
|
||||
|
@ -102,19 +220,19 @@ void QuadPlane::tailsitter_output(void)
|
|||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
|
||||
}
|
||||
|
||||
if (!assisted_flight) {
|
||||
if (!quadplane.assisted_flight) {
|
||||
// set AP_MotorsMatrix throttles for forward flight
|
||||
motors->output_motor_mask(throttle * 0.01f, tailsitter.motor_mask, plane.rudder_dt);
|
||||
motors->output_motor_mask(throttle * 0.01f, motor_mask, plane.rudder_dt);
|
||||
|
||||
// in forward flight: set motor tilt servos and throttles using FW controller
|
||||
if (tailsitter.vectored_forward_gain > 0) {
|
||||
if (vectored_forward_gain > 0) {
|
||||
// remove scaling from surface speed scaling and apply throttle scaling
|
||||
const float scaler = plane.control_mode == &plane.mode_manual?1:(tilt_throttle_scaling() / plane.get_speed_scaler());
|
||||
const float scaler = plane.control_mode == &plane.mode_manual?1:(quadplane.tilt_throttle_scaling() / plane.get_speed_scaler());
|
||||
// thrust vectoring in fixed wing flight
|
||||
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
|
||||
float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
|
||||
tilt_left = (elevator + aileron) * tailsitter.vectored_forward_gain * scaler;
|
||||
tilt_right = (elevator - aileron) * tailsitter.vectored_forward_gain * scaler;
|
||||
tilt_left = (elevator + aileron) * vectored_forward_gain * scaler;
|
||||
tilt_right = (elevator - aileron) * vectored_forward_gain * scaler;
|
||||
}
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
|
||||
|
@ -126,27 +244,27 @@ void QuadPlane::tailsitter_output(void)
|
|||
// the MultiCopter rate controller has already been run in an earlier call
|
||||
// to motors_output() from quadplane.update(), unless we are in assisted flight
|
||||
// tailsitter in TRANSITION_ANGLE_WAIT_FW is not really in assisted flight, its still in a VTOL mode
|
||||
if (assisted_flight && (transition_state != TRANSITION_ANGLE_WAIT_FW)) {
|
||||
hold_stabilize(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
|
||||
motors_output(true);
|
||||
if (quadplane.assisted_flight && (quadplane.transition_state != QuadPlane::TRANSITION_ANGLE_WAIT_FW)) {
|
||||
quadplane.hold_stabilize(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
|
||||
quadplane.motors_output(true);
|
||||
|
||||
if ((options & OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY) != 0) {
|
||||
if ((quadplane.options & QuadPlane::OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY) != 0) {
|
||||
// only use motors for Q assist, control surfaces remain under plane control
|
||||
// zero copter I terms and use plane
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
quadplane.attitude_control->reset_rate_controller_I_terms();
|
||||
|
||||
// output tilt motors
|
||||
tilt_left = 0.0f;
|
||||
tilt_right = 0.0f;
|
||||
if (tailsitter.vectored_hover_gain > 0) {
|
||||
if (vectored_hover_gain > 0) {
|
||||
const float hover_throttle = motors->get_throttle_hover();
|
||||
const float throttle = motors->get_throttle();
|
||||
float throttle_scaler = tailsitter.throttle_scale_max;
|
||||
float throttle_scaler = throttle_scale_max;
|
||||
if (is_positive(throttle)) {
|
||||
throttle_scaler = constrain_float(hover_throttle / throttle, tailsitter.gain_scaling_min, tailsitter.throttle_scale_max);
|
||||
throttle_scaler = constrain_float(hover_throttle / throttle, gain_scaling_min, throttle_scale_max);
|
||||
}
|
||||
tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft) * tailsitter.vectored_hover_gain * throttle_scaler;
|
||||
tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight) * tailsitter.vectored_hover_gain * throttle_scaler;
|
||||
tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft) * vectored_hover_gain * throttle_scaler;
|
||||
tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight) * vectored_hover_gain * throttle_scaler;
|
||||
}
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
|
||||
|
@ -156,7 +274,7 @@ void QuadPlane::tailsitter_output(void)
|
|||
return;
|
||||
}
|
||||
} else {
|
||||
motors_output(false);
|
||||
quadplane.motors_output(false);
|
||||
}
|
||||
|
||||
// In full Q assist it is better to use cotper I and zero plane
|
||||
|
@ -170,12 +288,12 @@ void QuadPlane::tailsitter_output(void)
|
|||
|
||||
if (hal.util->get_soft_armed()) {
|
||||
// scale surfaces for throttle
|
||||
tailsitter_speed_scaling();
|
||||
speed_scaling();
|
||||
}
|
||||
|
||||
tilt_left = 0.0f;
|
||||
tilt_right = 0.0f;
|
||||
if (tailsitter.vectored_hover_gain > 0) {
|
||||
if (vectored_hover_gain > 0) {
|
||||
// thrust vectoring VTOL modes
|
||||
tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);
|
||||
tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight);
|
||||
|
@ -184,16 +302,16 @@ void QuadPlane::tailsitter_output(void)
|
|||
power law. This allows the motors to point straight up for
|
||||
takeoff without integrator windup
|
||||
*/
|
||||
float des_pitch_cd = attitude_control->get_att_target_euler_cd().y;
|
||||
int32_t pitch_error_cd = (des_pitch_cd - ahrs_view->pitch_sensor) * 0.5;
|
||||
float des_pitch_cd = quadplane.attitude_control->get_att_target_euler_cd().y;
|
||||
int32_t pitch_error_cd = (des_pitch_cd - quadplane.ahrs_view->pitch_sensor) * 0.5;
|
||||
float extra_pitch = constrain_float(pitch_error_cd, -SERVO_MAX, SERVO_MAX) / SERVO_MAX;
|
||||
float extra_sign = extra_pitch > 0?1:-1;
|
||||
float extra_elevator = 0;
|
||||
if (!is_zero(extra_pitch) && in_vtol_mode()) {
|
||||
extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * SERVO_MAX;
|
||||
if (!is_zero(extra_pitch) && quadplane.in_vtol_mode()) {
|
||||
extra_elevator = extra_sign * powf(fabsf(extra_pitch), vectored_hover_power) * SERVO_MAX;
|
||||
}
|
||||
tilt_left = extra_elevator + tilt_left * tailsitter.vectored_hover_gain;
|
||||
tilt_right = extra_elevator + tilt_right * tailsitter.vectored_hover_gain;
|
||||
tilt_left = extra_elevator + tilt_left * vectored_hover_gain;
|
||||
tilt_right = extra_elevator + tilt_right * vectored_hover_gain;
|
||||
}
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
|
||||
|
@ -214,20 +332,19 @@ void QuadPlane::tailsitter_output(void)
|
|||
motors->limit.yaw = true;
|
||||
}
|
||||
|
||||
if (tailsitter.input_mask_chan > 0 &&
|
||||
tailsitter.input_mask > 0 &&
|
||||
RC_Channels::get_radio_in(tailsitter.input_mask_chan-1) > RC_Channel::AUX_PWM_TRIGGER_HIGH) {
|
||||
if (input_mask_chan > 0 && input_mask > 0 &&
|
||||
RC_Channels::get_radio_in(input_mask_chan-1) > RC_Channel::AUX_PWM_TRIGGER_HIGH) {
|
||||
// the user is learning to prop-hang
|
||||
if (tailsitter.input_mask & TAILSITTER_MASK_AILERON) {
|
||||
if (input_mask & TAILSITTER_MASK_AILERON) {
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz());
|
||||
}
|
||||
if (tailsitter.input_mask & TAILSITTER_MASK_ELEVATOR) {
|
||||
if (input_mask & TAILSITTER_MASK_ELEVATOR) {
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz());
|
||||
}
|
||||
if (tailsitter.input_mask & TAILSITTER_MASK_THROTTLE) {
|
||||
if (input_mask & TAILSITTER_MASK_THROTTLE) {
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
|
||||
}
|
||||
if (tailsitter.input_mask & TAILSITTER_MASK_RUDDER) {
|
||||
if (input_mask & TAILSITTER_MASK_RUDDER) {
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.channel_rudder->get_control_in_zero_dz());
|
||||
}
|
||||
}
|
||||
|
@ -237,21 +354,21 @@ void QuadPlane::tailsitter_output(void)
|
|||
/*
|
||||
return true when we have completed enough of a transition to switch to fixed wing control
|
||||
*/
|
||||
bool QuadPlane::tailsitter_transition_fw_complete(void)
|
||||
bool Tailsitter::transition_fw_complete(void)
|
||||
{
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
// instant trainsition when disarmed, no message
|
||||
return true;
|
||||
}
|
||||
if (labs(ahrs_view->pitch_sensor) > tailsitter.transition_angle_fw*100) {
|
||||
if (labs(quadplane.ahrs_view->pitch_sensor) > transition_angle_fw*100) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done");
|
||||
return true;
|
||||
}
|
||||
if (labs(ahrs_view->roll_sensor) > MAX(4500, plane.roll_limit_cd + 500)) {
|
||||
if (labs(quadplane.ahrs_view->roll_sensor) > MAX(4500, plane.roll_limit_cd + 500)) {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Transition FW done, roll error");
|
||||
return true;
|
||||
}
|
||||
if (AP_HAL::millis() - transition_start_ms > ((tailsitter.transition_angle_fw+(transition_initial_pitch*0.01f))/tailsitter.transition_rate_fw)*1500) {
|
||||
if (AP_HAL::millis() - quadplane.transition_start_ms > ((transition_angle_fw+(quadplane.transition_initial_pitch*0.01f))/transition_rate_fw)*1500) {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Transition FW done, timeout");
|
||||
return true;
|
||||
}
|
||||
|
@ -263,22 +380,22 @@ bool QuadPlane::tailsitter_transition_fw_complete(void)
|
|||
/*
|
||||
return true when we have completed enough of a transition to switch to VTOL control
|
||||
*/
|
||||
bool QuadPlane::tailsitter_transition_vtol_complete(void) const
|
||||
bool Tailsitter::transition_vtol_complete(void) const
|
||||
{
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
// instant trainsition when disarmed, no message
|
||||
return true;
|
||||
}
|
||||
// for vectored tailsitters at zero pilot throttle
|
||||
if ((plane.quadplane.get_pilot_throttle() < .05f) && plane.quadplane._is_vectored) {
|
||||
if ((quadplane.get_pilot_throttle() < .05f) && _is_vectored) {
|
||||
// if we are not moving (hence on the ground?) or don't know
|
||||
// transition immediately to tilt motors up and prevent prop strikes
|
||||
if (ahrs.groundspeed() < 1.0f) {
|
||||
if (quadplane.ahrs.groundspeed() < 1.0f) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done, zero throttle");
|
||||
return true;
|
||||
}
|
||||
}
|
||||
const float trans_angle = get_tailsitter_transition_angle_vtol();
|
||||
const float trans_angle = get_transition_angle_vtol();
|
||||
if (labs(plane.ahrs.pitch_sensor) > trans_angle*100) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done");
|
||||
return true;
|
||||
|
@ -291,20 +408,19 @@ bool QuadPlane::tailsitter_transition_vtol_complete(void) const
|
|||
gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, roll error");
|
||||
return true;
|
||||
}
|
||||
if (AP_HAL::millis() - transition_start_ms > ((trans_angle-(transition_initial_pitch*0.01f))/tailsitter.transition_rate_vtol)*1500) {
|
||||
if (AP_HAL::millis() - quadplane.transition_start_ms > ((trans_angle-(quadplane.transition_initial_pitch*0.01f))/transition_rate_vtol)*1500) {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, timeout");
|
||||
return true;
|
||||
}
|
||||
// still waiting
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
quadplane.attitude_control->reset_rate_controller_I_terms();
|
||||
return false;
|
||||
}
|
||||
|
||||
// handle different tailsitter input types
|
||||
void QuadPlane::tailsitter_check_input(void)
|
||||
void Tailsitter::check_input(void)
|
||||
{
|
||||
if (tailsitter_active() &&
|
||||
(tailsitter.input_type & TAILSITTER_INPUT_PLANE)) {
|
||||
if (active() && (input_type & TAILSITTER_INPUT_PLANE)) {
|
||||
// the user has asked for body frame controls when tailsitter
|
||||
// is active. We switch around the control_in value for the
|
||||
// channels to do this, as that ensures the value is
|
||||
|
@ -319,15 +435,15 @@ void QuadPlane::tailsitter_check_input(void)
|
|||
/*
|
||||
return true if we are a tailsitter transitioning to VTOL flight
|
||||
*/
|
||||
bool QuadPlane::in_tailsitter_vtol_transition(uint32_t now) const
|
||||
bool Tailsitter::in_vtol_transition(uint32_t now) const
|
||||
{
|
||||
if (!is_tailsitter() || !in_vtol_mode()) {
|
||||
if (!enabled() || !quadplane.in_vtol_mode()) {
|
||||
return false;
|
||||
}
|
||||
if (transition_state == TRANSITION_ANGLE_WAIT_VTOL) {
|
||||
if (quadplane.transition_state == QuadPlane::TRANSITION_ANGLE_WAIT_VTOL) {
|
||||
return true;
|
||||
}
|
||||
if ((now != 0) && ((now - last_vtol_mode_ms) > 1000)) {
|
||||
if ((now != 0) && ((now - quadplane.last_vtol_mode_ms) > 1000)) {
|
||||
// only just come out of forward flight
|
||||
return true;
|
||||
}
|
||||
|
@ -337,44 +453,44 @@ bool QuadPlane::in_tailsitter_vtol_transition(uint32_t now) const
|
|||
/*
|
||||
return true if we are a tailsitter in FW flight
|
||||
*/
|
||||
bool QuadPlane::is_tailsitter_in_fw_flight(void) const
|
||||
bool Tailsitter::is_in_fw_flight(void) const
|
||||
{
|
||||
return is_tailsitter() && !in_vtol_mode() && transition_state == TRANSITION_DONE;
|
||||
return enabled() && !quadplane.in_vtol_mode() && quadplane.transition_state == QuadPlane::TRANSITION_DONE;
|
||||
}
|
||||
|
||||
/*
|
||||
return the tailsitter.transition_angle_vtol value if non zero, otherwise returns the tailsitter.transition_angle_fw value.
|
||||
*/
|
||||
int8_t QuadPlane::get_tailsitter_transition_angle_vtol() const
|
||||
int8_t Tailsitter::get_transition_angle_vtol() const
|
||||
{
|
||||
if (tailsitter.transition_angle_vtol == 0) {
|
||||
return tailsitter.transition_angle_fw;
|
||||
if (transition_angle_vtol == 0) {
|
||||
return transition_angle_fw;
|
||||
}
|
||||
return tailsitter.transition_angle_vtol;
|
||||
return transition_angle_vtol;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
account for speed scaling of control surfaces in VTOL modes
|
||||
*/
|
||||
void QuadPlane::tailsitter_speed_scaling(void)
|
||||
void Tailsitter::speed_scaling(void)
|
||||
{
|
||||
const float hover_throttle = motors->get_throttle_hover();
|
||||
const float throttle = motors->get_throttle();
|
||||
float spd_scaler = 1.0f;
|
||||
|
||||
// Scaleing with throttle
|
||||
float throttle_scaler = tailsitter.throttle_scale_max;
|
||||
float throttle_scaler = throttle_scale_max;
|
||||
if (is_positive(throttle)) {
|
||||
throttle_scaler = constrain_float(hover_throttle / throttle, tailsitter.gain_scaling_min, tailsitter.throttle_scale_max);
|
||||
throttle_scaler = constrain_float(hover_throttle / throttle, gain_scaling_min, throttle_scale_max);
|
||||
}
|
||||
|
||||
if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ATT_THR) != 0) {
|
||||
if ((gain_scaling_mask & TAILSITTER_GSCL_ATT_THR) != 0) {
|
||||
// reduce gains when flying at high speed in Q modes:
|
||||
|
||||
// critical parameter: violent oscillations if too high
|
||||
// sudden loss of attitude control if too low
|
||||
const float min_scale = tailsitter.gain_scaling_min;
|
||||
const float min_scale = gain_scaling_min;
|
||||
float tthr = 1.25f * hover_throttle;
|
||||
|
||||
// reduce control surface throws at large tilt
|
||||
|
@ -391,7 +507,7 @@ void QuadPlane::tailsitter_speed_scaling(void)
|
|||
const float alpha = (1 - min_scale) / c_trans_angle;
|
||||
const float beta = 1 - alpha * c_trans_angle;
|
||||
|
||||
const float c_tilt = ahrs_view->get_rotation_body_to_ned().c.z;
|
||||
const float c_tilt = quadplane.ahrs_view->get_rotation_body_to_ned().c.z;
|
||||
if (c_tilt < c_trans_angle) {
|
||||
spd_scaler = constrain_float(beta + alpha * c_tilt, min_scale, 1.0f);
|
||||
// reduce throttle attenuation threshold too
|
||||
|
@ -413,16 +529,16 @@ void QuadPlane::tailsitter_speed_scaling(void)
|
|||
last_spd_scaler = spd_scaler;
|
||||
|
||||
// also apply throttle scaling if enabled
|
||||
if ((spd_scaler >= 1.0f) && ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0)) {
|
||||
if ((spd_scaler >= 1.0f) && ((gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0)) {
|
||||
spd_scaler = MAX(throttle_scaler,1.0f);
|
||||
}
|
||||
|
||||
} else if (((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_DISK_THEORY) != 0) && is_positive(tailsitter.disk_loading.get())) {
|
||||
} else if (((gain_scaling_mask & TAILSITTER_GSCL_DISK_THEORY) != 0) && is_positive(disk_loading.get())) {
|
||||
// Use disk theory to estimate the velocity over the control surfaces
|
||||
// https://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/node86.html
|
||||
|
||||
float airspeed;
|
||||
if (!ahrs.airspeed_estimate(airspeed)) {
|
||||
if (!quadplane.ahrs.airspeed_estimate(airspeed)) {
|
||||
// No airspeed estimate, use throttle scaling
|
||||
spd_scaler = throttle_scaler;
|
||||
|
||||
|
@ -438,30 +554,30 @@ void QuadPlane::tailsitter_speed_scaling(void)
|
|||
|
||||
const float rho = SSL_AIR_DENSITY * plane.barometer.get_air_density_ratio();
|
||||
float hover_rho = rho;
|
||||
if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
|
||||
if ((gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
|
||||
// if applying altitude correction use sea level density for hover case
|
||||
hover_rho = SSL_AIR_DENSITY;
|
||||
}
|
||||
|
||||
// hover case: (t / t_h) = 1 and U0 = 0
|
||||
const float sq_hover_outflow = (tailsitter.disk_loading.get() * GRAVITY_MSS) / (0.5f * hover_rho);
|
||||
const float sq_hover_outflow = (disk_loading.get() * GRAVITY_MSS) / (0.5f * hover_rho);
|
||||
|
||||
|
||||
// calculate the true outflow speed
|
||||
const float sq_outflow = (((throttle/hover_throttle) * tailsitter.disk_loading.get() * GRAVITY_MSS) / (0.5f * rho)) + sq(MAX(airspeed,0));
|
||||
const float sq_outflow = (((throttle/hover_throttle) * disk_loading.get() * GRAVITY_MSS) / (0.5f * rho)) + sq(MAX(airspeed,0));
|
||||
|
||||
// Scale by the ratio of squared hover outflow velocity to squared actual outflow velocity
|
||||
spd_scaler = tailsitter.throttle_scale_max;
|
||||
spd_scaler = throttle_scale_max;
|
||||
if (is_positive(sq_outflow)) {
|
||||
spd_scaler = constrain_float(sq_hover_outflow / sq_outflow, tailsitter.gain_scaling_min.get(), tailsitter.throttle_scale_max.get());
|
||||
spd_scaler = constrain_float(sq_hover_outflow / sq_outflow, gain_scaling_min.get(), throttle_scale_max.get());
|
||||
}
|
||||
}
|
||||
|
||||
} else if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0) {
|
||||
} else if ((gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0) {
|
||||
spd_scaler = throttle_scaler;
|
||||
}
|
||||
|
||||
if ((tailsitter.gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
|
||||
if ((gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
|
||||
// air density correction
|
||||
spd_scaler /= plane.barometer.get_air_density_ratio();
|
||||
}
|
||||
|
|
|
@ -0,0 +1,116 @@
|
|||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
class QuadPlane;
|
||||
class AP_MotorsMulticopter;
|
||||
class Tailsitter
|
||||
{
|
||||
friend class QuadPlane;
|
||||
friend class Plane;
|
||||
public:
|
||||
|
||||
Tailsitter(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors);
|
||||
|
||||
bool enabled() const { return enable > 0;}
|
||||
|
||||
// return true when flying a control surface only tailsitter
|
||||
bool is_control_surface_tailsitter(void) const;
|
||||
|
||||
// return true when flying a tailsitter in VTOL
|
||||
bool active(void);
|
||||
|
||||
// create outputs for tailsitters
|
||||
void output(void);
|
||||
|
||||
// handle different tailsitter input types
|
||||
void check_input(void);
|
||||
|
||||
// check if we have completed transition to fixed wing
|
||||
bool transition_fw_complete(void);
|
||||
|
||||
// return true if we are a tailsitter in FW flight
|
||||
bool is_in_fw_flight(void) const;
|
||||
|
||||
// check if we have completed transition to vtol
|
||||
bool transition_vtol_complete(void) const;
|
||||
|
||||
// return true if transistion to VTOL flight
|
||||
bool in_vtol_transition(uint32_t now = 0) const;
|
||||
|
||||
// account for control surface speed scaling in VTOL modes
|
||||
void speed_scaling(void);
|
||||
|
||||
// return the transition_angle_vtol value
|
||||
int8_t get_transition_angle_vtol() const;
|
||||
|
||||
|
||||
// true when flying a tilt-vectored tailsitter
|
||||
bool _is_vectored;
|
||||
|
||||
// tailsitter speed scaler
|
||||
float last_spd_scaler = 1.0f; // used to slew rate limiting with TAILSITTER_GSCL_ATT_THR option
|
||||
float log_spd_scaler; // for QTUN log
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// bit 0 enables plane mode and bit 1 enables body-frame roll mode
|
||||
enum input {
|
||||
TAILSITTER_INPUT_PLANE = (1U<<0),
|
||||
TAILSITTER_INPUT_BF_ROLL = (1U<<1)
|
||||
};
|
||||
|
||||
enum mask {
|
||||
TAILSITTER_MASK_AILERON = (1U<<0),
|
||||
TAILSITTER_MASK_ELEVATOR = (1U<<1),
|
||||
TAILSITTER_MASK_THROTTLE = (1U<<2),
|
||||
TAILSITTER_MASK_RUDDER = (1U<<3),
|
||||
};
|
||||
|
||||
enum gscl_mask {
|
||||
TAILSITTER_GSCL_THROTTLE = (1U<<0),
|
||||
TAILSITTER_GSCL_ATT_THR = (1U<<1),
|
||||
TAILSITTER_GSCL_DISK_THEORY = (1U<<2),
|
||||
TAILSITTER_GSCL_ALTITUDE = (1U<<3),
|
||||
};
|
||||
|
||||
AP_Int8 enable;
|
||||
AP_Int8 transition_angle_fw;
|
||||
AP_Float transition_rate_fw;
|
||||
AP_Int8 transition_angle_vtol;
|
||||
AP_Float transition_rate_vtol;
|
||||
AP_Int8 input_type;
|
||||
AP_Int8 input_mask;
|
||||
AP_Int8 input_mask_chan;
|
||||
AP_Float vectored_forward_gain;
|
||||
AP_Float vectored_hover_gain;
|
||||
AP_Float vectored_hover_power;
|
||||
AP_Float throttle_scale_max;
|
||||
AP_Float gain_scaling_min;
|
||||
AP_Float max_roll_angle;
|
||||
AP_Int16 motor_mask;
|
||||
AP_Float scaling_speed_min;
|
||||
AP_Float scaling_speed_max;
|
||||
AP_Int16 gain_scaling_mask;
|
||||
AP_Float disk_loading;
|
||||
|
||||
private:
|
||||
|
||||
// refences for convenience
|
||||
QuadPlane& quadplane;
|
||||
AP_MotorsMulticopter*& motors;
|
||||
|
||||
};
|
Loading…
Reference in New Issue