2015-11-24 04:24:04 -04:00
# include "Plane.h"
2020-09-02 23:45:52 -03:00
# include "AC_AttitudeControl/AC_AttitudeControl_TS.h"
2015-11-24 04:24:04 -04:00
const AP_Param : : GroupInfo QuadPlane : : var_info [ ] = {
2015-12-26 06:40:40 -04:00
// @Param: ENABLE
// @DisplayName: Enable QuadPlane
2016-04-22 07:20:45 -03:00
// @Description: This enables QuadPlane functionality, assuming multicopter motors start on output 5. If this is set to 2 then when starting AUTO mode it will initially be in VTOL AUTO mode.
// @Values: 0:Disable,1:Enable,2:Enable VTOL AUTO
2015-12-26 06:40:40 -04:00
// @User: Standard
2015-12-29 07:43:43 -04:00
AP_GROUPINFO_FLAGS ( " ENABLE " , 1 , QuadPlane , enable , 0 , AP_PARAM_FLAG_ENABLE ) ,
2015-12-26 06:40:40 -04:00
// @Group: M_
2015-11-24 04:24:04 -04:00
// @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp
2017-02-27 02:39:46 -04:00
AP_SUBGROUPVARPTR ( motors , " M_ " , 2 , QuadPlane , plane . quadplane . motors_var_info ) ,
2015-11-24 04:24:04 -04:00
2016-02-15 04:45:56 -04:00
// 3 ~ 8 were used by quadplane attitude control PIDs
2015-11-24 04:24:04 -04:00
// @Param: ANGLE_MAX
// @DisplayName: Angle Max
2016-04-28 21:23:26 -03:00
// @Description: Maximum lean angle in all VTOL flight modes
2017-05-02 10:36:25 -03:00
// @Units: cdeg
2021-05-20 10:08:46 -03:00
// @Increment: 10
2015-11-24 04:24:04 -04:00
// @Range: 1000 8000
// @User: Advanced
2016-04-28 21:23:26 -03:00
AP_GROUPINFO ( " ANGLE_MAX " , 10 , QuadPlane , aparm . angle_max , 3000 ) ,
2015-11-24 04:24:04 -04:00
// @Param: TRANSITION_MS
// @DisplayName: Transition time
2015-12-26 04:51:05 -04:00
// @Description: Transition time in milliseconds after minimum airspeed is reached
2017-05-02 10:36:25 -03:00
// @Units: ms
2015-11-24 04:24:04 -04:00
// @Range: 0 30000
// @User: Advanced
2015-12-26 06:40:40 -04:00
AP_GROUPINFO ( " TRANSITION_MS " , 11 , QuadPlane , transition_time_ms , 5000 ) ,
2015-11-24 04:24:04 -04:00
2018-01-24 08:14:15 -04:00
// 12 ~ 16 were used by position, velocity and acceleration PIDs
2018-12-18 00:52:22 -04:00
// @Group: P
2015-11-24 04:24:04 -04:00
// @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
2015-12-26 06:40:40 -04:00
AP_SUBGROUPPTR ( pos_control , " P " , 17 , QuadPlane , AC_PosControl ) ,
2015-12-26 03:45:42 -04:00
// @Param: VELZ_MAX
2021-03-16 18:54:03 -03:00
// @DisplayName: Pilot maximum vertical speed up
// @Description: The maximum ascending vertical velocity the pilot may request in cm/s
2017-05-02 10:36:25 -03:00
// @Units: cm/s
2015-12-26 03:45:42 -04:00
// @Range: 50 500
// @Increment: 10
// @User: Standard
2021-03-16 18:54:03 -03:00
AP_GROUPINFO ( " VELZ_MAX " , 18 , QuadPlane , pilot_velocity_z_max_up , 250 ) ,
// @Param: VELZ_MAX_DN
// @DisplayName: Pilot maximum vertical speed down
// @Description: The maximum vertical velocity the pilot may request in cm/s going down. If 0, uses Q_VELZ_MAX value.
// @Units: cm/s
// @Range: 50 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO ( " VELZ_MAX_DN " , 60 , QuadPlane , pilot_velocity_z_max_dn , 0 ) ,
// @Param: ACCEL_Z
2015-12-26 03:45:42 -04:00
// @DisplayName: Pilot vertical acceleration
// @Description: The vertical acceleration used when pilot is controlling the altitude
// @Units: cm/s/s
// @Range: 50 500
// @Increment: 10
// @User: Standard
2015-12-26 06:40:40 -04:00
AP_GROUPINFO ( " ACCEL_Z " , 19 , QuadPlane , pilot_accel_z , 250 ) ,
2015-12-26 03:45:42 -04:00
2015-12-26 04:51:05 -04:00
// @Group: WP_
// @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
2015-12-26 06:40:40 -04:00
AP_SUBGROUPPTR ( wp_nav , " WP_ " , 20 , QuadPlane , AC_WPNav ) ,
// @Param: RC_SPEED
// @DisplayName: RC output speed in Hz
// @Description: This is the PWM refresh rate in Hz for QuadPlane quad motors
// @Units: Hz
// @Range: 50 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO ( " RC_SPEED " , 21 , QuadPlane , rc_speed , 490 ) ,
2021-10-11 17:58:00 -03:00
// @Param: THR_MIN_PWM
// @DisplayName: Minimum PWM output
// @Description: This is the minimum PWM output for the quad motors
// @Units: PWM
// @Range: 800 2200
// @Increment: 1
// @User: Standard
2021-09-04 14:16:53 -03:00
// 22: THR_MIN_PWM
2021-10-11 17:58:00 -03:00
// @Param: THR_MAX_PWM
// @DisplayName: Maximum PWM output
// @Description: This is the maximum PWM output for the quad motors
// @Units: PWM
// @Range: 800 2200
// @Increment: 1
// @User: Standard
2021-09-04 14:16:53 -03:00
// 23: THR_MAX_PWM
2015-12-26 06:40:40 -04:00
// @Param: ASSIST_SPEED
// @DisplayName: Quadplane assistance speed
// @Description: This is the speed below which the quad motors will provide stability and lift assistance in fixed wing modes. Zero means no assistance except during transition
// @Units: m/s
// @Range: 0 100
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO ( " ASSIST_SPEED " , 24 , QuadPlane , assist_speed , 0 ) ,
2016-01-02 00:25:49 -04:00
// @Param: YAW_RATE_MAX
// @DisplayName: Maximum yaw rate
2017-10-19 06:59:02 -03:00
// @Description: This is the maximum yaw rate for pilot input on rudder stick in degrees/second
2017-05-02 10:36:25 -03:00
// @Units: deg/s
2016-01-02 00:25:49 -04:00
// @Range: 50 500
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " YAW_RATE_MAX " , 25 , QuadPlane , yaw_rate_max , 100 ) ,
2016-01-02 02:55:56 -04:00
// @Param: LAND_SPEED
// @DisplayName: Land speed
// @Description: The descent speed for the final stage of landing in cm/s
// @Units: cm/s
// @Range: 30 200
// @Increment: 10
// @User: Standard
AP_GROUPINFO ( " LAND_SPEED " , 26 , QuadPlane , land_speed_cms , 50 ) ,
// @Param: LAND_FINAL_ALT
// @DisplayName: Land final altitude
// @Description: The altitude at which we should switch to Q_LAND_SPEED descent rate
// @Units: m
// @Range: 0.5 50
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO ( " LAND_FINAL_ALT " , 27 , QuadPlane , land_final_alt , 6 ) ,
2016-01-06 00:19:57 -04:00
2016-06-16 00:02:41 -03:00
// 28 was used by THR_MID
2016-01-15 01:49:49 -04:00
// @Param: TRAN_PIT_MAX
// @DisplayName: Transition max pitch
// @Description: Maximum pitch during transition to auto fixed wing flight
// @User: Standard
// @Range: 0 30
2017-05-02 10:36:25 -03:00
// @Units: deg
2016-01-15 01:49:49 -04:00
// @Increment: 1
AP_GROUPINFO ( " TRAN_PIT_MAX " , 29 , QuadPlane , transition_pitch_max , 3 ) ,
2016-02-07 20:00:19 -04:00
2016-12-30 19:50:01 -04:00
// frame class was moved from 30 when consolidating AP_Motors classes
# define FRAME_CLASS_OLD_IDX 30
2016-02-07 20:00:19 -04:00
// @Param: FRAME_CLASS
// @DisplayName: Frame Class
// @Description: Controls major frame class for multicopter component
2021-01-19 13:20:45 -04:00
// @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 7:Tri, 10: TailSitter, 12:DodecaHexa, 14:Deca, 15:Scripting Matrix
2016-02-07 20:00:19 -04:00
// @User: Standard
2016-12-30 19:50:01 -04:00
AP_GROUPINFO ( " FRAME_CLASS " , 46 , QuadPlane , frame_class , 1 ) ,
2016-02-07 20:00:19 -04:00
// @Param: FRAME_TYPE
// @DisplayName: Frame Type (+, X or V)
// @Description: Controls motor mixing for multicopter component
2020-08-10 00:34:12 -03:00
// @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15:I, 16:MOTOR_FRAME_TYPE_NYT_PLUS, 17:MOTOR_FRAME_TYPE_NYT_X, 18: BetaFlightXReversed
2016-02-07 20:00:19 -04:00
// @User: Standard
AP_GROUPINFO ( " FRAME_TYPE " , 31 , QuadPlane , frame_type , 1 ) ,
2016-04-20 02:13:20 -03:00
// @Param: VFWD_GAIN
// @DisplayName: Forward velocity hold gain
2016-06-03 22:04:03 -03:00
// @Description: Controls use of forward motor in vtol modes. If this is zero then the forward motor will not be used for position control in VTOL modes. A value of 0.05 is a good place to start if you want to use the forward motor for position control. No forward motor will be used in QSTABILIZE or QHOVER modes. Use QLOITER for position hold with the forward motor.
2016-04-20 02:13:20 -03:00
// @Range: 0 0.5
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO ( " VFWD_GAIN " , 32 , QuadPlane , vel_forward . gain , 0 ) ,
2016-04-20 03:23:17 -03:00
// @Param: WVANE_GAIN
// @DisplayName: Weathervaning gain
2016-06-03 22:04:03 -03:00
// @Description: This controls the tendency to yaw to face into the wind. A value of 0.1 is to start with and will give a slow turn into the wind. Use a value of 0.4 for more rapid response. The weathervaning works by turning into the direction of roll.
2016-04-20 03:23:17 -03:00
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO ( " WVANE_GAIN " , 33 , QuadPlane , weathervane . gain , 0 ) ,
2016-04-21 08:52:25 -03:00
// @Param: WVANE_MINROLL
// @DisplayName: Weathervaning min roll
// @Description: This set the minimum roll in degrees before active weathervaning will start. This may need to be larger if your aircraft has bad roll trim.
// @Range: 0 10
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO ( " WVANE_MINROLL " , 34 , QuadPlane , weathervane . min_roll , 1 ) ,
2016-04-29 02:31:08 -03:00
// @Param: RTL_ALT
// @DisplayName: QRTL return altitude
// @Description: The altitude which QRTL mode heads to initially
// @Units: m
// @Range: 1 200
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " RTL_ALT " , 35 , QuadPlane , qrtl_alt , 15 ) ,
2016-04-29 03:31:22 -03:00
// @Param: RTL_MODE
// @DisplayName: VTOL RTL mode
2021-06-12 22:17:39 -03:00
// @Description: If this is set to 1 then an RTL will change to QRTL when within RTL_RADIUS meters of the RTL destination, VTOL approach: vehicle will RTL at RTL alt and circle with a radius of Q_FW_LND_APR_RAD down to Q_RLT_ALT and then transission into the wind and QRTL, see 'AUTO VTOL Landing', QRTL Always: do a QRTL instead of RTL
// @Values: 0:Disabled,1:Enabled,2:VTOL approach,3:QRTL Always
2016-04-29 03:31:22 -03:00
// @User: Standard
AP_GROUPINFO ( " RTL_MODE " , 36 , QuadPlane , rtl_mode , 0 ) ,
2016-05-01 05:07:52 -03:00
// @Param: TILT_MASK
// @DisplayName: Tiltrotor mask
// @Description: This is a bitmask of motors that are tiltable in a tiltrotor (or tiltwing). The mask is in terms of the standard motor order for the frame type.
// @User: Standard
AP_GROUPINFO ( " TILT_MASK " , 37 , QuadPlane , tilt . tilt_mask , 0 ) ,
2017-02-24 06:05:27 -04:00
// @Param: TILT_RATE_UP
// @DisplayName: Tiltrotor upwards tilt rate
// @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from forward flight to hover
2017-05-02 10:36:25 -03:00
// @Units: deg/s
2016-05-01 05:07:52 -03:00
// @Increment: 1
// @Range: 10 300
// @User: Standard
2017-02-24 06:05:27 -04:00
AP_GROUPINFO ( " TILT_RATE_UP " , 38 , QuadPlane , tilt . max_rate_up_dps , 40 ) ,
2016-05-01 05:07:52 -03:00
// @Param: TILT_MAX
// @DisplayName: Tiltrotor maximum VTOL angle
// @Description: This is the maximum angle of the tiltable motors at which multicopter control will be enabled. Beyond this angle the plane will fly solely as a fixed wing aircraft and the motors will tilt to their maximum angle at the TILT_RATE
2017-05-02 10:36:25 -03:00
// @Units: deg
2016-05-01 05:07:52 -03:00
// @Increment: 1
// @Range: 20 80
// @User: Standard
AP_GROUPINFO ( " TILT_MAX " , 39 , QuadPlane , tilt . max_angle_deg , 45 ) ,
2016-05-05 19:27:47 -03:00
// @Param: GUIDED_MODE
// @DisplayName: Enable VTOL in GUIDED mode
// @Description: This enables use of VTOL in guided mode. When enabled the aircraft will switch to VTOL flight when the guided destination is reached and hover at the destination.
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO ( " GUIDED_MODE " , 40 , QuadPlane , guided_mode , 0 ) ,
2016-05-07 19:11:00 -03:00
2016-06-16 00:02:41 -03:00
// 41 was used by THR_MIN
2016-06-02 00:10:39 -03:00
// @Param: ESC_CAL
// @DisplayName: ESC Calibration
2019-12-08 17:22:19 -04:00
// @Description: This is used to calibrate the throttle range of the VTOL motors. Please read https://ardupilot.org/plane/docs/quadplane-esc-calibration.html before using. This parameter is automatically set back to 0 on every boot. This parameter only takes effect in QSTABILIZE mode. When set to 1 the output of all motors will come directly from the throttle stick when armed, and will be zero when disarmed. When set to 2 the output of all motors will be maximum when armed and zero when disarmed. Make sure you remove all properllers before using.
2016-06-02 00:10:39 -03:00
// @Values: 0:Disabled,1:ThrottleInput,2:FullInput
// @User: Standard
AP_GROUPINFO ( " ESC_CAL " , 42 , QuadPlane , esc_calibration , 0 ) ,
2016-06-03 19:35:09 -03:00
// @Param: VFWD_ALT
// @DisplayName: Forward velocity alt cutoff
// @Description: Controls altitude to disable forward velocity assist when below this relative altitude. This is useful to keep the forward velocity propeller from hitting the ground. Rangefinder height data is incorporated when available.
2018-12-27 10:48:30 -04:00
// @Units: m
2016-06-03 19:35:09 -03:00
// @Range: 0 10
// @Increment: 0.25
// @User: Standard
AP_GROUPINFO ( " VFWD_ALT " , 43 , QuadPlane , vel_forward_alt_cutoff , 0 ) ,
2016-07-25 02:46:17 -03:00
// @Param: LAND_ICE_CUT
// @DisplayName: Cut IC engine on landing
// @Description: This controls stopping an internal combustion engine in the final landing stage of a VTOL. This is important for aircraft where the forward thrust engine may experience prop-strike if left running during landing. This requires the engine controls are enabled using the ICE_* parameters.
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO ( " LAND_ICE_CUT " , 44 , QuadPlane , land_icengine_cut , 1 ) ,
2016-04-20 03:23:17 -03:00
2016-08-29 03:44:54 -03:00
// @Param: ASSIST_ANGLE
// @DisplayName: Quadplane assistance angle
2020-06-29 19:10:40 -03:00
// @Description: This is the angular error in attitude beyond which the quadplane VTOL motors will provide stability assistance. This will only be used if Q_ASSIST_SPEED is also non-zero. Assistance will be given if the attitude is outside the normal attitude limits by at least 5 degrees and the angular error in roll or pitch is greater than this angle for at least Q_ASSIST_DELAY seconds. Set to zero to disable angle assistance.
2017-05-02 10:36:25 -03:00
// @Units: deg
2016-08-29 03:44:54 -03:00
// @Range: 0 90
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " ASSIST_ANGLE " , 45 , QuadPlane , assist_angle , 30 ) ,
2016-07-03 09:02:38 -03:00
// @Param: TILT_TYPE
// @DisplayName: Tiltrotor type
2019-03-18 21:00:25 -03:00
// @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover, Bicopter tiltrottor must use the tailsitter frame class (10)
// @Values: 0:Continuous,1:Binary,2:VectoredYaw,3:Bicopter
2017-01-22 18:29:51 -04:00
AP_GROUPINFO ( " TILT_TYPE " , 47 , QuadPlane , tilt . tilt_type , TILT_TYPE_CONTINUOUS ) ,
2017-02-11 04:12:56 -04:00
2017-02-24 01:47:09 -04:00
// @Param: TAILSIT_ANGLE
2021-03-25 17:32:31 -03:00
// @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
2017-02-11 04:12:56 -04:00
// @Range: 5 80
2021-03-25 17:32:31 -03:00
AP_GROUPINFO ( " TAILSIT_ANG_VT " , 61 , QuadPlane , tailsitter . transition_angle_vtol , 0 ) ,
2017-02-24 06:05:27 -04:00
// @Param: TILT_RATE_DN
// @DisplayName: Tiltrotor downwards tilt rate
// @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from hover to forward flight. When this is zero the Q_TILT_RATE_UP value is used.
2017-05-02 10:36:25 -03:00
// @Units: deg/s
2017-02-24 06:05:27 -04:00
// @Increment: 1
// @Range: 10 300
// @User: Standard
AP_GROUPINFO ( " TILT_RATE_DN " , 49 , QuadPlane , tilt . max_rate_down_dps , 0 ) ,
2017-02-24 01:47:09 -04:00
// @Param: TAILSIT_INPUT
2019-12-01 21:11:06 -04:00
// @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 ) ,
2017-02-24 01:47:09 -04:00
2017-02-24 03:02:34 -04:00
// @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 ) ,
2017-04-09 21:01:58 -03:00
// @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 ) ,
2017-04-22 22:35:25 -03:00
// @Param: TILT_YAW_ANGLE
// @DisplayName: Tilt minimum angle for vectored yaw
2019-03-18 21:00:25 -03:00
// @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output. This needs to be set for Q_TILT_TYPE=3 to enable vectored control for yaw of tricopter tilt quadplanes. This is also used to limit the forwards travel of bicopter tilts when in VTOL modes
2017-04-22 22:35:25 -03:00
// @Range: 0 30
AP_GROUPINFO ( " TILT_YAW_ANGLE " , 55 , QuadPlane , tilt . tilt_yaw_angle , 0 ) ,
2017-05-04 18:34:27 -03:00
// @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 ) ,
2017-10-20 01:39:44 -03:00
// @Param: MAV_TYPE
// @DisplayName: MAVLink type identifier
// @Description: This controls the mavlink type given in HEARTBEAT messages. For some GCS types a particular setting will be needed for correct operation.
// @Values: 0:AUTO,1:FIXED_WING,2:QUADROTOR,3:COAXIAL,4:HELICOPTER,7:AIRSHIP,8:FREE_BALLOON,9:ROCKET,10:GROUND_ROVER,11:SURFACE_BOAT,12:SUBMARINE,16:FLAPPING_WING,17:KITE,19:VTOL_DUOROTOR,20:VTOL_QUADROTOR,21:VTOL_TILTROTOR
AP_GROUPINFO ( " MAV_TYPE " , 57 , QuadPlane , mav_type , 0 ) ,
2017-10-29 03:31:09 -03:00
// @Param: OPTIONS
// @DisplayName: quadplane options
2021-06-08 22:14:07 -03:00
// @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, ThrLandControl: enable throttle stick control of landing rate, DisableApproach: Disable use of approach and airbrake stages in VTOL landing, EnableLandResponsition: enable pilot controlled repositioning in AUTO land. Descent will pause while repositioning.
2021-06-08 03:02:15 -03:00
// @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,15:ThrLandControl,16:DisableApproach,17:EnableLandResponsition
2017-10-29 03:31:09 -03:00
AP_GROUPINFO ( " OPTIONS " , 58 , QuadPlane , options , 0 ) ,
2017-11-05 05:44:42 -04:00
AP_SUBGROUPEXTENSION ( " " , 59 , QuadPlane , var_info2 ) ,
2021-03-25 17:32:31 -03:00
2021-03-16 18:54:03 -03:00
// 60 is used above for VELZ_MAX_DN
2021-03-25 17:32:31 -03:00
// 61 is used above for TS_ANGLE_VTOL
2018-12-07 03:52:05 -04:00
2015-11-24 04:24:04 -04:00
AP_GROUPEND
} ;
2017-11-05 05:44:42 -04:00
// second table of user settable parameters for quadplanes, this
// allows us to go beyond the 64 parameter limit
const AP_Param : : GroupInfo QuadPlane : : var_info2 [ ] = {
// @Param: TRANS_DECEL
// @DisplayName: Transition deceleration
// @Description: This is deceleration rate that will be used in calculating the stopping distance when transitioning from fixed wing flight to multicopter flight.
// @Units: m/s/s
// @Increment: 0.1
// @Range: 0.2 5
// @User: Standard
AP_GROUPINFO ( " TRANS_DECEL " , 1 , QuadPlane , transition_decel , 2.0 ) ,
2018-03-27 23:24:05 -03:00
// @Group: LOIT_
// @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
AP_SUBGROUPPTR ( loiter_nav , " LOIT_ " , 2 , QuadPlane , AC_Loiter ) ,
2020-11-05 18:49:33 -04:00
// @Param: TAILSIT_GSCMAX
// @DisplayName: Maximum tailsitter gain scaling
// @Description: Maximum gain scaling for tailsitter Q_TAILSIT_GSCMSK options
2018-04-15 08:31:53 -03:00
// @Range: 1 5
// @User: Standard
2020-11-05 18:49:33 -04:00
AP_GROUPINFO ( " TAILSIT_GSCMAX " , 3 , QuadPlane , tailsitter . throttle_scale_max , 2 ) ,
2018-09-25 12:13:34 -03:00
2018-11-12 01:00:00 -04:00
// @Param: TRIM_PITCH
2018-09-25 12:13:34 -03:00
// @DisplayName: Quadplane AHRS trim pitch
2018-11-12 01:00:00 -04:00
// @Description: This sets the compensation for the pitch angle trim difference between forward and vertical flight pitch, NOTE! this is relative to forward flight trim not mounting locaiton. For tailsitters this is relative to a baseline of 90 degrees.
2018-09-25 12:13:34 -03:00
// @Units: deg
2018-11-12 01:00:00 -04:00
// @Range: -10 +10
// @Increment: 0.1
// @User: Advanced
2018-09-25 12:13:34 -03:00
// @RebootRequired: True
2018-11-12 01:00:00 -04:00
AP_GROUPINFO ( " TRIM_PITCH " , 4 , QuadPlane , ahrs_trim_pitch , 0 ) ,
2018-09-25 12:13:34 -03:00
2018-10-06 11:40:27 -03:00
// @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 ) ,
2018-12-07 03:52:05 -04:00
# if QAUTOTUNE_ENABLED
// @Group: AUTOTUNE_
2019-10-31 19:31:40 -03:00
// @Path: ../libraries/AC_AutoTune/AC_AutoTune.cpp
2018-12-07 03:52:05 -04:00
AP_SUBGROUPINFO ( qautotune , " AUTOTUNE_ " , 6 , QuadPlane , QAutoTune ) ,
# endif
2018-11-16 17:53:43 -04:00
// @Param: FW_LND_APR_RAD
// @DisplayName: Quadplane fixed wing landing approach radius
// @Description: This provides the radius used, when using a fixed wing landing approach. If set to 0 then the WP_LOITER_RAD will be selected.
// @Units: m
// @Range: 0 200
// @Increment: 5
// @User: Advanced
AP_GROUPINFO ( " FW_LND_APR_RAD " , 7 , QuadPlane , fw_land_approach_radius , 0 ) ,
2018-12-14 22:40:43 -04:00
// @Param: TRANS_FAIL
// @DisplayName: Quadplane transition failure time
// @Description: Maximum time allowed for forward transitions, exceeding this time will cancel the transition and the aircraft will immediately change to QLAND. 0 for no limit.
// @Units: s
// @Range: 0 20
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ( " TRANS_FAIL " , 8 , QuadPlane , transition_failure , 0 ) ,
2019-01-17 13:54:15 -04:00
// @Param: TAILSIT_MOTMX
2021-05-28 17:18:53 -03:00
// @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.
2019-01-17 13:54:15 -04:00
// @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 ) ,
2019-04-06 15:38:40 -03:00
// @Param: THROTTLE_EXPO
// @DisplayName: Throttle expo strength
// @Description: Amount of curvature in throttle curve: 0 is linear, 1 is cubic
// @Range: 0 1
// @Increment: .1
// @User: Advanced
AP_GROUPINFO ( " THROTTLE_EXPO " , 10 , QuadPlane , throttle_expo , 0.2 ) ,
2019-03-24 00:35:13 -03:00
// @Param: ACRO_RLL_RATE
// @DisplayName: QACRO mode roll rate
// @Description: The maximum roll rate at full stick deflection in QACRO mode
// @Units: deg/s
// @Range: 10 500
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " ACRO_RLL_RATE " , 11 , QuadPlane , acro_roll_rate , 360 ) ,
// @Param: ACRO_PIT_RATE
// @DisplayName: QACRO mode pitch rate
// @Description: The maximum pitch rate at full stick deflection in QACRO mode
// @Units: deg/s
// @Range: 10 500
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " ACRO_PIT_RATE " , 12 , QuadPlane , acro_pitch_rate , 180 ) ,
// @Param: ACRO_YAW_RATE
// @DisplayName: QACRO mode yaw rate
// @Description: The maximum yaw rate at full stick deflection in QACRO mode
// @Units: deg/s
// @Range: 10 500
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " ACRO_YAW_RATE " , 13 , QuadPlane , acro_yaw_rate , 90 ) ,
2018-12-15 01:23:56 -04:00
// @Param: TKOFF_FAIL_SCL
// @DisplayName: Takeoff time failure scalar
// @Description: Scalar for how long past the expected takeoff time a takeoff should be considered as failed and the vehicle will switch to QLAND. If set to 0 there is no limit on takeoff time.
// @Range: 1.1 5.0
// @Increment: 5.1
// @User: Advanced
AP_GROUPINFO ( " TKOFF_FAIL_SCL " , 14 , QuadPlane , takeoff_failure_scalar , 0 ) ,
2018-12-15 16:47:49 -04:00
// @Param: TKOFF_ARSP_LIM
// @DisplayName: Takeoff airspeed limit
// @Description: Airspeed limit during takeoff. If the airspeed exceeds this level the vehicle will switch to QLAND. This is useful for ensuring that you don't takeoff into excessively strong wind. If set to 0 there is no limit on airspeed during takeoff.
// @Units: m/s
// @Range: 0 20
// @Increment: 1
// @User: Advanced
AP_GROUPINFO ( " TKOFF_ARSP_LIM " , 15 , QuadPlane , maximum_takeoff_airspeed , 0 ) ,
2019-11-01 07:44:23 -03:00
// @Param: ASSIST_ALT
// @DisplayName: Quadplane assistance altitude
// @Description: This is the altitude below which quadplane assistance will be triggered. This acts the same way as Q_ASSIST_ANGLE and Q_ASSIST_SPEED, but triggers if the aircraft drops below the given altitude while the VTOL motors are not running. A value of zero disables this feature. The altutude is calculated as being above ground level. The height above ground is given from a Lidar used if available and RNGFND_LANDING=1. Otherwise it comes from terrain data if TERRAIN_FOLLOW=1 and comes from height above home otherwise.
// @Units: m
// @Range: 0 120
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " ASSIST_ALT " , 16 , QuadPlane , assist_alt , 0 ) ,
2019-04-25 16:39:27 -03:00
// @Param: TAILSIT_GSCMSK
// @DisplayName: Tailsitter gain scaling mask
2020-11-03 19:52:08 -04:00
// @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
2019-04-25 16:39:27 -03:00
// @User: Standard
2020-11-03 19:52:08 -04:00
// @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 ) ,
2019-04-25 16:39:27 -03:00
// @Param: TAILSIT_GSCMIN
2020-11-05 18:49:33 -04:00
// @DisplayName: Minimum tailsitter gain scaling
// @Description: Minimum gain scaling for tailsitter Q_TAILSIT_GSCMSK options
2019-04-25 16:39:27 -03:00
// @Range: 0.1 1
// @User: Standard
AP_GROUPINFO ( " TAILSIT_GSCMIN " , 18 , QuadPlane , tailsitter . gain_scaling_min , 0.4 ) ,
2020-06-16 15:52:26 -03:00
2020-02-16 21:05:13 -04:00
// @Param: ASSIST_DELAY
// @DisplayName: Quadplane assistance delay
// @Description: This is delay between the assistance thresholds being met and the assistance starting.
// @Units: s
// @Range: 0 2
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO ( " ASSIST_DELAY " , 19 , QuadPlane , assist_delay , 0.5 ) ,
2020-06-09 19:05:07 -03:00
// @Param: FWD_MANTHR_MAX
// @DisplayName: VTOL manual forward throttle max percent
// @Description: Maximum value for manual forward throttle; used with RC option FWD_THR (209)
// @Range: 0 100
// @RebootRequired: False
AP_GROUPINFO ( " FWD_MANTHR_MAX " , 20 , QuadPlane , fwd_thr_max , 0 ) ,
2020-11-03 19:52:08 -04:00
// @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
2021-01-08 10:17:01 -04:00
// @Units: kg/m/m
2020-11-03 19:52:08 -04:00
// @Range: 0 50
// @User: Standard
AP_GROUPINFO ( " TAILSIT_DSKLD " , 21 , QuadPlane , tailsitter . disk_loading , 0 ) ,
2021-01-03 01:52:23 -04:00
// @Param: TILT_FIX_ANGLE
// @DisplayName: Fixed wing tiltrotor angle
// @Description: This is the angle the motors tilt down when at maximum output for forward flight. Set this to a non-zero value to enable vectoring for roll/pitch in forward flight on tilt-vectored aircraft
// @Units: deg
// @Range: 0 30
// @User: Standard
AP_GROUPINFO ( " TILT_FIX_ANGLE " , 22 , QuadPlane , tilt . fixed_angle , 0 ) ,
// @Param: TILT_FIX_GAIN
// @DisplayName: Fixed wing tiltrotor gain
// @Description: This is the gain for use of tilting motors in fixed wing flight for tilt vectored quadplanes
// @Range: 0 1
// @User: Standard
AP_GROUPINFO ( " TILT_FIX_GAIN " , 23 , QuadPlane , tilt . fixed_gain , 0 ) ,
2021-03-25 17:32:31 -03:00
// @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 ) ,
// @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 ) ,
2021-01-03 01:52:23 -04:00
2017-11-05 05:44:42 -04:00
AP_GROUPEND
} ;
2017-04-03 19:18:30 -03:00
/*
defaults for all quadplanes
*/
2018-11-28 20:54:55 -04:00
static const struct AP_Param : : defaults_table_struct defaults_table [ ] = {
2016-04-01 02:44:49 -03:00
{ " Q_A_RAT_RLL_P " , 0.25 } ,
{ " Q_A_RAT_RLL_I " , 0.25 } ,
2019-06-27 06:36:57 -03:00
{ " Q_A_RAT_RLL_FLTD " , 10.0 } ,
2021-04-12 01:22:10 -03:00
{ " Q_A_RAT_RLL_SMAX " , 50.0 } ,
2016-04-01 02:44:49 -03:00
{ " Q_A_RAT_PIT_P " , 0.25 } ,
{ " Q_A_RAT_PIT_I " , 0.25 } ,
2019-06-27 06:36:57 -03:00
{ " Q_A_RAT_PIT_FLTD " , 10.0 } ,
2021-04-12 01:22:10 -03:00
{ " Q_A_RAT_PIT_SMAX " , 50.0 } ,
{ " Q_A_RAT_YAW_SMAX " , 50.0 } ,
2021-11-06 07:40:14 -03:00
{ " Q_A_RATE_R_MAX " , 75.0 } ,
{ " Q_A_RATE_P_MAX " , 75.0 } ,
{ " Q_A_RATE_Y_MAX " , 75.0 } ,
2017-02-13 06:35:41 -04:00
{ " Q_M_SPOOL_TIME " , 0.25 } ,
2018-06-09 04:41:02 -03:00
{ " Q_LOIT_ANG_MAX " , 15.0 } ,
{ " Q_LOIT_ACC_MAX " , 250.0 } ,
{ " Q_LOIT_BRK_ACCEL " , 50.0 } ,
{ " Q_LOIT_BRK_JERK " , 250 } ,
{ " Q_LOIT_SPEED " , 500 } ,
2021-03-02 00:43:27 -04:00
{ " Q_WP_SPEED " , 500 } ,
{ " Q_WP_ACCEL " , 100 } ,
2016-04-01 02:40:06 -03:00
} ;
2017-04-03 19:18:30 -03:00
/*
extra defaults for tailsitters
*/
2018-11-28 20:54:55 -04:00
static const struct AP_Param : : defaults_table_struct defaults_table_tailsitter [ ] = {
2017-04-03 19:18:30 -03:00
{ " KFF_RDDRMIX " , 0.02 } ,
{ " Q_A_RAT_PIT_FF " , 0.2 } ,
{ " Q_A_RAT_YAW_FF " , 0.2 } ,
2019-09-08 17:18:06 -03:00
{ " Q_A_RAT_YAW_I " , 0.18 } ,
{ " Q_A_ANGLE_BOOST " , 0 } ,
2017-04-03 19:18:30 -03:00
{ " LIM_PITCH_MAX " , 3000 } ,
{ " LIM_PITCH_MIN " , - 3000 } ,
{ " MIXING_GAIN " , 1.0 } ,
2017-04-05 00:37:49 -03:00
{ " RUDD_DT_GAIN " , 10 } ,
2017-11-05 02:25:00 -04:00
{ " Q_TRANSITION_MS " , 2000 } ,
2021-05-18 21:12:49 -03:00
{ " Q_TRANS_DECEL " , 6 } ,
2017-04-03 19:18:30 -03:00
} ;
2018-01-24 00:40:49 -04:00
/*
conversion table for quadplane parameters
*/
const AP_Param : : ConversionInfo q_conversion_table [ ] = {
{ Parameters : : k_param_quadplane , 4044 , AP_PARAM_FLOAT , " Q_P_POSZ_P " } , // Q_PZ_P
{ Parameters : : k_param_quadplane , 4045 , AP_PARAM_FLOAT , " Q_P_POSXY_P " } , // Q_PXY_P
{ Parameters : : k_param_quadplane , 4046 , AP_PARAM_FLOAT , " Q_P_VELXY_P " } , // Q_VXY_P
{ Parameters : : k_param_quadplane , 78 , AP_PARAM_FLOAT , " Q_P_VELXY_I " } , // Q_VXY_I
{ Parameters : : k_param_quadplane , 142 , AP_PARAM_FLOAT , " Q_P_VELXY_IMAX " } , // Q_VXY_IMAX
2021-08-03 01:06:07 -03:00
{ Parameters : : k_param_quadplane , 206 , AP_PARAM_FLOAT , " Q_P_VELXY_FLTE " } , // Q_VXY_FILT_HZ
2018-01-24 00:40:49 -04:00
{ Parameters : : k_param_quadplane , 4047 , AP_PARAM_FLOAT , " Q_P_VELZ_P " } , // Q_VZ_P
2018-01-27 02:41:35 -04:00
{ Parameters : : k_param_quadplane , 4048 , AP_PARAM_FLOAT , " Q_P_ACCZ_P " } , // Q_AZ_P
{ Parameters : : k_param_quadplane , 80 , AP_PARAM_FLOAT , " Q_P_ACCZ_I " } , // Q_AZ_I
{ Parameters : : k_param_quadplane , 144 , AP_PARAM_FLOAT , " Q_P_ACCZ_D " } , // Q_AZ_D
{ Parameters : : k_param_quadplane , 336 , AP_PARAM_FLOAT , " Q_P_ACCZ_IMAX " } , // Q_AZ_IMAX
2019-06-27 06:36:57 -03:00
{ Parameters : : k_param_quadplane , 400 , AP_PARAM_FLOAT , " Q_P_ACCZ_FLTD " } , // Q_AZ_FILT
2018-01-27 02:41:35 -04:00
{ Parameters : : k_param_quadplane , 464 , AP_PARAM_FLOAT , " Q_P_ACCZ_FF " } , // Q_AZ_FF
2018-03-27 23:24:05 -03:00
{ Parameters : : k_param_quadplane , 276 , AP_PARAM_FLOAT , " Q_LOIT_SPEED " } , // Q_WP_LOIT_SPEED
{ Parameters : : k_param_quadplane , 468 , AP_PARAM_FLOAT , " Q_LOIT_BRK_JERK " } , // Q_WP_LOIT_JERK
{ Parameters : : k_param_quadplane , 532 , AP_PARAM_FLOAT , " Q_LOIT_ACC_MAX " } , // Q_WP_LOIT_MAXA
{ Parameters : : k_param_quadplane , 596 , AP_PARAM_FLOAT , " Q_LOIT_BRK_ACCEL " } , // Q_WP_LOIT_MINA
2019-06-27 06:36:57 -03:00
{ Parameters : : k_param_q_attitude_control , 385 , AP_PARAM_FLOAT , " Q_A_RAT_RLL_FLTD " } , // Q_A_RAT_RLL_FILT
{ Parameters : : k_param_q_attitude_control , 386 , AP_PARAM_FLOAT , " Q_A_RAT_PIT_FLTD " } , // Q_A_RAT_PIT_FILT
{ Parameters : : k_param_q_attitude_control , 387 , AP_PARAM_FLOAT , " Q_A_RAT_YAW_FLTE " } , // Q_A_RAT_YAW_FILT
{ Parameters : : k_param_q_attitude_control , 449 , AP_PARAM_FLOAT , " Q_A_RAT_RLL_FF " } , // Q_A_RAT_RLL_FF
{ Parameters : : k_param_q_attitude_control , 450 , AP_PARAM_FLOAT , " Q_A_RAT_PIT_FF " } , // Q_A_RAT_PIT_FF
{ Parameters : : k_param_q_attitude_control , 451 , AP_PARAM_FLOAT , " Q_A_RAT_YAW_FF " } , // Q_A_RAT_YAW_FILT
2021-09-04 14:16:53 -03:00
{ Parameters : : k_param_quadplane , 22 , AP_PARAM_INT16 , " Q_M_PWM_MIN " } ,
{ Parameters : : k_param_quadplane , 23 , AP_PARAM_INT16 , " Q_M_PWM_MAX " } ,
2018-01-24 00:40:49 -04:00
} ;
2015-11-24 04:24:04 -04:00
QuadPlane : : QuadPlane ( AP_AHRS_NavEKF & _ahrs ) :
ahrs ( _ahrs )
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2017-11-05 05:44:42 -04:00
AP_Param : : setup_object_defaults ( this , var_info2 ) ;
2020-11-06 12:20:02 -04:00
if ( _singleton ! = nullptr ) {
AP_HAL : : panic ( " Can only be one Quadplane " ) ;
}
_singleton = this ;
2015-11-24 04:24:04 -04:00
}
2016-02-07 20:00:19 -04:00
// setup default motors for the frame class
void QuadPlane : : setup_default_channels ( uint8_t num_motors )
{
for ( uint8_t i = 0 ; i < num_motors ; i + + ) {
2017-10-23 00:16:14 -03:00
SRV_Channels : : set_aux_channel_default ( SRV_Channels : : get_motor_function ( i ) , CH_5 + i ) ;
2016-02-07 20:00:19 -04:00
}
}
2016-01-06 03:17:08 -04:00
bool QuadPlane : : setup ( void )
2015-11-24 04:24:04 -04:00
{
2016-01-06 03:17:08 -04:00
if ( initialised ) {
return true ;
}
if ( ! enable | | hal . util - > get_soft_armed ( ) ) {
return false ;
2015-12-26 06:40:40 -04:00
}
2016-04-02 17:49:38 -03:00
float loop_delta_t = 1.0 / plane . scheduler . get_loop_rate_hz ( ) ;
2016-12-30 19:50:01 -04:00
2017-02-11 04:12:56 -04:00
enum Rotation rotation = ROTATION_NONE ;
2019-02-04 13:14:01 -04:00
2016-12-30 19:50:01 -04:00
/*
cope with upgrade from old AP_Motors values for frame_class
*/
AP_Int8 old_class ;
const AP_Param : : ConversionInfo cinfo { Parameters : : k_param_quadplane , FRAME_CLASS_OLD_IDX , AP_PARAM_INT8 , nullptr } ;
if ( AP_Param : : find_old_parameter ( & cinfo , & old_class ) & & ! frame_class . load ( ) ) {
uint8_t new_value = 0 ;
// map from old values to new values
switch ( old_class . get ( ) ) {
case 0 :
new_value = AP_Motors : : MOTOR_FRAME_QUAD ;
break ;
case 1 :
new_value = AP_Motors : : MOTOR_FRAME_HEXA ;
break ;
case 2 :
new_value = AP_Motors : : MOTOR_FRAME_OCTA ;
break ;
case 3 :
new_value = AP_Motors : : MOTOR_FRAME_OCTAQUAD ;
break ;
case 4 :
new_value = AP_Motors : : MOTOR_FRAME_Y6 ;
break ;
}
frame_class . set_and_save ( new_value ) ;
}
2016-04-02 17:49:38 -03:00
2016-01-06 03:17:08 -04:00
if ( hal . util - > available_memory ( ) <
2018-08-24 02:42:37 -03:00
4096 + sizeof ( * motors ) + sizeof ( * attitude_control ) + sizeof ( * pos_control ) + sizeof ( * wp_nav ) + sizeof ( * ahrs_view ) + sizeof ( * loiter_nav ) ) {
2019-12-16 17:17:54 -04:00
AP_BoardConfig : : config_error ( " Not enough memory for quadplane " ) ;
2016-01-06 03:17:08 -04:00
}
2016-02-07 20:00:19 -04:00
2015-12-26 06:40:40 -04:00
/*
dynamically allocate the key objects for quadplane . This ensures
that the objects don ' t affect the vehicle unless enabled and
also saves memory when not in use
*/
2020-11-06 10:09:23 -04:00
switch ( ( AP_Motors : : motor_frame_class ) frame_class ) {
2016-12-12 09:08:16 -04:00
case AP_Motors : : MOTOR_FRAME_QUAD :
2016-02-07 20:00:19 -04:00
setup_default_channels ( 4 ) ;
break ;
2016-12-12 09:08:16 -04:00
case AP_Motors : : MOTOR_FRAME_HEXA :
2016-02-07 20:00:19 -04:00
setup_default_channels ( 6 ) ;
break ;
2016-12-12 09:08:16 -04:00
case AP_Motors : : MOTOR_FRAME_OCTA :
case AP_Motors : : MOTOR_FRAME_OCTAQUAD :
2016-02-07 20:00:19 -04:00
setup_default_channels ( 8 ) ;
break ;
2016-12-12 09:08:16 -04:00
case AP_Motors : : MOTOR_FRAME_Y6 :
2016-04-21 21:28:06 -03:00
setup_default_channels ( 7 ) ;
2016-03-12 19:04:25 -04:00
break ;
2017-01-09 04:19:42 -04:00
case AP_Motors : : MOTOR_FRAME_TRI :
SRV_Channels : : set_default_function ( CH_5 , SRV_Channel : : k_motor1 ) ;
SRV_Channels : : set_default_function ( CH_6 , SRV_Channel : : k_motor2 ) ;
SRV_Channels : : set_default_function ( CH_8 , SRV_Channel : : k_motor4 ) ;
SRV_Channels : : set_default_function ( CH_11 , SRV_Channel : : k_motor7 ) ;
2017-02-07 19:47:17 -04:00
AP_Param : : set_frame_type_flags ( AP_PARAM_FRAME_TRICOPTER ) ;
2017-01-09 04:19:42 -04:00
break ;
2017-02-11 01:50:03 -04:00
case AP_Motors : : MOTOR_FRAME_TAILSITTER :
2021-10-18 06:47:24 -03:00
case AP_Motors : : MOTOR_FRAME_SCRIPTING_MATRIX :
2017-02-11 01:50:03 -04:00
break ;
2016-02-07 20:00:19 -04:00
default :
2020-11-06 10:09:23 -04:00
AP_BoardConfig : : config_error ( " Unsupported Q_FRAME_CLASS %u " , frame_class ) ;
2016-02-07 20:00:19 -04:00
}
2017-02-11 01:50:03 -04:00
2019-01-17 13:54:15 -04:00
if ( tailsitter . motor_mask = = 0 ) {
// this is a normal quadplane
2020-11-06 10:09:23 -04:00
switch ( ( AP_Motors : : motor_frame_class ) frame_class ) {
2019-01-17 13:54:15 -04:00
case AP_Motors : : MOTOR_FRAME_TRI :
motors = new AP_MotorsTri ( plane . scheduler . get_loop_rate_hz ( ) , rc_speed ) ;
motors_var_info = AP_MotorsTri : : var_info ;
break ;
case AP_Motors : : MOTOR_FRAME_TAILSITTER :
// this is a duo-motor tailsitter (vectored thrust if tilt.tilt_mask != 0)
motors = new AP_MotorsTailsitter ( plane . scheduler . get_loop_rate_hz ( ) , rc_speed ) ;
motors_var_info = AP_MotorsTailsitter : : var_info ;
2019-03-18 21:00:25 -03:00
if ( tilt . tilt_type ! = TILT_TYPE_BICOPTER ) {
rotation = ROTATION_PITCH_90 ;
}
2019-01-17 13:54:15 -04:00
break ;
default :
motors = new AP_MotorsMatrix ( plane . scheduler . get_loop_rate_hz ( ) , rc_speed ) ;
motors_var_info = AP_MotorsMatrix : : var_info ;
break ;
}
} else {
// this is a copter tailsitter with motor layout specified by frame_class and frame_type
// tilting motors are not supported (tiltrotor control variables are ignored)
if ( tilt . tilt_mask ! = 0 ) {
2021-05-14 07:18:55 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_ERROR , " Warning: Motor tilt not supported \n " ) ;
2019-01-17 13:54:15 -04:00
}
2017-02-11 04:12:56 -04:00
rotation = ROTATION_PITCH_90 ;
2020-08-09 11:23:49 -03:00
motors = new AP_MotorsMatrix ( plane . scheduler . get_loop_rate_hz ( ) , rc_speed ) ;
motors_var_info = AP_MotorsMatrix : : var_info ;
2017-01-09 04:19:42 -04:00
}
2019-05-01 19:44:00 -03:00
2015-12-26 06:40:40 -04:00
if ( ! motors ) {
2019-12-16 17:17:54 -04:00
AP_BoardConfig : : config_error ( " Unable to allocate %s " , " motors " ) ;
2015-12-26 06:40:40 -04:00
}
2017-02-11 04:12:56 -04:00
2017-02-27 02:39:46 -04:00
AP_Param : : load_object_from_eeprom ( motors , motors_var_info ) ;
2017-02-11 04:12:56 -04:00
// create the attitude view used by the VTOL code
2018-11-12 01:00:00 -04:00
ahrs_view = ahrs . create_view ( rotation , ahrs_trim_pitch ) ;
2017-02-11 04:12:56 -04:00
if ( ahrs_view = = nullptr ) {
2019-12-16 17:17:54 -04:00
AP_BoardConfig : : config_error ( " Unable to allocate %s " , " ahrs_view " ) ;
2017-02-11 04:12:56 -04:00
}
2018-09-25 12:13:34 -03:00
2020-11-29 11:46:01 -04:00
attitude_control = new AC_AttitudeControl_TS ( * ahrs_view , aparm , * motors , loop_delta_t ) ;
2015-12-26 06:40:40 -04:00
if ( ! attitude_control ) {
2019-12-16 17:17:54 -04:00
AP_BoardConfig : : config_error ( " Unable to allocate %s " , " attitude_control " ) ;
2015-12-26 06:40:40 -04:00
}
2020-09-02 23:45:52 -03:00
2015-12-26 06:40:40 -04:00
AP_Param : : load_object_from_eeprom ( attitude_control , attitude_control - > var_info ) ;
2021-05-03 11:44:05 -03:00
pos_control = new AC_PosControl ( * ahrs_view , inertial_nav , * motors , * attitude_control , loop_delta_t ) ;
2015-12-26 06:40:40 -04:00
if ( ! pos_control ) {
2019-12-16 17:17:54 -04:00
AP_BoardConfig : : config_error ( " Unable to allocate %s " , " pos_control " ) ;
2015-12-26 06:40:40 -04:00
}
AP_Param : : load_object_from_eeprom ( pos_control , pos_control - > var_info ) ;
2017-02-11 18:34:08 -04:00
wp_nav = new AC_WPNav ( inertial_nav , * ahrs_view , * pos_control , * attitude_control ) ;
2016-10-26 23:54:15 -03:00
if ( ! wp_nav ) {
2019-12-16 17:17:54 -04:00
AP_BoardConfig : : config_error ( " Unable to allocate %s " , " wp_nav " ) ;
2015-12-26 06:40:40 -04:00
}
AP_Param : : load_object_from_eeprom ( wp_nav , wp_nav - > var_info ) ;
2021-04-07 14:41:24 -03:00
# if AP_TERRAIN_AVAILABLE
wp_nav - > set_terrain ( & plane . terrain ) ;
# endif
2015-12-26 06:40:40 -04:00
2018-03-27 23:24:05 -03:00
loiter_nav = new AC_Loiter ( inertial_nav , * ahrs_view , * pos_control , * attitude_control ) ;
if ( ! loiter_nav ) {
2019-12-16 17:17:54 -04:00
AP_BoardConfig : : config_error ( " Unable to allocate %s " , " loiter_nav " ) ;
2018-03-27 23:24:05 -03:00
}
AP_Param : : load_object_from_eeprom ( loiter_nav , loiter_nav - > var_info ) ;
2020-11-06 10:09:23 -04:00
motors - > init ( frame_class , frame_type ) ;
2021-09-04 14:14:09 -03:00
motors - > update_throttle_range ( ) ;
2015-12-26 06:40:40 -04:00
motors - > set_update_rate ( rc_speed ) ;
motors - > set_interlock ( true ) ;
2016-09-01 08:32:24 -03:00
attitude_control - > parameter_sanity_check ( ) ;
2015-12-26 06:40:40 -04:00
2020-09-02 23:45:52 -03:00
// TODO: update this if servo function assignments change
// used by relax_attitude_control() to control special behavior for vectored tailsitters
2020-11-06 10:09:23 -04:00
_is_vectored = ( frame_class = = AP_Motors : : MOTOR_FRAME_TAILSITTER ) & &
2020-09-02 23:45:52 -03:00
( ! is_zero ( tailsitter . vectored_hover_gain ) & &
( SRV_Channels : : function_assigned ( SRV_Channel : : k_tiltMotorLeft ) | |
SRV_Channels : : function_assigned ( SRV_Channel : : k_tiltMotorRight ) ) ) ;
2019-10-17 03:03:48 -03:00
// setup the trim of any motors used by AP_Motors so I/O board
2016-01-01 22:29:05 -04:00
// failsafe will disable motors
2016-05-06 05:09:07 -03:00
for ( uint8_t i = 0 ; i < 8 ; i + + ) {
2017-10-23 00:16:14 -03:00
SRV_Channel : : Aux_servo_function_t func = SRV_Channels : : get_motor_function ( i ) ;
2021-09-04 14:16:53 -03:00
SRV_Channels : : set_failsafe_pwm ( func , motors - > get_pwm_output_min ( ) ) ;
2016-01-01 22:29:05 -04:00
}
2015-12-26 06:40:40 -04:00
transition_state = TRANSITION_DONE ;
2019-05-01 19:44:00 -03:00
if ( tilt . tilt_mask ! = 0 ) {
// setup tilt compensation
motors - > set_thrust_compensation_callback ( FUNCTOR_BIND_MEMBER ( & QuadPlane : : tilt_compensate , void , float * , uint8_t ) ) ;
if ( tilt . tilt_type = = TILT_TYPE_VECTORED_YAW ) {
// setup tilt servos for vectored yaw
SRV_Channels : : set_range ( SRV_Channel : : k_tiltMotorLeft , 1000 ) ;
SRV_Channels : : set_range ( SRV_Channel : : k_tiltMotorRight , 1000 ) ;
2020-12-18 06:54:08 -04:00
SRV_Channels : : set_range ( SRV_Channel : : k_tiltMotorRear , 1000 ) ;
SRV_Channels : : set_range ( SRV_Channel : : k_tiltMotorRearLeft , 1000 ) ;
SRV_Channels : : set_range ( SRV_Channel : : k_tiltMotorRearRight , 1000 ) ;
2019-05-01 19:44:00 -03:00
}
2017-04-22 22:35:25 -03:00
}
2020-05-26 13:41:08 -03:00
// default QAssist state as set with Q_OPTIONS
if ( ( options & OPTION_Q_ASSIST_FORCE_ENABLE ) ! = 0 ) {
q_assist_state = Q_ASSIST_STATE_ENUM : : Q_ASSIST_FORCE ;
}
2016-04-01 02:40:06 -03:00
setup_defaults ( ) ;
2018-01-24 00:40:49 -04:00
AP_Param : : convert_old_parameters ( & q_conversion_table [ 0 ] , ARRAY_SIZE ( q_conversion_table ) ) ;
2019-01-17 13:54:15 -04:00
2021-03-25 17:32:31 -03:00
// Set tailsitter transition rate to match old caculation
if ( ! tailsitter . transition_rate_fw . configured ( ) ) {
tailsitter . transition_rate_fw . set_and_save ( tailsitter . transition_angle_fw / ( transition_time_ms / 2000.0f ) ) ;
}
2021-11-05 19:53:25 -03:00
// init wp_nav variables after detaults are setup
wp_nav - > wp_and_spline_init ( ) ;
2020-04-18 20:01:25 -03:00
// param count will have changed
AP_Param : : invalidate_count ( ) ;
2021-05-14 07:18:55 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " QuadPlane initialised, class: %s, type: %s " , motors - > get_frame_string ( ) , motors - > get_type_string ( ) ) ;
2016-01-06 03:17:08 -04:00
initialised = true ;
return true ;
2015-11-24 04:24:04 -04:00
}
2017-04-03 19:18:30 -03:00
/*
setup default parameters from defaults_table
*/
void QuadPlane : : setup_defaults ( void )
{
2018-11-28 20:54:55 -04:00
AP_Param : : set_defaults_from_table ( defaults_table , ARRAY_SIZE ( defaults_table ) ) ;
2017-04-03 19:18:30 -03:00
2020-11-06 10:09:23 -04:00
if ( frame_class = = AP_Motors : : MOTOR_FRAME_TAILSITTER ) {
2018-11-28 20:54:55 -04:00
AP_Param : : set_defaults_from_table ( defaults_table_tailsitter , ARRAY_SIZE ( defaults_table_tailsitter ) ) ;
2017-04-03 19:18:30 -03:00
}
2016-06-02 00:10:39 -03:00
// reset ESC calibration
if ( esc_calibration ! = 0 ) {
esc_calibration . set_and_save ( 0 ) ;
}
2020-12-02 22:13:25 -04:00
// Quadplanes need the same level of GPS error checking as Copters do, Plane is more relaxed
AP_Param : : set_default_by_name ( " EK2_CHECK_SCALE " , 100 ) ;
AP_Param : : set_default_by_name ( " EK3_CHECK_SCALE " , 100 ) ;
2016-04-01 02:40:06 -03:00
}
2016-06-02 00:10:39 -03:00
// run ESC calibration
void QuadPlane : : run_esc_calibration ( void )
{
if ( ! motors - > armed ( ) ) {
motors - > set_throttle_passthrough_for_esc_calibration ( 0 ) ;
AP_Notify : : flags . esc_calibration = false ;
return ;
}
if ( ! AP_Notify : : flags . esc_calibration ) {
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Starting ESC calibration " ) ;
2016-06-02 00:10:39 -03:00
}
AP_Notify : : flags . esc_calibration = true ;
switch ( esc_calibration ) {
case 1 :
// throttle based calibration
2018-11-09 18:38:43 -04:00
motors - > set_throttle_passthrough_for_esc_calibration ( plane . get_throttle_input ( ) * 0.01f ) ;
2016-06-02 00:10:39 -03:00
break ;
case 2 :
// full range calibration
motors - > set_throttle_passthrough_for_esc_calibration ( 1 ) ;
break ;
}
}
2015-12-26 03:45:42 -04:00
// init quadplane stabilize mode
void QuadPlane : : init_stabilize ( void )
{
2015-12-26 05:13:20 -04:00
throttle_wait = false ;
2015-12-26 03:45:42 -04:00
}
2020-12-15 00:03:42 -04:00
/*
when doing a forward transition of a tilt - vectored quadplane we use
euler angle control to maintain good yaw . This updates the yaw
target based on pilot input and target roll
*/
void QuadPlane : : update_yaw_target ( void )
{
uint32_t now = AP_HAL : : millis ( ) ;
if ( now - tilt . transition_yaw_set_ms > 100 | |
! is_zero ( get_pilot_input_yaw_rate_cds ( ) ) ) {
// lock initial yaw when transition is started or when
// pilot commands a yaw change. This allows us to track
// straight in transitions for tilt-vectored planes, but
// allows for turns when level transition is not wanted
tilt . transition_yaw_cd = ahrs . yaw_sensor ;
}
/*
now calculate the equivalent yaw rate for a coordinated turn for
the desired bank angle given the airspeed
*/
float aspeed ;
bool have_airspeed = ahrs . airspeed_estimate ( aspeed ) ;
2021-02-26 19:33:09 -04:00
if ( have_airspeed & & labs ( plane . nav_roll_cd ) > 1000 ) {
2020-12-15 00:03:42 -04:00
float dt = ( now - tilt . transition_yaw_set_ms ) * 0.001 ;
// calculate the yaw rate to achieve the desired turn rate
const float airspeed_min = MAX ( plane . aparm . airspeed_min , 5 ) ;
2021-02-26 19:33:09 -04:00
const float yaw_rate_cds = fixedwing_turn_rate ( plane . nav_roll_cd * 0.01 , MAX ( aspeed , airspeed_min ) ) * 100 ;
2020-12-15 00:03:42 -04:00
tilt . transition_yaw_cd + = yaw_rate_cds * dt ;
}
tilt . transition_yaw_set_ms = now ;
}
2017-03-13 04:45:46 -03:00
/*
ask the multicopter attitude control to match the roll and pitch rates being demanded by the
fixed wing controller if not in a pure VTOL mode
*/
2017-07-10 01:00:45 -03:00
void QuadPlane : : multicopter_attitude_rate_update ( float yaw_rate_cds )
2017-03-13 04:45:46 -03:00
{
2018-09-13 21:02:18 -03:00
check_attitude_relax ( ) ;
2020-12-15 00:03:42 -04:00
bool use_multicopter_control = in_vtol_mode ( ) & & ! in_tailsitter_vtol_transition ( ) ;
bool use_multicopter_eulers = false ;
if ( ! use_multicopter_control & &
tilt . is_vectored & &
transition_state < = TRANSITION_TIMER ) {
update_yaw_target ( ) ;
use_multicopter_control = true ;
use_multicopter_eulers = true ;
}
2020-01-02 15:07:09 -04:00
// normal control modes for VTOL and FW flight
2020-10-07 10:51:19 -03:00
// tailsitter in transition to VTOL flight is not really in a VTOL mode yet
2020-12-15 00:03:42 -04:00
if ( use_multicopter_control ) {
2020-01-02 15:07:09 -04:00
// tailsitter-only body-frame roll control options
// Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw.
if ( is_tailsitter ( ) & &
( tailsitter . input_type & TAILSITTER_INPUT_BF_ROLL ) ) {
if ( ! ( tailsitter . input_type & TAILSITTER_INPUT_PLANE ) ) {
// In multicopter input mode, the roll and yaw stick axes are independent of pitch
attitude_control - > input_euler_rate_yaw_euler_angle_pitch_bf_roll ( false ,
plane . nav_roll_cd ,
plane . nav_pitch_cd ,
yaw_rate_cds ) ;
return ;
} else {
// In plane input mode, the roll and yaw sticks are swapped
// and their effective axes rotate from yaw to roll and vice versa
// as pitch goes from zero to 90.
// So it is necessary to also rotate their scaling.
// Get the roll angle and yaw rate limits
int16_t roll_limit = aparm . angle_max ;
// separate limit for tailsitter roll, if set
if ( tailsitter . max_roll_angle > 0 ) {
roll_limit = tailsitter . max_roll_angle * 100.0f ;
}
// Prevent a divide by zero
float yaw_rate_limit = ( ( yaw_rate_max < 1.0f ) ? 1 : yaw_rate_max ) * 100.0f ;
float yaw2roll_scale = roll_limit / yaw_rate_limit ;
// Rotate as a function of Euler pitch and swap roll/yaw
float euler_pitch = radians ( .01f * plane . nav_pitch_cd ) ;
float spitch = fabsf ( sinf ( euler_pitch ) ) ;
float y2r_scale = linear_interpolate ( 1 , yaw2roll_scale , spitch , 0 , 1 ) ;
float p_yaw_rate = plane . nav_roll_cd / y2r_scale ;
float p_roll_angle = - y2r_scale * yaw_rate_cds ;
attitude_control - > input_euler_rate_yaw_euler_angle_pitch_bf_roll ( true ,
p_roll_angle ,
plane . nav_pitch_cd ,
p_yaw_rate ) ;
return ;
2019-11-30 21:25:46 -04:00
}
2019-03-03 13:54:23 -04:00
}
2019-04-27 17:03:37 -03:00
2020-12-15 00:03:42 -04:00
if ( use_multicopter_eulers ) {
attitude_control - > input_euler_angle_roll_pitch_yaw ( plane . nav_roll_cd ,
plane . nav_pitch_cd ,
tilt . transition_yaw_cd ,
true ) ;
} else {
// use euler angle attitude control
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( plane . nav_roll_cd ,
plane . nav_pitch_cd ,
yaw_rate_cds ) ;
}
2017-03-13 04:45:46 -03:00
} else {
// use the fixed wing desired rates
2019-06-27 06:31:52 -03:00
float roll_rate = plane . rollController . get_pid_info ( ) . target ;
float pitch_rate = plane . pitchController . get_pid_info ( ) . target ;
2020-01-02 15:07:09 -04:00
if ( is_tailsitter ( ) ) {
// tailsitter roll and yaw swapped due to change in reference frame
attitude_control - > input_rate_bf_roll_pitch_yaw_2 ( yaw_rate_cds , pitch_rate * 100.0f , - roll_rate * 100.0f ) ;
} else {
attitude_control - > input_rate_bf_roll_pitch_yaw_2 ( roll_rate * 100.0f , pitch_rate * 100.0f , yaw_rate_cds ) ;
}
2017-03-13 04:45:46 -03:00
}
}
2015-12-26 04:27:13 -04:00
// hold in stabilize with given throttle
void QuadPlane : : hold_stabilize ( float throttle_in )
2015-12-26 04:51:05 -04:00
{
2015-11-24 04:24:04 -04:00
// call attitude controller
2017-07-10 01:00:45 -03:00
multicopter_attitude_rate_update ( get_desired_yaw_rate_cds ( ) ) ;
2015-11-24 04:24:04 -04:00
2020-07-07 21:22:56 -03:00
if ( ( throttle_in < = 0 ) & & ( air_mode = = AirMode : : OFF ) ) {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : GROUND_IDLE ) ;
2018-12-28 02:35:08 -04:00
attitude_control - > set_throttle_out ( 0 , true , 0 ) ;
2020-09-02 23:45:52 -03:00
relax_attitude_control ( ) ;
2016-01-05 19:32:25 -04:00
} else {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2020-01-02 15:07:09 -04:00
bool should_boost = true ;
if ( is_tailsitter ( ) & & assisted_flight ) {
// tailsitters in forward flight should not use angle boost
should_boost = false ;
}
attitude_control - > set_throttle_out ( throttle_in , should_boost , 0 ) ;
2016-01-05 19:32:25 -04:00
}
2015-12-26 04:27:13 -04:00
}
// quadplane stabilize mode
void QuadPlane : : control_stabilize ( void )
{
2016-06-02 00:10:39 -03:00
// special check for ESC calibration in QSTABILIZE
if ( esc_calibration ! = 0 ) {
run_esc_calibration ( ) ;
return ;
}
// normal QSTABILIZE mode
2019-04-06 15:38:40 -03:00
float pilot_throttle_scaled = get_pilot_throttle ( ) ;
2015-12-26 04:27:13 -04:00
hold_stabilize ( pilot_throttle_scaled ) ;
2015-11-24 04:24:04 -04:00
}
2017-03-13 04:04:09 -03:00
// run the multicopter Z controller
void QuadPlane : : run_z_controller ( void )
{
2018-01-24 20:38:07 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
2021-05-03 11:44:05 -03:00
if ( ! pos_control - > is_active_z ( ) ) {
2021-05-19 11:08:14 -03:00
// set vertical speed and acceleration limits
2021-05-03 11:44:05 -03:00
pos_control - > set_max_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2021-05-19 11:08:14 -03:00
// initialise the vertical position controller
2020-10-30 18:36:15 -03:00
if ( ! is_tailsitter ( ) ) {
2021-05-03 11:44:05 -03:00
pos_control - > init_z_controller ( ) ;
2020-10-30 18:36:15 -03:00
} else {
2021-05-19 11:08:14 -03:00
// initialise the vertical position controller with no descent
2021-05-03 11:44:05 -03:00
pos_control - > init_z_controller_no_descent ( ) ;
2020-10-30 18:36:15 -03:00
}
2021-08-28 10:50:19 -03:00
last_pidz_init_ms = now ;
2017-03-13 04:04:09 -03:00
}
last_pidz_active_ms = now ;
2021-04-04 16:48:00 -03:00
pos_control - > update_z_controller ( ) ;
2017-03-13 04:04:09 -03:00
}
2020-09-02 23:45:52 -03:00
void QuadPlane : : relax_attitude_control ( )
{
// disable roll and yaw control for vectored tailsitters
// if not a vectored tailsitter completely disable attitude control
attitude_control - > relax_attitude_controllers ( _is_vectored ) ;
}
2018-09-13 21:02:18 -03:00
/*
check if we should relax the attitude controllers
We relax them whenever we will be using them after a period of
inactivity
*/
void QuadPlane : : check_attitude_relax ( void )
{
uint32_t now = AP_HAL : : millis ( ) ;
2018-09-14 21:03:05 -03:00
if ( now - last_att_control_ms > 100 ) {
2020-09-02 23:45:52 -03:00
relax_attitude_control ( ) ;
2018-09-13 21:02:18 -03:00
}
last_att_control_ms = now ;
}
2019-02-17 14:42:05 -04:00
/*
init QACRO mode
*/
void QuadPlane : : init_qacro ( void )
{
throttle_wait = false ;
transition_state = TRANSITION_DONE ;
attitude_control - > relax_attitude_controllers ( ) ;
}
2015-12-26 03:45:42 -04:00
// init quadplane hover mode
void QuadPlane : : init_hover ( void )
{
2021-05-19 11:08:14 -03:00
// set vertical speed and acceleration limits
2021-05-03 11:44:05 -03:00
pos_control - > set_max_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2021-07-08 01:16:41 -03:00
pos_control - > set_correction_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2021-05-14 06:16:37 -03:00
set_climb_rate_cms ( 0 , false ) ;
2015-12-26 05:13:20 -04:00
init_throttle_wait ( ) ;
2015-12-26 03:45:42 -04:00
}
2017-04-18 09:40:06 -03:00
/*
check for an EKF yaw reset
*/
void QuadPlane : : check_yaw_reset ( void )
{
2019-01-13 00:24:51 -04:00
if ( ! initialised ) {
return ;
}
2017-04-18 09:40:06 -03:00
float yaw_angle_change_rad = 0.0f ;
uint32_t new_ekfYawReset_ms = ahrs . getLastYawResetAngle ( yaw_angle_change_rad ) ;
if ( new_ekfYawReset_ms ! = ekfYawReset_ms ) {
2018-01-26 23:45:46 -04:00
attitude_control - > inertial_frame_reset ( ) ;
2017-04-18 09:40:06 -03:00
ekfYawReset_ms = new_ekfYawReset_ms ;
2021-11-14 23:54:34 -04:00
AP : : logger ( ) . Write_Event ( LogEvent : : EKF_YAW_RESET ) ;
2017-04-18 09:40:06 -03:00
}
}
2021-05-14 06:16:37 -03:00
void QuadPlane : : set_climb_rate_cms ( float target_climb_rate_cms , bool force_descend )
{
2021-06-21 04:22:56 -03:00
pos_control - > input_vel_accel_z ( target_climb_rate_cms , 0 , force_descend ) ;
2021-05-14 06:16:37 -03:00
}
2015-12-26 04:27:13 -04:00
/*
hold hover with target climb rate
*/
2021-05-14 06:16:37 -03:00
void QuadPlane : : hold_hover ( float target_climb_rate_cms )
2015-12-26 03:45:42 -04:00
{
2016-02-04 00:03:39 -04:00
// motors use full range
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2016-02-04 00:03:39 -04:00
2021-05-19 11:08:14 -03:00
// set vertical speed and acceleration limits
2021-05-03 11:44:05 -03:00
pos_control - > set_max_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2015-12-26 03:45:42 -04:00
// call attitude controller
2017-07-10 01:00:45 -03:00
multicopter_attitude_rate_update ( get_desired_yaw_rate_cds ( ) ) ;
2015-12-26 03:45:42 -04:00
// call position controller
2021-05-14 06:16:37 -03:00
set_climb_rate_cms ( target_climb_rate_cms , false ) ;
2017-03-13 04:04:09 -03:00
run_z_controller ( ) ;
2015-12-26 04:27:13 -04:00
}
2019-04-06 15:38:40 -03:00
float QuadPlane : : get_pilot_throttle ( )
{
2019-03-24 00:35:13 -03:00
// get scaled throttle input
float throttle_in = plane . channel_throttle - > get_control_in ( ) ;
// normalize to [0,1]
throttle_in / = plane . channel_throttle - > get_range ( ) ;
2019-04-06 15:38:40 -03:00
2019-10-02 22:14:34 -03:00
if ( is_positive ( throttle_expo ) ) {
// get hover throttle level [0,1]
float thr_mid = motors - > get_throttle_hover ( ) ;
float thrust_curve_expo = constrain_float ( throttle_expo , 0.0f , 1.0f ) ;
2019-04-06 15:38:40 -03:00
2019-10-02 22:14:34 -03:00
// this puts mid stick at hover throttle
return throttle_curve ( thr_mid , thrust_curve_expo , throttle_in ) ; ;
} else {
return throttle_in ;
}
2019-04-06 15:38:40 -03:00
}
2020-11-29 17:43:05 -04:00
/*
get_pilot_desired_angle - transform pilot ' s roll or pitch input into a desired lean angle .
The angle_max_cd and angle_limit_cd are mode dependent
*/
void QuadPlane : : get_pilot_desired_lean_angles ( float & roll_out_cd , float & pitch_out_cd , float angle_max_cd , float angle_limit_cd ) const
{
// failsafe check
if ( plane . failsafe . rc_failsafe | | plane . failsafe . throttle_counter > 0 ) {
roll_out_cd = 0 ;
pitch_out_cd = 0 ;
return ;
}
// fetch roll and pitch inputs
roll_out_cd = plane . channel_roll - > get_control_in ( ) ;
pitch_out_cd = plane . channel_pitch - > get_control_in ( ) ;
// limit max lean angle, always allow for 10 degrees
angle_limit_cd = constrain_float ( angle_limit_cd , 1000.0f , angle_max_cd ) ;
// scale roll and pitch inputs to ANGLE_MAX parameter range
float scaler = angle_max_cd / 4500.0 ;
roll_out_cd * = scaler ;
pitch_out_cd * = scaler ;
// apply circular limit
float total_in = norm ( pitch_out_cd , roll_out_cd ) ;
if ( total_in > angle_limit_cd ) {
float ratio = angle_limit_cd / total_in ;
roll_out_cd * = ratio ;
pitch_out_cd * = ratio ;
}
// apply lateral tilt to euler roll conversion
roll_out_cd = 100 * degrees ( atanf ( cosf ( radians ( pitch_out_cd * 0.01 ) ) * tanf ( radians ( roll_out_cd * 0.01 ) ) ) ) ;
}
2021-05-14 07:28:58 -03:00
/*
get pilot throttle in for landing code . Return value on scale of 0 to 1
*/
float QuadPlane : : get_pilot_land_throttle ( void ) const
{
if ( plane . rc_failsafe_active ( ) ) {
// assume zero throttle if lost RC
return 0 ;
}
// get scaled throttle input
float throttle_in = plane . channel_throttle - > get_control_in ( ) ;
// normalize to [0,1]
throttle_in / = plane . channel_throttle - > get_range ( ) ;
return constrain_float ( throttle_in , 0 , 1 ) ;
}
2019-02-17 14:42:05 -04:00
/*
control QACRO mode
*/
void QuadPlane : : control_qacro ( void )
{
if ( throttle_wait ) {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : GROUND_IDLE ) ;
2018-12-28 02:35:08 -04:00
attitude_control - > set_throttle_out ( 0 , true , 0 ) ;
2020-09-02 23:45:52 -03:00
relax_attitude_control ( ) ;
2019-02-17 14:42:05 -04:00
} else {
check_attitude_relax ( ) ;
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2019-02-17 14:42:05 -04:00
// convert the input to the desired body frame rate
float target_roll = 0 ;
2019-03-24 00:35:13 -03:00
float target_pitch = plane . channel_pitch - > norm_input ( ) * acro_pitch_rate * 100.0f ;
2019-02-17 14:42:05 -04:00
float target_yaw = 0 ;
if ( is_tailsitter ( ) ) {
// Note that the 90 degree Y rotation for copter mode swaps body-frame roll and yaw
2019-04-25 16:39:27 -03:00
target_roll = plane . channel_rudder - > norm_input ( ) * acro_yaw_rate * 100.0f ;
target_yaw = - plane . channel_roll - > norm_input ( ) * acro_roll_rate * 100.0f ;
2019-02-17 14:42:05 -04:00
} else {
2019-03-24 00:35:13 -03:00
target_roll = plane . channel_roll - > norm_input ( ) * acro_roll_rate * 100.0f ;
target_yaw = plane . channel_rudder - > norm_input ( ) * acro_yaw_rate * 100.0 ;
2019-02-17 14:42:05 -04:00
}
2019-04-06 15:38:40 -03:00
float throttle_out = get_pilot_throttle ( ) ;
2019-02-17 14:42:05 -04:00
// run attitude controller
2019-03-06 17:03:33 -04:00
if ( plane . g . acro_locking ) {
attitude_control - > input_rate_bf_roll_pitch_yaw_3 ( target_roll , target_pitch , target_yaw ) ;
} else {
attitude_control - > input_rate_bf_roll_pitch_yaw_2 ( target_roll , target_pitch , target_yaw ) ;
}
2019-02-17 14:42:05 -04:00
// output pilot's throttle without angle boost
attitude_control - > set_throttle_out ( throttle_out , false , 10.0f ) ;
}
}
2015-12-26 04:27:13 -04:00
/*
control QHOVER mode
*/
void QuadPlane : : control_hover ( void )
{
2015-12-26 05:13:20 -04:00
if ( throttle_wait ) {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : GROUND_IDLE ) ;
2018-12-28 02:35:08 -04:00
attitude_control - > set_throttle_out ( 0 , true , 0 ) ;
2020-09-02 23:45:52 -03:00
relax_attitude_control ( ) ;
2021-05-03 11:44:05 -03:00
pos_control - > relax_z_controller ( 0 ) ;
2015-12-26 05:13:20 -04:00
} else {
hold_hover ( get_pilot_desired_climb_rate_cms ( ) ) ;
}
2015-12-26 03:45:42 -04:00
}
2015-12-26 04:51:05 -04:00
void QuadPlane : : init_loiter ( void )
{
2018-02-09 23:52:46 -04:00
// initialise loiter
2018-09-18 23:50:51 -03:00
loiter_nav - > clear_pilot_desired_acceleration ( ) ;
2018-06-09 04:41:02 -03:00
loiter_nav - > init_target ( ) ;
2015-12-26 04:51:05 -04:00
2021-05-19 11:08:14 -03:00
// set vertical speed and acceleration limits
2021-05-03 11:44:05 -03:00
pos_control - > set_max_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2021-07-08 01:16:41 -03:00
pos_control - > set_correction_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2015-12-26 04:51:05 -04:00
2015-12-26 05:13:20 -04:00
init_throttle_wait ( ) ;
2017-09-08 04:54:55 -03:00
// remember initial pitch
loiter_initial_pitch_cd = MAX ( plane . ahrs . pitch_sensor , 0 ) ;
2017-11-05 05:44:42 -04:00
// prevent re-init of target position
last_loiter_ms = AP_HAL : : millis ( ) ;
2015-12-26 04:51:05 -04:00
}
2019-02-22 02:04:35 -04:00
void QuadPlane : : init_qland ( void )
2016-03-09 03:20:41 -04:00
{
init_loiter ( ) ;
throttle_wait = false ;
2021-05-14 07:54:17 -03:00
setup_target_position ( ) ;
2021-05-18 02:15:46 -03:00
poscontrol . set_state ( QPOS_LAND_DESCEND ) ;
2021-05-14 22:12:16 -03:00
poscontrol . pilot_correction_done = false ;
2020-01-11 00:17:23 -04:00
last_land_final_agl = plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ;
2016-06-16 05:17:46 -03:00
landing_detect . lower_limit_start_ms = 0 ;
2019-02-01 23:49:02 -04:00
landing_detect . land_start_ms = 0 ;
2020-03-31 22:42:16 -03:00
# if LANDING_GEAR_ENABLED == ENABLED
plane . g2 . landing_gear . deploy_for_landing ( ) ;
# endif
2020-12-14 08:48:57 -04:00
# if AC_FENCE == ENABLED
2021-01-13 19:36:42 -04:00
plane . fence . auto_disable_fence_for_landing ( ) ;
2020-12-14 08:48:57 -04:00
# endif
2016-03-09 03:20:41 -04:00
}
2016-01-01 06:39:36 -04:00
// helper for is_flying()
bool QuadPlane : : is_flying ( void )
{
if ( ! available ( ) ) {
return false ;
}
2019-01-15 13:46:13 -04:00
if ( plane . control_mode = = & plane . mode_guided & & guided_takeoff ) {
2017-09-06 04:58:08 -03:00
return true ;
}
2016-07-24 17:08:36 -03:00
if ( motors - > get_throttle ( ) > 0.01f & & ! motors - > limit . throttle_lower ) {
2016-01-01 06:39:36 -04:00
return true ;
}
2018-04-25 04:21:11 -03:00
if ( in_tailsitter_vtol_transition ( ) ) {
return true ;
}
2016-01-01 06:39:36 -04:00
return false ;
}
2015-12-26 06:40:40 -04:00
// crude landing detector to prevent tipover
bool QuadPlane : : should_relax ( void )
{
2018-08-24 02:42:37 -03:00
const uint32_t tnow = millis ( ) ;
2016-06-09 10:20:50 -03:00
bool motor_at_lower_limit = motors - > limit . throttle_lower & & attitude_control - > is_throttle_mix_min ( ) ;
2016-07-24 17:08:36 -03:00
if ( motors - > get_throttle ( ) < 0.01f ) {
2016-01-01 07:09:11 -04:00
motor_at_lower_limit = true ;
}
2018-08-24 02:42:37 -03:00
2015-12-26 06:40:40 -04:00
if ( ! motor_at_lower_limit ) {
2016-06-16 05:17:46 -03:00
landing_detect . lower_limit_start_ms = 0 ;
2019-02-01 23:49:02 -04:00
landing_detect . land_start_ms = 0 ;
2018-08-24 02:42:37 -03:00
return false ;
} else if ( landing_detect . lower_limit_start_ms = = 0 ) {
landing_detect . lower_limit_start_ms = tnow ;
2015-12-26 06:40:40 -04:00
}
2018-08-24 02:42:37 -03:00
return ( tnow - landing_detect . lower_limit_start_ms ) > 1000 ;
2015-12-26 06:40:40 -04:00
}
2016-04-22 07:20:06 -03:00
// see if we are flying in vtol
2018-08-24 02:42:37 -03:00
bool QuadPlane : : is_flying_vtol ( void ) const
2016-04-22 07:20:06 -03:00
{
2016-07-26 18:43:16 -03:00
if ( ! available ( ) ) {
return false ;
}
2019-04-09 09:17:25 -03:00
if ( motors - > get_spool_state ( ) = = AP_Motors : : SpoolState : : SHUT_DOWN ) {
2019-02-19 05:59:19 -04:00
// assume that with no motor outputs we're not flying in VTOL mode
return false ;
}
2016-07-24 17:08:36 -03:00
if ( motors - > get_throttle ( ) > 0.01f ) {
// if we are demanding more than 1% throttle then don't consider aircraft landed
return true ;
}
2020-07-29 21:22:37 -03:00
if ( plane . control_mode - > is_vtol_man_throttle ( ) & & air_mode = = AirMode : : ON ) {
// in manual throttle modes with airmode on, don't consider aircraft landed
2019-02-17 14:42:05 -04:00
return true ;
}
2019-01-15 13:46:13 -04:00
if ( plane . control_mode = = & plane . mode_guided & & guided_takeoff ) {
2017-09-06 04:58:08 -03:00
return true ;
}
2020-07-26 11:35:51 -03:00
if ( plane . control_mode - > is_vtol_man_mode ( ) ) {
2016-07-24 17:08:36 -03:00
// in manual flight modes only consider aircraft landed when pilot demanded throttle is zero
2018-11-09 18:38:43 -04:00
return plane . get_throttle_input ( ) > 0 ;
2016-07-24 17:08:36 -03:00
}
2016-06-16 05:17:46 -03:00
if ( in_vtol_mode ( ) & & millis ( ) - landing_detect . lower_limit_start_ms > 5000 ) {
2016-07-24 17:08:36 -03:00
// use landing detector
2016-04-22 07:20:06 -03:00
return true ;
}
return false ;
}
2016-04-02 08:45:51 -03:00
/*
smooth out descent rate for landing to prevent a jerk as we get to
land_final_alt .
*/
2018-08-24 02:42:37 -03:00
float QuadPlane : : landing_descent_rate_cms ( float height_above_ground ) const
2016-04-02 08:45:51 -03:00
{
2021-05-18 02:15:46 -03:00
if ( poscontrol . get_state ( ) = = QPOS_LAND_FINAL ) {
2021-05-14 18:20:39 -03:00
// when in final use descent rate for final even if alt has climbed again
height_above_ground = MIN ( height_above_ground , land_final_alt ) ;
}
2021-05-14 07:28:58 -03:00
const float max_climb_speed = wp_nav - > get_default_speed_up ( ) ;
2019-01-24 01:03:12 -04:00
float ret = linear_interpolate ( land_speed_cms , wp_nav - > get_default_speed_down ( ) ,
2016-04-02 08:45:51 -03:00
height_above_ground ,
2017-09-08 03:43:32 -03:00
land_final_alt , land_final_alt + 6 ) ;
2021-05-14 07:28:58 -03:00
if ( ( options & OPTION_THR_LANDING_CONTROL ) ! = 0 ) {
// allow throttle control for landing speed
2021-05-19 11:08:14 -03:00
const float thr_in = get_pilot_land_throttle ( ) ;
2021-05-14 07:28:58 -03:00
const float dz = 0.1 ;
const float thresh1 = 0.5 + dz ;
const float thresh2 = 0.5 - dz ;
const float scaling = 1.0 / ( 0.5 - dz ) ;
if ( thr_in > thresh1 ) {
// start climbing
ret = - ( thr_in - thresh1 ) * scaling * max_climb_speed ;
} else if ( thr_in > thresh2 ) {
// hold height
ret = 0 ;
} else {
ret * = ( thresh2 - thr_in ) * scaling ;
}
}
2021-06-08 03:02:15 -03:00
if ( poscontrol . pilot_correction_active ) {
// stop descent when repositioning
ret = MIN ( 0 , ret ) ;
}
2016-04-02 08:45:51 -03:00
return ret ;
}
2015-12-26 04:51:05 -04:00
// run quadplane loiter controller
void QuadPlane : : control_loiter ( )
{
2015-12-26 05:13:20 -04:00
if ( throttle_wait ) {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : GROUND_IDLE ) ;
2018-12-28 02:35:08 -04:00
attitude_control - > set_throttle_out ( 0 , true , 0 ) ;
2020-09-02 23:45:52 -03:00
relax_attitude_control ( ) ;
2021-05-03 11:44:05 -03:00
pos_control - > relax_z_controller ( 0 ) ;
2018-09-18 23:50:51 -03:00
loiter_nav - > clear_pilot_desired_acceleration ( ) ;
2018-03-27 23:24:05 -03:00
loiter_nav - > init_target ( ) ;
2015-12-26 05:13:20 -04:00
return ;
}
2021-05-14 07:54:17 -03:00
if ( ! motors - > armed ( ) ) {
init_loiter ( ) ;
}
2015-12-26 06:40:40 -04:00
2018-09-13 21:02:18 -03:00
check_attitude_relax ( ) ;
2015-12-26 06:40:40 -04:00
if ( should_relax ( ) ) {
2018-03-27 23:24:05 -03:00
loiter_nav - > soften_for_landing ( ) ;
2015-12-26 06:40:40 -04:00
}
2016-01-01 03:18:53 -04:00
2018-08-24 02:42:37 -03:00
const uint32_t now = AP_HAL : : millis ( ) ;
if ( now - last_loiter_ms > 500 ) {
2018-09-18 23:50:51 -03:00
loiter_nav - > clear_pilot_desired_acceleration ( ) ;
2018-03-27 23:24:05 -03:00
loiter_nav - > init_target ( ) ;
2016-01-01 03:18:53 -04:00
}
2018-08-24 02:42:37 -03:00
last_loiter_ms = now ;
2016-02-04 00:03:39 -04:00
// motors use full range
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2016-02-04 00:03:39 -04:00
2021-05-19 11:08:14 -03:00
// set vertical speed and acceleration limits
2021-05-03 11:44:05 -03:00
pos_control - > set_max_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2015-12-26 04:51:05 -04:00
// process pilot's roll and pitch input
2020-11-29 17:43:05 -04:00
float target_roll_cd , target_pitch_cd ;
get_pilot_desired_lean_angles ( target_roll_cd , target_pitch_cd , loiter_nav - > get_angle_max_cd ( ) , attitude_control - > get_althold_lean_angle_max ( ) ) ;
2021-05-03 11:44:05 -03:00
loiter_nav - > set_pilot_desired_acceleration ( target_roll_cd , target_pitch_cd ) ;
2020-11-29 17:43:05 -04:00
2015-12-26 04:51:05 -04:00
// run loiter controller
2021-05-14 18:20:39 -03:00
if ( ! pos_control - > is_active_xy ( ) ) {
pos_control - > init_xy_controller ( ) ;
}
2018-09-05 03:48:15 -03:00
loiter_nav - > update ( ) ;
2015-12-26 04:51:05 -04:00
// nav roll and pitch are controller by loiter controller
2018-03-27 23:24:05 -03:00
plane . nav_roll_cd = loiter_nav - > get_roll ( ) ;
plane . nav_pitch_cd = loiter_nav - > get_pitch ( ) ;
2015-12-26 04:51:05 -04:00
2017-11-05 06:35:32 -04:00
if ( now - last_pidz_init_ms < ( uint32_t ) transition_time_ms * 2 & & ! is_tailsitter ( ) ) {
2017-09-08 04:54:55 -03:00
// we limit pitch during initial transition
float pitch_limit_cd = linear_interpolate ( loiter_initial_pitch_cd , aparm . angle_max ,
now ,
2017-11-05 06:35:32 -04:00
last_pidz_init_ms , last_pidz_init_ms + transition_time_ms * 2 ) ;
2017-09-08 04:54:55 -03:00
if ( plane . nav_pitch_cd > pitch_limit_cd ) {
plane . nav_pitch_cd = pitch_limit_cd ;
2021-05-19 11:08:14 -03:00
pos_control - > set_externally_limited_xy ( ) ;
2017-09-08 04:54:55 -03:00
}
}
// call attitude controller with conservative smoothing gain of 4.0f
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( plane . nav_roll_cd ,
plane . nav_pitch_cd ,
2017-07-10 01:00:45 -03:00
get_desired_yaw_rate_cds ( ) ) ;
2017-09-08 04:54:55 -03:00
2019-01-15 13:46:13 -04:00
if ( plane . control_mode = = & plane . mode_qland ) {
2021-05-18 02:15:46 -03:00
if ( poscontrol . get_state ( ) < QPOS_LAND_FINAL & & check_land_final ( ) ) {
poscontrol . set_state ( QPOS_LAND_FINAL ) ;
2021-05-14 07:54:17 -03:00
setup_target_position ( ) ;
2016-09-28 16:18:38 -03:00
// cut IC engine if enabled
if ( land_icengine_cut ! = 0 ) {
plane . g2 . ice_control . engine_control ( 0 , 0 , 0 ) ;
}
2016-03-09 03:20:41 -04:00
}
2019-02-01 23:49:02 -04:00
float height_above_ground = plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ;
2021-05-14 18:20:39 -03:00
float descent_rate_cms = landing_descent_rate_cms ( height_above_ground ) ;
2021-05-14 06:16:37 -03:00
2021-05-18 02:15:46 -03:00
if ( poscontrol . get_state ( ) = = QPOS_LAND_FINAL & & ( options & OPTION_DISABLE_GROUND_EFFECT_COMP ) = = 0 ) {
2021-05-28 20:54:15 -03:00
ahrs . set_touchdown_expected ( true ) ;
2021-05-14 18:20:39 -03:00
}
set_climb_rate_cms ( - descent_rate_cms , descent_rate_cms > 0 ) ;
2016-03-09 03:20:41 -04:00
check_land_complete ( ) ;
2019-01-15 13:46:13 -04:00
} else if ( plane . control_mode = = & plane . mode_guided & & guided_takeoff ) {
2021-05-14 06:16:37 -03:00
set_climb_rate_cms ( 0 , false ) ;
2016-03-09 03:20:41 -04:00
} else {
// update altitude target and call position controller
2021-05-14 06:16:37 -03:00
set_climb_rate_cms ( get_pilot_desired_climb_rate_cms ( ) , false ) ;
2016-03-09 03:20:41 -04:00
}
2017-03-13 04:04:09 -03:00
run_z_controller ( ) ;
2015-12-26 04:51:05 -04:00
}
/*
2016-01-09 18:42:46 -04:00
get pilot input yaw rate in cd / s
2015-12-26 04:51:05 -04:00
*/
2018-08-24 02:42:37 -03:00
float QuadPlane : : get_pilot_input_yaw_rate_cds ( void ) const
2016-01-09 18:42:46 -04:00
{
2020-07-29 21:22:37 -03:00
bool manual_air_mode = plane . control_mode - > is_vtol_man_throttle ( ) & & ( air_mode = = AirMode : : ON ) ;
if ( ! manual_air_mode & &
2020-12-23 01:25:35 -04:00
plane . get_throttle_input ( ) < = 0 & & ! plane . control_mode - > does_auto_throttle ( ) & &
2019-03-06 23:12:56 -04:00
plane . arming . get_rudder_arming_type ( ) ! = AP_Arming : : RudderArming : : IS_DISABLED ) {
2016-01-09 18:42:46 -04:00
// the user may be trying to disarm
return 0 ;
}
2020-05-07 16:43:43 -03:00
if ( ( plane . g . stick_mixing = = STICK_MIXING_DISABLED ) & &
( plane . control_mode = = & plane . mode_qrtl | |
2020-09-22 14:00:39 -03:00
plane . control_mode - > is_guided_mode ( ) | |
2020-05-07 16:43:43 -03:00
in_vtol_auto ( ) ) ) {
2020-03-31 00:49:40 -03:00
return 0 ;
}
2016-01-09 18:42:46 -04:00
// add in rudder input
2019-11-28 11:24:00 -04:00
float max_rate = yaw_rate_max ;
if ( is_tailsitter ( ) & &
2019-12-01 21:11:06 -04:00
tailsitter . input_type & TAILSITTER_INPUT_BF_ROLL ) {
2019-11-30 21:25:46 -04:00
// must have a non-zero max yaw rate for scaling to work
2019-11-28 11:24:00 -04:00
max_rate = ( yaw_rate_max < 1.0f ) ? 1 : yaw_rate_max ;
}
return plane . channel_rudder - > get_control_in ( ) * max_rate / 45 ;
2016-01-09 18:42:46 -04:00
}
/*
get overall desired yaw rate in cd / s
*/
float QuadPlane : : get_desired_yaw_rate_cds ( void )
2015-12-26 04:51:05 -04:00
{
2016-01-02 00:25:49 -04:00
float yaw_cds = 0 ;
2015-12-26 06:40:40 -04:00
if ( assisted_flight ) {
// use bank angle to get desired yaw rate
2016-01-09 18:42:46 -04:00
yaw_cds + = desired_auto_yaw_rate_cds ( ) ;
2015-12-26 06:40:40 -04:00
}
2020-07-29 21:22:37 -03:00
bool manual_air_mode = plane . control_mode - > is_vtol_man_throttle ( ) & & ( air_mode = = AirMode : : ON ) ;
2020-12-23 01:25:35 -04:00
if ( plane . get_throttle_input ( ) < = 0 & & ! plane . control_mode - > does_auto_throttle ( ) & & ! manual_air_mode ) {
2016-01-02 00:16:31 -04:00
// the user may be trying to disarm
return 0 ;
}
2016-01-09 18:42:46 -04:00
// add in pilot input
yaw_cds + = get_pilot_input_yaw_rate_cds ( ) ;
2016-04-20 03:23:17 -03:00
// add in weathervaning
yaw_cds + = get_weathervane_yaw_rate_cds ( ) ;
2016-01-02 00:25:49 -04:00
return yaw_cds ;
2015-12-26 04:51:05 -04:00
}
// get pilot desired climb rate in cm/s
2018-08-24 02:42:37 -03:00
float QuadPlane : : get_pilot_desired_climb_rate_cms ( void ) const
2015-12-26 04:51:05 -04:00
{
2017-10-27 17:21:28 -03:00
if ( plane . failsafe . rc_failsafe | | plane . failsafe . throttle_counter > 0 ) {
2015-12-26 06:40:40 -04:00
// descend at 0.5m/s for now
return - 50 ;
}
uint16_t dead_zone = plane . channel_throttle - > get_dead_zone ( ) ;
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
uint16_t trim = ( plane . channel_throttle - > get_radio_max ( ) + plane . channel_throttle - > get_radio_min ( ) ) / 2 ;
2021-03-16 18:54:03 -03:00
const float throttle_request = plane . channel_throttle - > pwm_to_angle_dz_trim ( dead_zone , trim ) * 0.01f ;
return throttle_request * ( throttle_request > 0.0f ? pilot_velocity_z_max_up : get_pilot_velocity_z_max_dn ( ) ) ;
2015-12-26 04:51:05 -04:00
}
2015-12-26 05:13:20 -04:00
/*
initialise throttle_wait based on throttle and is_flying ( )
*/
void QuadPlane : : init_throttle_wait ( void )
{
2018-11-09 18:38:43 -04:00
if ( plane . get_throttle_input ( ) > = 10 | |
2015-12-26 05:13:20 -04:00
plane . is_flying ( ) ) {
throttle_wait = false ;
} else {
throttle_wait = true ;
}
}
2015-11-24 04:24:04 -04:00
// set motor arming
void QuadPlane : : set_armed ( bool armed )
{
2015-12-26 06:40:40 -04:00
if ( ! initialised ) {
return ;
}
motors - > armed ( armed ) ;
}
/*
estimate desired climb rate for assistance ( in cm / s )
*/
2018-08-24 02:42:37 -03:00
float QuadPlane : : assist_climb_rate_cms ( void ) const
2015-12-26 06:40:40 -04:00
{
2016-01-20 02:21:54 -04:00
float climb_rate ;
2020-12-23 01:25:35 -04:00
if ( plane . control_mode - > does_auto_throttle ( ) ) {
2016-01-20 02:21:54 -04:00
// use altitude_error_cm, spread over 10s interval
2018-09-17 21:35:24 -03:00
climb_rate = plane . altitude_error_cm * 0.1f ;
2016-01-20 02:21:54 -04:00
} else {
// otherwise estimate from pilot input
climb_rate = plane . g . flybywire_climb_rate * ( plane . nav_pitch_cd / ( float ) plane . aparm . pitch_limit_max_cd ) ;
2018-11-09 18:38:43 -04:00
climb_rate * = plane . get_throttle_input ( ) ;
2015-11-24 04:24:04 -04:00
}
2019-01-24 01:03:12 -04:00
climb_rate = constrain_float ( climb_rate , - wp_nav - > get_default_speed_down ( ) , wp_nav - > get_default_speed_up ( ) ) ;
2017-04-08 01:39:55 -03:00
// bring in the demanded climb rate over 2 seconds
2018-09-17 21:35:24 -03:00
const uint32_t ramp_up_time_ms = 2000 ;
2018-01-24 20:38:07 -04:00
const uint32_t dt_since_start = last_pidz_active_ms - last_pidz_init_ms ;
2018-09-17 21:35:24 -03:00
if ( dt_since_start < ramp_up_time_ms ) {
climb_rate = linear_interpolate ( 0 , climb_rate , dt_since_start , 0 , ramp_up_time_ms ) ;
2017-04-08 01:39:55 -03:00
}
2015-12-26 06:40:40 -04:00
return climb_rate ;
2015-11-24 04:24:04 -04:00
}
2015-12-26 06:40:40 -04:00
/*
calculate desired yaw rate for assistance
*/
2018-08-24 02:42:37 -03:00
float QuadPlane : : desired_auto_yaw_rate_cds ( void ) const
2015-12-26 06:40:40 -04:00
{
float aspeed ;
2020-01-06 20:45:27 -04:00
if ( ! ahrs . airspeed_estimate ( aspeed ) | | aspeed < plane . aparm . airspeed_min ) {
2016-08-07 21:48:36 -03:00
aspeed = plane . aparm . airspeed_min ;
2015-12-26 06:40:40 -04:00
}
if ( aspeed < 1 ) {
aspeed = 1 ;
}
float yaw_rate = degrees ( GRAVITY_MSS * tanf ( radians ( plane . nav_roll_cd * 0.01f ) ) / aspeed ) * 100 ;
return yaw_rate ;
}
2015-11-24 04:24:04 -04:00
2016-08-29 03:44:54 -03:00
/*
return true if the quadplane should provide stability assistance
*/
2020-01-02 15:06:39 -04:00
bool QuadPlane : : assistance_needed ( float aspeed , bool have_airspeed )
2016-08-29 03:44:54 -03:00
{
2020-09-02 23:45:52 -03:00
if ( assist_speed < = 0 | | is_control_surface_tailsitter ( ) ) {
2016-08-29 03:44:54 -03:00
// assistance disabled
in_angle_assist = false ;
angle_error_start_ms = 0 ;
return false ;
}
2019-11-01 07:44:23 -03:00
2020-10-04 19:22:50 -03:00
// assistance due to Q_ASSIST_SPEED
// if option bit is enabled only allow assist with real airspeed sensor
if ( ( have_airspeed & & aspeed < assist_speed ) & &
( ( ( options & OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST ) = = 0 ) | | ahrs . airspeed_sensor_enabled ( ) ) ) {
2016-08-29 03:44:54 -03:00
in_angle_assist = false ;
angle_error_start_ms = 0 ;
return true ;
}
2019-11-01 07:44:23 -03:00
const uint32_t now = AP_HAL : : millis ( ) ;
/*
optional assistance when altitude is too close to the ground
*/
if ( assist_alt > 0 ) {
float height_above_ground = plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ;
if ( height_above_ground < assist_alt ) {
if ( alt_error_start_ms = = 0 ) {
alt_error_start_ms = now ;
}
2020-02-16 21:05:13 -04:00
if ( now - alt_error_start_ms > assist_delay * 1000 ) {
// we've been below assistant alt for Q_ASSIST_DELAY seconds
2019-11-01 07:44:23 -03:00
if ( ! in_alt_assist ) {
in_alt_assist = true ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Alt assist %.1fm " , height_above_ground ) ;
}
return true ;
}
} else {
in_alt_assist = false ;
alt_error_start_ms = 0 ;
}
}
2016-08-29 03:44:54 -03:00
if ( assist_angle < = 0 ) {
in_angle_assist = false ;
angle_error_start_ms = 0 ;
return false ;
}
/*
now check if we should provide assistance due to attitude error
*/
const uint16_t allowed_envelope_error_cd = 500U ;
if ( labs ( ahrs . roll_sensor ) < = plane . aparm . roll_limit_cd + allowed_envelope_error_cd & &
ahrs . pitch_sensor < plane . aparm . pitch_limit_max_cd + allowed_envelope_error_cd & &
2017-04-18 08:26:13 -03:00
ahrs . pitch_sensor > - ( allowed_envelope_error_cd - plane . aparm . pitch_limit_min_cd ) ) {
2016-08-29 03:44:54 -03:00
// we are inside allowed attitude envelope
in_angle_assist = false ;
angle_error_start_ms = 0 ;
return false ;
}
2018-07-28 00:43:26 -03:00
int32_t max_angle_cd = 100U * assist_angle ;
2016-08-29 03:44:54 -03:00
if ( ( labs ( ahrs . roll_sensor - plane . nav_roll_cd ) < max_angle_cd & &
labs ( ahrs . pitch_sensor - plane . nav_pitch_cd ) < max_angle_cd ) ) {
// not beyond angle error
angle_error_start_ms = 0 ;
in_angle_assist = false ;
return false ;
}
2019-11-01 07:44:23 -03:00
2016-08-29 03:44:54 -03:00
if ( angle_error_start_ms = = 0 ) {
2018-01-24 20:38:07 -04:00
angle_error_start_ms = now ;
2016-08-29 03:44:54 -03:00
}
2020-02-16 21:05:13 -04:00
bool ret = ( now - angle_error_start_ms ) > = assist_delay * 1000 ;
2016-08-29 03:44:54 -03:00
if ( ret & & ! in_angle_assist ) {
in_angle_assist = true ;
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Angle assist r=%d p=%d " ,
2016-08-29 03:44:54 -03:00
( int ) ( ahrs . roll_sensor / 100 ) ,
( int ) ( ahrs . pitch_sensor / 100 ) ) ;
}
return ret ;
}
2020-02-17 20:17:50 -04:00
// return true if it is safe to provide assistance
bool QuadPlane : : assistance_safe ( )
{
2020-12-23 01:25:35 -04:00
return hal . util - > get_soft_armed ( ) & & ( ( plane . control_mode - > does_auto_throttle ( ) & & ! plane . throttle_suppressed )
2020-02-17 20:17:50 -04:00
| | plane . get_throttle_input ( ) > 0
| | plane . is_flying ( ) ) ;
}
2015-11-24 04:24:04 -04:00
/*
2015-12-26 04:27:13 -04:00
update for transition from quadplane to fixed wing mode
2015-11-24 04:24:04 -04:00
*/
void QuadPlane : : update_transition ( void )
{
2019-01-15 13:46:13 -04:00
if ( plane . control_mode = = & plane . mode_manual | |
plane . control_mode = = & plane . mode_acro | |
plane . control_mode = = & plane . mode_training ) {
2016-01-03 01:46:34 -04:00
// in manual modes quad motors are always off
2017-02-11 01:50:03 -04:00
if ( ! tilt . motors_active & & ! is_tailsitter ( ) ) {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : SHUT_DOWN ) ;
2016-05-01 20:33:54 -03:00
motors - > output ( ) ;
}
2015-12-26 04:27:13 -04:00
transition_state = TRANSITION_DONE ;
2018-12-14 22:40:43 -04:00
transition_start_ms = 0 ;
transition_low_airspeed_ms = 0 ;
2016-10-11 03:04:22 -03:00
assisted_flight = false ;
2015-12-26 04:27:13 -04:00
return ;
}
2018-12-14 22:40:43 -04:00
const uint32_t now = millis ( ) ;
if ( ! hal . util - > get_soft_armed ( ) ) {
// reset the failure timer if we haven't started transitioning
transition_start_ms = now ;
} else if ( ( transition_state ! = TRANSITION_DONE ) & &
( transition_start_ms ! = 0 ) & &
( transition_failure > 0 ) & &
( ( now - transition_start_ms ) > ( ( uint32_t ) transition_failure * 1000 ) ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " Transition failed, exceeded time limit " ) ;
2019-10-17 00:49:32 -03:00
plane . set_mode ( plane . mode_qland , ModeReason : : VTOL_FAILED_TRANSITION ) ;
2018-12-14 22:40:43 -04:00
}
2015-12-26 06:40:40 -04:00
float aspeed ;
2020-01-06 20:45:27 -04:00
bool have_airspeed = ahrs . airspeed_estimate ( aspeed ) ;
2017-11-19 01:05:01 -04:00
// tailsitters use angle wait, not airspeed wait
if ( is_tailsitter ( ) & & transition_state = = TRANSITION_AIRSPEED_WAIT ) {
transition_state = TRANSITION_ANGLE_WAIT_FW ;
}
2020-01-02 15:06:39 -04:00
2015-12-26 06:40:40 -04:00
/*
see if we should provide some assistance
*/
2020-02-17 20:17:50 -04:00
if ( assistance_safe ( ) & & ( q_assist_state = = Q_ASSIST_STATE_ENUM : : Q_ASSIST_FORCE | |
( q_assist_state = = Q_ASSIST_STATE_ENUM : : Q_ASSIST_ENABLED & & assistance_needed ( aspeed , have_airspeed ) ) ) ) {
2015-12-26 06:40:40 -04:00
// the quad should provide some assistance to the plane
assisted_flight = true ;
2020-01-02 15:06:39 -04:00
if ( ! is_tailsitter ( ) ) {
2021-05-12 01:32:13 -03:00
// update transition state for vehicles using airspeed wait
2020-01-02 15:06:39 -04:00
if ( transition_state ! = TRANSITION_AIRSPEED_WAIT ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Transition started airspeed %.1f " , ( double ) aspeed ) ;
}
transition_state = TRANSITION_AIRSPEED_WAIT ;
if ( transition_start_ms = = 0 ) {
transition_start_ms = now ;
}
}
2015-12-26 06:40:40 -04:00
} else {
assisted_flight = false ;
}
2016-01-09 06:50:59 -04:00
2017-02-11 04:12:56 -04:00
if ( is_tailsitter ( ) ) {
2017-10-30 01:19:38 -03:00
if ( transition_state = = TRANSITION_ANGLE_WAIT_FW & &
tailsitter_transition_fw_complete ( ) ) {
2017-02-11 04:12:56 -04:00
transition_state = TRANSITION_DONE ;
2018-12-14 22:40:43 -04:00
transition_start_ms = 0 ;
transition_low_airspeed_ms = 0 ;
2017-02-11 04:12:56 -04:00
}
}
2018-09-13 07:57:10 -03:00
// if rotors are fully forward then we are not transitioning,
// unless we are waiting for airspeed to increase (in which case
// the tilt will decrease rapidly)
if ( tiltrotor_fully_fwd ( ) & & transition_state ! = TRANSITION_AIRSPEED_WAIT ) {
2020-12-15 00:03:42 -04:00
if ( transition_state = = TRANSITION_TIMER ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Transition FW done " ) ;
}
2017-01-22 22:06:31 -04:00
transition_state = TRANSITION_DONE ;
2018-12-14 22:40:43 -04:00
transition_start_ms = 0 ;
transition_low_airspeed_ms = 0 ;
2017-01-22 22:06:31 -04:00
}
2016-01-09 06:50:59 -04:00
if ( transition_state < TRANSITION_TIMER ) {
// set a single loop pitch limit in TECS
2016-05-27 02:48:14 -03:00
if ( plane . ahrs . groundspeed ( ) < 3 ) {
// until we have some ground speed limit to zero pitch
plane . TECS_controller . set_pitch_max_limit ( 0 ) ;
} else {
plane . TECS_controller . set_pitch_max_limit ( transition_pitch_max ) ;
}
2016-01-20 03:30:48 -04:00
} else if ( transition_state < TRANSITION_DONE ) {
plane . TECS_controller . set_pitch_max_limit ( ( transition_pitch_max + 1 ) * 2 ) ;
2016-01-09 06:50:59 -04:00
}
2016-10-04 21:52:52 -03:00
if ( transition_state < TRANSITION_DONE ) {
// during transition we ask TECS to use a synthetic
// airspeed. Otherwise the pitch limits will throw off the
// throttle calculation which is driven by pitch
plane . TECS_controller . use_synthetic_airspeed ( ) ;
}
2015-12-26 06:40:40 -04:00
2015-12-26 04:27:13 -04:00
switch ( transition_state ) {
case TRANSITION_AIRSPEED_WAIT : {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2015-12-26 04:27:13 -04:00
// we hold in hover until the required airspeed is reached
2015-12-26 05:13:20 -04:00
if ( transition_start_ms = = 0 ) {
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Transition airspeed wait " ) ;
2018-12-14 22:40:43 -04:00
transition_start_ms = now ;
2015-12-26 05:13:20 -04:00
}
2018-12-14 22:40:43 -04:00
transition_low_airspeed_ms = now ;
2016-08-07 21:48:36 -03:00
if ( have_airspeed & & aspeed > plane . aparm . airspeed_min & & ! assisted_flight ) {
2015-12-26 04:27:13 -04:00
transition_state = TRANSITION_TIMER ;
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Transition airspeed reached %.1f " , ( double ) aspeed ) ;
2015-12-26 04:27:13 -04:00
}
2016-01-01 05:42:10 -04:00
assisted_flight = true ;
2018-09-18 00:40:12 -03:00
2021-02-27 19:31:11 -04:00
// do not allow a climb on the quad motors during transition a
// climb would add load to the airframe, and prolongs the
// transition. We don't limit the climb rate on tilt rotors as
// otherwise the plane can end up in high-alpha flight with
// low VTOL thrust and may not complete a transition
2018-09-18 00:40:12 -03:00
float climb_rate_cms = assist_climb_rate_cms ( ) ;
2021-02-27 19:31:11 -04:00
if ( ( options & OPTION_LEVEL_TRANSITION ) & & tilt . tilt_mask = = 0 ) {
2018-09-18 00:40:12 -03:00
climb_rate_cms = MIN ( climb_rate_cms , 0.0f ) ;
}
hold_hover ( climb_rate_cms ) ;
2018-01-05 08:58:40 -04:00
2020-12-15 00:03:42 -04:00
if ( ! tilt . is_vectored ) {
2020-12-03 02:42:39 -04:00
// set desired yaw to current yaw in both desired angle
// and rate request. This reduces wing twist in transition
// due to multicopter yaw demands. This is disabled when
// using vectored yaw for tilt-rotors as the yaw control
// is needed to maintain good control in forward
// transitions
2021-05-24 10:42:30 -03:00
attitude_control - > reset_yaw_target_and_rate ( ) ;
2020-12-03 02:42:39 -04:00
attitude_control - > rate_bf_yaw_target ( ahrs . get_gyro ( ) . z ) ;
}
2018-01-05 08:58:40 -04:00
2015-12-26 06:40:40 -04:00
last_throttle = motors - > get_throttle ( ) ;
2017-01-28 19:40:56 -04:00
// reset integrators while we are below target airspeed as we
// may build up too much while still primarily under
// multicopter control
plane . pitchController . reset_I ( ) ;
plane . rollController . reset_I ( ) ;
// give full authority to attitude control
2019-06-17 12:58:17 -03:00
attitude_control - > set_throttle_mix_max ( 1.0f ) ;
2015-12-26 04:27:13 -04:00
break ;
}
case TRANSITION_TIMER : {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2015-12-26 04:27:13 -04:00
// after airspeed is reached we degrade throttle over the
// transition time, but continue to stabilize
2018-12-14 22:40:43 -04:00
const uint32_t transition_timer_ms = now - transition_low_airspeed_ms ;
if ( transition_timer_ms > ( unsigned ) transition_time_ms ) {
2015-12-26 04:27:13 -04:00
transition_state = TRANSITION_DONE ;
2018-12-14 22:40:43 -04:00
transition_start_ms = 0 ;
transition_low_airspeed_ms = 0 ;
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Transition done " ) ;
2015-12-26 04:27:13 -04:00
}
2016-06-10 04:35:43 -03:00
float trans_time_ms = ( float ) transition_time_ms . get ( ) ;
2018-12-14 22:40:43 -04:00
float transition_scale = ( trans_time_ms - transition_timer_ms ) / trans_time_ms ;
2017-01-28 19:40:56 -04:00
float throttle_scaled = last_throttle * transition_scale ;
// set zero throttle mix, to give full authority to
// throttle. This ensures that the fixed wing controllers get
// a chance to learn the right integrators during the transition
attitude_control - > set_throttle_mix_value ( 0.5 * transition_scale ) ;
2017-01-24 02:59:28 -04:00
if ( throttle_scaled < 0.01 ) {
// ensure we don't drop all the way to zero or the motors
// will stop stabilizing
throttle_scaled = 0.01 ;
2015-12-26 04:27:13 -04:00
}
2016-01-01 00:36:03 -04:00
assisted_flight = true ;
2015-12-26 04:27:13 -04:00
hold_stabilize ( throttle_scaled ) ;
2018-01-05 08:58:40 -04:00
// set desired yaw to current yaw in both desired angle and
// rate request while waiting for transition to
// complete. Navigation should be controlled by fixed wing
2020-12-14 17:47:46 -04:00
// control surfaces at this stage.
// We disable this for vectored yaw tilt rotors as they do need active
// yaw control throughout the transition
if ( ! tilt . is_vectored ) {
2021-05-24 10:42:30 -03:00
attitude_control - > reset_yaw_target_and_rate ( ) ;
2020-12-14 17:47:46 -04:00
attitude_control - > rate_bf_yaw_target ( ahrs . get_gyro ( ) . z ) ;
}
2015-12-26 04:27:13 -04:00
break ;
}
2017-10-30 01:19:38 -03:00
case TRANSITION_ANGLE_WAIT_FW : {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2017-02-11 04:12:56 -04:00
assisted_flight = true ;
2019-01-21 18:38:07 -04:00
uint32_t dt = now - transition_start_ms ;
2021-03-25 17:32:31 -03:00
// multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
2021-04-07 12:22:19 -03:00
plane . nav_pitch_cd = constrain_float ( transition_initial_pitch - ( tailsitter . transition_rate_fw * dt ) * 0.1f * ( plane . fly_inverted ( ) ? - 1.0f : 1.0f ) , - 8500 , 8500 ) ;
2017-10-30 01:19:38 -03:00
plane . nav_roll_cd = 0 ;
2018-09-13 21:02:18 -03:00
check_attitude_relax ( ) ;
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( plane . nav_roll_cd ,
2017-10-30 01:19:38 -03:00
plane . nav_pitch_cd ,
2017-07-10 01:00:45 -03:00
0 ) ;
2020-04-15 20:39:05 -03:00
// set throttle at either hover throttle or current throttle, whichever is higher, through the transition
2020-08-28 14:32:38 -03:00
attitude_control - > set_throttle_out ( MAX ( motors - > get_throttle_hover ( ) , attitude_control - > get_throttle_in ( ) ) , true , 0 ) ;
2017-02-11 04:12:56 -04:00
break ;
}
2017-10-30 01:19:38 -03:00
case TRANSITION_ANGLE_WAIT_VTOL :
2021-05-12 01:32:13 -03:00
// nothing to do, this is handled in the fixed wing attitude controller
2018-09-17 21:35:24 -03:00
return ;
2017-10-30 01:19:38 -03:00
2015-12-26 04:27:13 -04:00
case TRANSITION_DONE :
2017-02-11 01:50:03 -04:00
if ( ! tilt . motors_active & & ! is_tailsitter ( ) ) {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : SHUT_DOWN ) ;
2016-05-01 20:33:54 -03:00
motors - > output ( ) ;
}
2018-09-17 21:35:24 -03:00
return ;
2015-11-24 04:24:04 -04:00
}
2018-09-17 21:35:24 -03:00
motors_output ( ) ;
2016-09-20 22:46:15 -03:00
}
2015-11-24 04:24:04 -04:00
/*
update motor output for quadplane
*/
void QuadPlane : : update ( void )
{
2016-01-06 03:17:08 -04:00
if ( ! setup ( ) ) {
2015-12-26 06:40:40 -04:00
return ;
}
2019-02-04 13:14:01 -04:00
if ( ( ahrs_view ! = NULL ) & & ! is_equal ( _last_ahrs_trim_pitch , ahrs_trim_pitch . get ( ) ) ) {
_last_ahrs_trim_pitch = ahrs_trim_pitch . get ( ) ;
ahrs_view - > set_pitch_trim ( _last_ahrs_trim_pitch ) ;
}
2019-01-08 21:05:40 -04:00
# if ADVANCED_FAILSAFE == ENABLED
2019-08-21 03:01:17 -03:00
if ( plane . afs . should_crash_vehicle ( ) & & ! plane . afs . terminating_vehicle_via_landing ( ) ) {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : SHUT_DOWN ) ;
2016-07-22 05:36:28 -03:00
motors - > output ( ) ;
return ;
}
2019-01-08 21:05:40 -04:00
# endif
2016-07-22 05:36:28 -03:00
2016-03-12 19:05:10 -04:00
if ( motor_test . running ) {
motor_test_output ( ) ;
return ;
}
2017-01-28 19:40:56 -04:00
2019-02-25 20:25:11 -04:00
if ( SRV_Channels : : get_emergency_stop ( ) ) {
attitude_control - > reset_rate_controller_I_terms ( ) ;
}
2017-10-29 03:08:43 -03:00
if ( ! hal . util - > get_soft_armed ( ) ) {
/*
make sure we don ' t have any residual control from previous flight stages
*/
2018-08-16 20:59:41 -03:00
if ( is_tailsitter ( ) ) {
// tailsitters only relax I terms, to make ground testing easier
attitude_control - > reset_rate_controller_I_terms ( ) ;
} else {
// otherwise full relax
attitude_control - > relax_attitude_controllers ( ) ;
}
2021-05-03 11:44:05 -03:00
// todo: do you want to set the throttle at this point?
pos_control - > relax_z_controller ( 0 ) ;
2017-10-29 03:08:43 -03:00
}
2021-01-16 20:07:15 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
2016-03-09 03:20:41 -04:00
if ( ! in_vtol_mode ( ) ) {
2021-06-04 05:56:59 -03:00
// we're in a fixed wing mode, cope with transitions and check
// for assistance needed
2015-11-24 04:24:04 -04:00
update_transition ( ) ;
} else {
2017-10-30 01:19:38 -03:00
2015-12-26 06:40:40 -04:00
assisted_flight = false ;
2020-02-17 20:17:50 -04:00
2015-12-26 05:13:20 -04:00
// output to motors
2016-03-24 22:11:56 -03:00
motors_output ( ) ;
2017-10-30 01:19:38 -03:00
if ( now - last_vtol_mode_ms > 1000 & & is_tailsitter ( ) ) {
/*
we are just entering a VTOL mode as a tailsitter , set
our transition state so the fixed wing controller brings
the nose up before we start trying to fly as a
multicopter
*/
transition_state = TRANSITION_ANGLE_WAIT_VTOL ;
transition_start_ms = now ;
2021-03-25 17:32:31 -03:00
transition_initial_pitch = constrain_float ( ahrs . pitch_sensor , - 8500 , 8500 ) ;
2017-10-30 01:19:38 -03:00
} else if ( is_tailsitter ( ) & &
transition_state = = TRANSITION_ANGLE_WAIT_VTOL ) {
2020-02-17 20:17:50 -04:00
float aspeed ;
bool have_airspeed = ahrs . airspeed_estimate ( aspeed ) ;
// provide asistance in forward flight portion of tailsitter transision
if ( assistance_safe ( ) & & ( q_assist_state = = Q_ASSIST_STATE_ENUM : : Q_ASSIST_FORCE | |
( q_assist_state = = Q_ASSIST_STATE_ENUM : : Q_ASSIST_ENABLED & & assistance_needed ( aspeed , have_airspeed ) ) ) ) {
assisted_flight = true ;
}
2017-10-30 01:19:38 -03:00
if ( tailsitter_transition_vtol_complete ( ) ) {
/*
we have completed transition to VTOL as a tailsitter ,
setup for the back transition when needed
*/
transition_state = TRANSITION_ANGLE_WAIT_FW ;
transition_start_ms = now ;
}
2016-01-01 03:18:53 -04:00
} else {
2017-10-30 01:19:38 -03:00
/*
setup the transition state appropriately for next time we go into a non - VTOL mode
*/
transition_start_ms = 0 ;
2018-12-14 22:40:43 -04:00
transition_low_airspeed_ms = 0 ;
2017-10-30 01:19:38 -03:00
if ( throttle_wait & & ! plane . is_flying ( ) ) {
transition_state = TRANSITION_DONE ;
} else if ( is_tailsitter ( ) ) {
/*
setup for the transition back to fixed wing for later
*/
transition_state = TRANSITION_ANGLE_WAIT_FW ;
transition_start_ms = now ;
2021-01-10 13:11:00 -04:00
transition_initial_pitch = constrain_float ( ahrs_view - > pitch_sensor , - 8500 , 8500 ) ;
2017-10-30 01:19:38 -03:00
} else {
/*
setup for airspeed wait for later
*/
transition_state = TRANSITION_AIRSPEED_WAIT ;
}
last_throttle = motors - > get_throttle ( ) ;
2016-01-01 03:18:53 -04:00
}
2017-10-30 01:19:38 -03:00
last_vtol_mode_ms = now ;
2015-12-26 05:13:20 -04:00
}
// disable throttle_wait when throttle rises above 10%
2015-12-26 06:40:40 -04:00
if ( throttle_wait & &
2018-11-09 18:38:43 -04:00
( plane . get_throttle_input ( ) > 10 | |
2017-10-27 17:21:28 -03:00
plane . failsafe . rc_failsafe | |
plane . failsafe . throttle_counter > 0 ) ) {
2015-12-26 05:13:20 -04:00
throttle_wait = false ;
2015-11-24 04:24:04 -04:00
}
2016-05-01 05:07:52 -03:00
tiltrotor_update ( ) ;
2021-01-16 20:07:15 -04:00
// motors logging
if ( motors - > armed ( ) ) {
const bool motors_active = in_vtol_mode ( ) | | assisted_flight ;
if ( motors_active & & ( motors - > get_spool_state ( ) ! = AP_Motors : : SpoolState : : SHUT_DOWN ) ) {
// log RATE at main loop rate
ahrs_view - > Write_Rate ( * motors , * attitude_control , * pos_control ) ;
// log CTRL at 10 Hz
if ( now - last_ctrl_log_ms > 100 ) {
last_ctrl_log_ms = now ;
attitude_control - > control_monitor_log ( ) ;
}
}
// log QTUN at 25 Hz if motors are active, or have been active in the last quarter second
if ( ( motors_active | | ( now - last_motors_active_ms < 250 ) ) & & ( now - last_qtun_log_ms > 40 ) ) {
last_qtun_log_ms = now ;
Log_Write_QControl_Tuning ( ) ;
}
}
2015-11-24 04:24:04 -04:00
}
2015-12-26 06:40:40 -04:00
2016-08-01 19:21:44 -03:00
/*
see if motors should be shutdown . If they should be then change AP_Motors state to
2019-04-09 09:17:25 -03:00
AP_Motors : : DesiredSpoolState : : SHUT_DOWN
2016-08-01 19:21:44 -03:00
This is a safety check to prevent accidental motor runs on the
ground , such as if RC fails and QRTL is started
*/
2019-02-20 20:01:32 -04:00
void QuadPlane : : update_throttle_suppression ( void )
2016-08-01 19:21:44 -03:00
{
// if the motors have been running in the last 2 seconds then
// allow them to run now
if ( AP_HAL : : millis ( ) - last_motors_active_ms < 2000 ) {
return ;
}
// see if motors are already disabled
2019-04-09 09:17:25 -03:00
if ( motors - > get_desired_spool_state ( ) < AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) {
2016-08-01 19:21:44 -03:00
return ;
}
// if the users throttle is above zero then allow motors to run
2018-11-09 18:38:43 -04:00
if ( plane . get_throttle_input ( ) ! = 0 ) {
2016-08-01 19:21:44 -03:00
return ;
}
2020-07-07 21:22:56 -03:00
// if in a VTOL manual throttle mode and air_mode is on then allow motors to run
if ( plane . control_mode - > is_vtol_man_throttle ( ) & & air_mode = = AirMode : : ON ) {
return ;
}
2016-08-01 19:21:44 -03:00
// if we are in a fixed wing auto throttle mode and we have
// unsuppressed the throttle then allow motors to run
2020-12-23 01:25:35 -04:00
if ( plane . control_mode - > does_auto_throttle ( ) & & ! plane . throttle_suppressed ) {
2016-08-01 19:21:44 -03:00
return ;
}
// if our vertical velocity is greater than 1m/s then allow motors to run
if ( fabsf ( inertial_nav . get_velocity_z ( ) ) > 100 ) {
return ;
}
// if we are more than 5m from home altitude then allow motors to run
if ( plane . relative_ground_altitude ( plane . g . rangefinder_landing ) > 5 ) {
return ;
}
// allow for takeoff
2019-01-15 13:46:13 -04:00
if ( plane . control_mode = = & plane . mode_auto & & is_vtol_takeoff ( plane . mission . get_current_nav_cmd ( ) . id ) ) {
2016-08-01 19:21:44 -03:00
return ;
}
// motors should be in the spin when armed state to warn user they could become active
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : GROUND_IDLE ) ;
2016-08-01 19:21:44 -03:00
motors - > set_throttle ( 0 ) ;
last_motors_active_ms = 0 ;
}
2018-04-10 19:07:24 -03:00
// update estimated throttle required to hover (if necessary)
// called at 100hz
void QuadPlane : : update_throttle_hover ( )
{
2019-02-20 20:01:32 -04:00
if ( ! available ( ) ) {
2018-04-10 19:07:24 -03:00
return ;
}
// if not armed or landed exit
if ( ! motors - > armed ( ) | | ! is_flying_vtol ( ) ) {
return ;
}
// do not update while climbing or descending
2021-05-03 11:44:05 -03:00
if ( ! is_zero ( pos_control - > get_vel_desired_cms ( ) . z ) ) {
2018-04-10 19:07:24 -03:00
return ;
}
2019-04-06 15:38:40 -03:00
// do not update if quadplane forward motor is running (wing may be generating lift)
2020-09-02 08:56:08 -03:00
// we use the THR_MIN value to account for petrol motors idling at THR_MIN
if ( ! is_tailsitter ( ) & & ( SRV_Channels : : get_output_scaled ( SRV_Channel : : k_throttle ) > MAX ( 0 , plane . aparm . throttle_min + 10 ) ) ) {
2019-04-06 15:38:40 -03:00
return ;
}
2019-10-03 21:28:08 -03:00
// don't update if Z controller not running
const uint32_t now = AP_HAL : : millis ( ) ;
if ( now - last_pidz_active_ms > 20 ) {
return ;
}
2018-04-10 19:07:24 -03:00
// get throttle output
float throttle = motors - > get_throttle ( ) ;
2019-10-03 21:28:08 -03:00
float aspeed ;
// calc average throttle if we are in a level hover and low airspeed
2018-04-10 19:07:24 -03:00
if ( throttle > 0.0f & & fabsf ( inertial_nav . get_velocity_z ( ) ) < 60 & &
2019-10-03 21:28:08 -03:00
labs ( ahrs_view - > roll_sensor ) < 500 & & labs ( ahrs_view - > pitch_sensor ) < 500 & &
2020-01-06 20:45:27 -04:00
ahrs . airspeed_estimate ( aspeed ) & & aspeed < plane . aparm . airspeed_min * 0.3 ) {
2018-04-10 19:07:24 -03:00
// Can we set the time constant automatically
motors - > update_throttle_hover ( 0.01f ) ;
}
}
2016-03-24 22:11:56 -03:00
/*
output motors and do any copter needed
*/
2018-09-17 21:35:24 -03:00
void QuadPlane : : motors_output ( bool run_rate_controller )
2016-03-24 22:11:56 -03:00
{
2018-09-17 21:35:24 -03:00
if ( run_rate_controller ) {
attitude_control - > rate_controller_run ( ) ;
}
2020-06-11 12:15:56 -03:00
/* Delay for ARMING_DELAY_MS after arming before allowing props to spin:
1 ) for safety ( OPTION_DELAY_ARMING )
2 ) to allow motors to return to vertical ( OPTION_DISARMED_TILT )
*/
if ( ( options & OPTION_DISARMED_TILT ) | | ( options & OPTION_DELAY_ARMING ) ) {
2020-06-16 15:52:26 -03:00
if ( plane . arming . get_delay_arming ( ) ) {
// delay motor start after arming
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : SHUT_DOWN ) ;
2020-06-16 15:52:26 -03:00
motors - > output ( ) ;
return ;
2020-06-11 12:15:56 -03:00
}
}
2019-01-08 21:05:40 -04:00
# if ADVANCED_FAILSAFE == ENABLED
2019-08-21 03:01:17 -03:00
if ( ! hal . util - > get_soft_armed ( ) | |
( plane . afs . should_crash_vehicle ( ) & & ! plane . afs . terminating_vehicle_via_landing ( ) ) | |
SRV_Channels : : get_emergency_stop ( ) ) {
2019-01-08 21:05:40 -04:00
# else
if ( ! hal . util - > get_soft_armed ( ) | | SRV_Channels : : get_emergency_stop ( ) ) {
# endif
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : SHUT_DOWN ) ;
2016-07-22 05:36:28 -03:00
motors - > output ( ) ;
return ;
}
2019-01-15 13:46:13 -04:00
if ( esc_calibration & & AP_Notify : : flags . esc_calibration & & plane . control_mode = = & plane . mode_qstabilize ) {
2016-06-02 00:10:39 -03:00
// output is direct from run_esc_calibration()
return ;
}
2016-08-01 19:21:44 -03:00
2020-02-17 20:17:50 -04:00
if ( in_tailsitter_vtol_transition ( ) & & ! assisted_flight ) {
2017-10-30 01:19:38 -03:00
/*
don ' t run the motor outputs while in tailsitter - > vtol
transition . That is taken care of by the fixed wing
stabilisation code
*/
return ;
}
2016-08-01 19:21:44 -03:00
// see if motors should be shut down
2019-02-20 20:01:32 -04:00
update_throttle_suppression ( ) ;
2016-08-01 19:21:44 -03:00
2016-03-24 22:11:56 -03:00
motors - > output ( ) ;
2016-08-01 19:21:44 -03:00
// remember when motors were last active for throttle suppression
if ( motors - > get_throttle ( ) > 0.01f | | tilt . motors_active ) {
last_motors_active_ms = AP_HAL : : millis ( ) ;
}
2021-01-16 20:07:15 -04:00
2016-03-24 22:11:56 -03:00
}
2015-12-26 06:40:40 -04:00
/*
update control mode for quadplane modes
*/
void QuadPlane : : control_run ( void )
{
if ( ! initialised ) {
return ;
}
2019-01-15 13:46:13 -04:00
switch ( plane . control_mode - > mode_number ( ) ) {
case Mode : : Number : : QACRO :
2019-02-17 14:42:05 -04:00
control_qacro ( ) ;
2019-05-03 20:37:10 -03:00
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QSTABILIZE :
2015-12-26 06:40:40 -04:00
control_stabilize ( ) ;
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QHOVER :
2015-12-26 06:40:40 -04:00
control_hover ( ) ;
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QLOITER :
case Mode : : Number : : QLAND :
2015-12-26 06:40:40 -04:00
control_loiter ( ) ;
2016-04-29 02:31:08 -03:00
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QRTL :
2016-04-29 02:31:08 -03:00
control_qrtl ( ) ;
break ;
2018-12-07 03:52:05 -04:00
# if QAUTOTUNE_ENABLED
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QAUTOTUNE :
2018-12-07 03:52:05 -04:00
qautotune . run ( ) ;
break ;
# endif
2015-12-26 06:40:40 -04:00
default :
break ;
}
2019-05-04 11:52:53 -03:00
2015-12-26 06:40:40 -04:00
// we also stabilize using fixed wing surfaces
float speed_scaler = plane . get_speed_scaler ( ) ;
2019-05-04 11:52:53 -03:00
if ( plane . control_mode - > mode_number ( ) = = Mode : : Number : : QACRO ) {
plane . stabilize_acro ( speed_scaler ) ;
} else {
plane . stabilize_roll ( speed_scaler ) ;
plane . stabilize_pitch ( speed_scaler ) ;
}
2015-12-26 06:40:40 -04:00
}
/*
enter a quadplane mode
*/
bool QuadPlane : : init_mode ( void )
{
2016-01-06 03:17:08 -04:00
if ( ! setup ( ) ) {
return false ;
}
2015-12-26 06:40:40 -04:00
if ( ! initialised ) {
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " QuadPlane mode refused " ) ;
2015-12-26 06:40:40 -04:00
return false ;
}
2016-06-02 00:10:39 -03:00
AP_Notify : : flags . esc_calibration = false ;
2019-01-15 13:46:13 -04:00
switch ( plane . control_mode - > mode_number ( ) ) {
case Mode : : Number : : QSTABILIZE :
2015-12-26 06:40:40 -04:00
init_stabilize ( ) ;
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QHOVER :
2015-12-26 06:40:40 -04:00
init_hover ( ) ;
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QLOITER :
2015-12-26 06:40:40 -04:00
init_loiter ( ) ;
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QLAND :
2019-02-22 02:04:35 -04:00
init_qland ( ) ;
2016-03-09 03:20:41 -04:00
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QRTL :
2016-04-29 02:31:08 -03:00
init_qrtl ( ) ;
break ;
2019-01-15 13:46:13 -04:00
case Mode : : Number : : GUIDED :
2017-09-06 04:58:08 -03:00
guided_takeoff = false ;
break ;
2018-12-07 03:52:05 -04:00
# if QAUTOTUNE_ENABLED
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QAUTOTUNE :
2018-12-07 03:52:05 -04:00
return qautotune . init ( ) ;
# endif
2019-01-15 13:46:13 -04:00
case Mode : : Number : : QACRO :
2019-02-17 14:42:05 -04:00
init_qacro ( ) ;
break ;
2015-12-26 06:40:40 -04:00
default :
break ;
}
return true ;
}
/*
handle a MAVLink DO_VTOL_TRANSITION
*/
2021-02-01 12:26:22 -04:00
bool QuadPlane : : handle_do_vtol_transition ( enum MAV_VTOL_STATE state ) const
2015-12-26 06:40:40 -04:00
{
if ( ! available ( ) ) {
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " VTOL not available " ) ;
2016-04-22 05:22:31 -03:00
return false ;
2015-12-26 06:40:40 -04:00
}
2019-01-15 13:46:13 -04:00
if ( plane . control_mode ! = & plane . mode_auto ) {
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " VTOL transition only in AUTO " ) ;
2016-04-22 05:22:31 -03:00
return false ;
2015-12-26 06:40:40 -04:00
}
2016-04-22 05:22:31 -03:00
switch ( state ) {
2015-12-26 06:40:40 -04:00
case MAV_VTOL_STATE_MC :
if ( ! plane . auto_state . vtol_mode ) {
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " Entered VTOL mode " ) ;
2015-12-26 06:40:40 -04:00
}
plane . auto_state . vtol_mode = true ;
2016-04-22 05:22:31 -03:00
return true ;
2015-12-26 06:40:40 -04:00
case MAV_VTOL_STATE_FW :
if ( plane . auto_state . vtol_mode ) {
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " Exited VTOL mode " ) ;
2015-12-26 06:40:40 -04:00
}
plane . auto_state . vtol_mode = false ;
2016-04-22 05:22:31 -03:00
return true ;
default :
break ;
2015-12-26 06:40:40 -04:00
}
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " Invalid VTOL mode " ) ;
2016-04-22 05:22:31 -03:00
return false ;
2015-12-26 06:40:40 -04:00
}
2016-01-01 06:39:36 -04:00
/*
are we in a VTOL auto state ?
*/
2017-10-30 01:19:38 -03:00
bool QuadPlane : : in_vtol_auto ( void ) const
2016-01-01 06:39:36 -04:00
{
2019-02-20 20:01:32 -04:00
if ( ! available ( ) ) {
2016-05-05 22:28:26 -03:00
return false ;
}
2019-01-15 13:46:13 -04:00
if ( plane . control_mode ! = & plane . mode_auto ) {
2016-01-01 06:39:36 -04:00
return false ;
}
if ( plane . auto_state . vtol_mode ) {
return true ;
}
2017-10-29 03:31:09 -03:00
uint16_t id = plane . mission . get_current_nav_cmd ( ) . id ;
switch ( id ) {
2016-01-01 06:39:36 -04:00
case MAV_CMD_NAV_VTOL_TAKEOFF :
return true ;
2016-05-11 02:57:41 -03:00
case MAV_CMD_NAV_LOITER_UNLIM :
case MAV_CMD_NAV_LOITER_TIME :
2017-10-21 03:30:40 -03:00
case MAV_CMD_NAV_LOITER_TURNS :
case MAV_CMD_NAV_LOITER_TO_ALT :
2021-06-04 23:01:55 -03:00
return plane . auto_state . vtol_loiter ;
2017-10-29 03:31:09 -03:00
case MAV_CMD_NAV_TAKEOFF :
return is_vtol_takeoff ( id ) ;
2018-09-25 01:11:26 -03:00
case MAV_CMD_NAV_VTOL_LAND :
2017-10-29 03:31:09 -03:00
case MAV_CMD_NAV_LAND :
return is_vtol_land ( id ) ;
2016-01-01 06:39:36 -04:00
default :
return false ;
}
}
2016-02-20 05:20:27 -04:00
/*
2021-06-04 23:01:55 -03:00
are we in a VTOL mode ? This is used to decide if we run the
transition handling code or not
2016-02-20 05:20:27 -04:00
*/
2017-10-30 01:19:38 -03:00
bool QuadPlane : : in_vtol_mode ( void ) const
2016-02-20 05:20:27 -04:00
{
2019-02-20 20:01:32 -04:00
if ( ! available ( ) ) {
2016-04-09 05:48:22 -03:00
return false ;
}
2021-05-18 20:20:48 -03:00
if ( plane . control_mode = = & plane . mode_qrtl & &
( poscontrol . get_state ( ) = = QPOS_APPROACH | |
poscontrol . get_state ( ) = = QPOS_AIRBRAKE ) ) {
return false ;
}
if ( in_vtol_land_approach ( ) & &
poscontrol . get_state ( ) = = QPOS_APPROACH ) {
return false ;
}
2021-06-04 23:01:55 -03:00
if ( plane . control_mode - > is_vtol_mode ( ) ) {
return true ;
}
if ( plane . control_mode - > is_guided_mode ( )
& & plane . auto_state . vtol_loiter & &
poscontrol . get_state ( ) > QPOS_APPROACH ) {
return true ;
}
2021-09-16 19:46:03 -03:00
if ( plane . control_mode = = & plane . mode_guided & &
guided_takeoff ) {
return true ;
}
2021-06-04 23:01:55 -03:00
if ( in_vtol_auto ( ) ) {
if ( ! plane . auto_state . vtol_loiter | | poscontrol . get_state ( ) > QPOS_APPROACH ) {
return true ;
}
}
return false ;
2016-02-20 05:20:27 -04:00
}
2020-05-04 20:12:14 -03:00
/*
are we in a VTOL mode that needs position and velocity estimates ?
*/
bool QuadPlane : : in_vtol_posvel_mode ( void ) const
{
if ( ! available ( ) ) {
return false ;
}
return ( plane . control_mode = = & plane . mode_qloiter | |
plane . control_mode = = & plane . mode_qland | |
plane . control_mode = = & plane . mode_qrtl | |
plane . control_mode = = & plane . mode_qautotune | |
2020-09-22 14:00:39 -03:00
( plane . control_mode - > is_guided_mode ( ) & &
2021-06-04 05:56:59 -03:00
plane . auto_state . vtol_loiter & &
poscontrol . get_state ( ) > QPOS_APPROACH ) | |
2020-05-04 20:12:14 -03:00
in_vtol_auto ( ) ) ;
}
2016-04-28 23:53:20 -03:00
2021-05-14 07:54:17 -03:00
/*
update landing positioning offset
*/
void QuadPlane : : update_land_positioning ( void )
{
2021-06-08 03:02:15 -03:00
if ( ( options & OPTION_REPOSITION_LANDING ) = = 0 ) {
2021-05-14 07:54:17 -03:00
// not enabled
poscontrol . pilot_correction_active = false ;
2021-05-14 18:20:39 -03:00
poscontrol . target_vel_cms . zero ( ) ;
2021-05-14 07:54:17 -03:00
return ;
}
const float scale = 1.0 / 4500 ;
float roll_in = plane . channel_roll - > get_control_in ( ) * scale ;
float pitch_in = plane . channel_pitch - > get_control_in ( ) * scale ;
2021-06-08 03:02:15 -03:00
2021-05-14 22:12:16 -03:00
// limit correction speed to accel with stopping time constant of 0.5s
const float speed_max_cms = wp_nav - > get_wp_acceleration ( ) * 0.5 ;
2021-05-14 07:54:17 -03:00
const float dt = plane . scheduler . get_loop_period_s ( ) ;
2021-05-14 18:20:39 -03:00
poscontrol . target_vel_cms = Vector3f ( - pitch_in , roll_in , 0 ) * speed_max_cms ;
2021-05-14 22:12:16 -03:00
poscontrol . target_vel_cms . rotate_xy ( ahrs_view - > yaw ) ;
2021-05-14 07:54:17 -03:00
2021-06-17 22:20:26 -03:00
poscontrol . target_cm + = ( poscontrol . target_vel_cms * dt ) . topostype ( ) ;
2021-05-14 07:54:17 -03:00
2021-06-08 03:02:15 -03:00
poscontrol . pilot_correction_active = ( ! is_zero ( roll_in ) | | ! is_zero ( pitch_in ) ) ;
2021-05-14 22:12:16 -03:00
if ( poscontrol . pilot_correction_active ) {
poscontrol . pilot_correction_done = true ;
}
2021-05-14 07:54:17 -03:00
}
2021-05-14 05:08:34 -03:00
/*
run ( and possibly init ) xy controller
*/
void QuadPlane : : run_xy_controller ( void )
{
if ( ! pos_control - > is_active_xy ( ) ) {
2021-05-14 22:12:16 -03:00
pos_control - > set_max_speed_accel_xy ( wp_nav - > get_default_speed_xy ( ) , wp_nav - > get_wp_acceleration ( ) ) ;
2021-07-08 01:16:41 -03:00
pos_control - > set_correction_speed_accel_xy ( wp_nav - > get_default_speed_xy ( ) , wp_nav - > get_wp_acceleration ( ) ) ;
2021-05-14 05:08:34 -03:00
pos_control - > init_xy_controller ( ) ;
}
pos_control - > update_xy_controller ( ) ;
}
2021-05-18 20:20:48 -03:00
/*
initialise QPOS_APPROACH
*/
void QuadPlane : : poscontrol_init_approach ( void )
{
2021-11-22 18:06:30 -04:00
const float dist = plane . current_loc . get_distance ( plane . next_WP_loc ) ;
2021-05-18 20:20:48 -03:00
if ( ( options & OPTION_DISABLE_APPROACH ) ! = 0 ) {
// go straight to QPOS_POSITION1
poscontrol . set_state ( QPOS_POSITION1 ) ;
2021-11-22 18:06:30 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL Position1 d=%.1f " , dist ) ;
2021-05-18 20:20:48 -03:00
} else if ( poscontrol . get_state ( ) ! = QPOS_APPROACH ) {
2021-10-28 17:36:09 -03:00
// check if we are close to the destination. We don't want to
// do a full approach when very close
if ( dist < transition_threshold ( ) ) {
if ( is_tailsitter ( ) | | motors - > get_desired_spool_state ( ) = = AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL position1 d=%.1f " , dist ) ;
poscontrol . set_state ( QPOS_POSITION1 ) ;
} else {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL short d=%.1f " , dist ) ;
poscontrol . set_state ( QPOS_AIRBRAKE ) ;
}
} else {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL approach d=%.1f " , dist ) ;
poscontrol . set_state ( QPOS_APPROACH ) ;
}
2021-06-01 05:10:53 -03:00
poscontrol . thrust_loss_start_ms = 0 ;
2021-05-18 20:20:48 -03:00
}
}
2021-06-04 05:56:59 -03:00
/*
change position control state
*/
void QuadPlane : : PosControlState : : set_state ( enum position_control_state s )
{
2021-06-12 00:58:31 -03:00
if ( state ! = s ) {
2021-06-12 23:18:39 -03:00
auto & qp = plane . quadplane ;
2021-06-12 04:46:43 -03:00
pilot_correction_done = false ;
2021-06-12 00:58:31 -03:00
// handle resets needed for when the state changes
if ( s = = QPOS_POSITION1 ) {
reached_wp_speed = false ;
2021-06-12 23:18:39 -03:00
qp . attitude_control - > reset_yaw_target_and_rate ( ) ;
2021-11-22 18:53:50 -04:00
pos1_start_speed = plane . ahrs . groundspeed_vector ( ) . length ( ) ;
2021-06-12 00:58:31 -03:00
} else if ( s = = QPOS_POSITION2 ) {
// POSITION2 changes target speed, so we need to change it
// back to normal
qp . pos_control - > set_max_speed_accel_xy ( qp . wp_nav - > get_default_speed_xy ( ) ,
qp . wp_nav - > get_wp_acceleration ( ) ) ;
2021-07-08 01:16:41 -03:00
qp . pos_control - > set_correction_speed_accel_xy ( qp . wp_nav - > get_default_speed_xy ( ) ,
qp . wp_nav - > get_wp_acceleration ( ) ) ;
2021-06-18 22:55:06 -03:00
} else if ( s = = QPOS_AIRBRAKE ) {
// start with zero integrator on vertical throttle
qp . pos_control - > get_accel_z_pid ( ) . set_integrator ( 0 ) ;
2021-06-12 00:58:31 -03:00
}
}
2021-06-04 05:56:59 -03:00
state = s ;
last_state_change_ms = AP_HAL : : millis ( ) ;
2021-10-27 15:58:36 -03:00
// we consider setting the state to be equivalent to running to
// prevent code from overriding the state as stale
last_run_ms = last_state_change_ms ;
2021-06-04 05:56:59 -03:00
}
2015-12-26 06:40:40 -04:00
/*
2016-04-28 23:53:20 -03:00
main landing controller . Used for landing and RTL .
2015-12-26 06:40:40 -04:00
*/
2016-05-05 19:27:47 -03:00
void QuadPlane : : vtol_position_controller ( void )
2015-12-26 06:40:40 -04:00
{
2016-01-09 01:26:13 -04:00
if ( ! setup ( ) ) {
return ;
}
2016-04-01 03:29:51 -03:00
2016-04-28 23:53:20 -03:00
const Location & loc = plane . next_WP_loc ;
2021-06-04 05:56:59 -03:00
uint32_t now_ms = AP_HAL : : millis ( ) ;
2017-04-22 23:49:48 -03:00
2021-06-09 21:41:35 -03:00
// distance that we switch to QPOS_POSITION2
const float position2_dist_threshold = 5.0 ;
// target speed when we reach position2 threshold
const float position2_target_speed = 2.0 ;
2018-09-13 21:02:18 -03:00
check_attitude_relax ( ) ;
2021-10-27 15:58:36 -03:00
if ( hal . util - > get_soft_armed ( ) ) {
poscontrol . last_run_ms = now_ms ;
}
2018-03-09 02:28:02 -04:00
// horizontal position control
2021-05-18 02:15:46 -03:00
switch ( poscontrol . get_state ( ) ) {
2016-04-28 23:53:20 -03:00
2021-06-08 21:54:20 -03:00
case QPOS_NONE :
poscontrol . set_state ( QPOS_POSITION1 ) ;
INTERNAL_ERROR ( AP_InternalError : : error_t : : flow_of_control ) ;
break ;
2021-05-18 20:20:48 -03:00
case QPOS_APPROACH :
2021-06-04 05:56:59 -03:00
if ( in_vtol_mode ( ) ) {
// this means we're not running update_transition() and
// thus not doing qassist checking, force POSITION1 mode
// now. We don't expect this to trigger, it is a failsafe
// for a logic error
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL position1 nvtol " ) ;
poscontrol . set_state ( QPOS_POSITION1 ) ;
INTERNAL_ERROR ( AP_InternalError : : error_t : : flow_of_control ) ;
}
FALLTHROUGH ;
2021-05-18 20:20:48 -03:00
case QPOS_AIRBRAKE : {
float aspeed ;
const Vector2f closing_vel = landing_closing_velocity ( ) ;
const Vector2f desired_closing_vel = landing_desired_closing_velocity ( ) ;
const float groundspeed = plane . ahrs . groundspeed ( ) ;
const float distance = plane . auto_state . wp_distance ;
const float closing_speed = closing_vel . length ( ) ;
const float desired_closing_speed = desired_closing_vel . length ( ) ;
if ( ! plane . ahrs . airspeed_estimate ( aspeed ) ) {
aspeed = groundspeed ;
}
// speed for crossover to POSITION1 controller
2021-05-19 23:49:44 -03:00
const float aspeed_threshold = MAX ( plane . aparm . airspeed_min - 2 , assist_speed ) ;
2021-05-18 20:20:48 -03:00
// run fixed wing navigation
2021-05-19 00:38:12 -03:00
plane . nav_controller - > update_waypoint ( plane . current_loc , loc ) ;
2021-05-18 20:20:48 -03:00
// use TECS for throttle
SRV_Channels : : set_output_scaled ( SRV_Channel : : k_throttle , plane . SpdHgt_Controller - > get_throttle_demand ( ) ) ;
// use TECS for pitch
int32_t commanded_pitch = plane . SpdHgt_Controller - > get_pitch_demand ( ) ;
plane . nav_pitch_cd = constrain_int32 ( commanded_pitch , plane . pitch_limit_min_cd , plane . aparm . pitch_limit_max_cd . get ( ) ) ;
if ( poscontrol . get_state ( ) = = QPOS_AIRBRAKE ) {
// don't allow down pitch in airbrake
plane . nav_pitch_cd = MAX ( plane . nav_pitch_cd , 0 ) ;
}
// use nav controller roll
plane . calc_nav_roll ( ) ;
const float stop_distance = stopping_distance ( ) ;
2021-06-18 22:55:06 -03:00
if ( poscontrol . get_state ( ) = = QPOS_AIRBRAKE ) {
hold_hover ( 0 ) ;
}
2021-05-18 20:20:48 -03:00
/*
see if we should start airbraking stage . For non - tailsitters
we can use the VTOL motors as airbrakes by firing them up
before we transition . This gives a smoother transition and
gives us a nice lot of deceleration
*/
if ( poscontrol . get_state ( ) = = QPOS_APPROACH & & distance < stop_distance ) {
if ( is_tailsitter ( ) | | motors - > get_desired_spool_state ( ) = = AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) {
// tailsitters don't use airbrake stage for landing
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL position1 v=%.1f d=%.0f sd=%.0f h=%.1f " ,
groundspeed ,
plane . auto_state . wp_distance ,
stop_distance ,
plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ) ;
poscontrol . set_state ( QPOS_POSITION1 ) ;
} else {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL airbrake v=%.1f d=%.0f sd=%.0f h=%.1f " ,
groundspeed ,
distance ,
stop_distance ,
plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ) ;
poscontrol . set_state ( QPOS_AIRBRAKE ) ;
}
}
/*
we must switch to POSITION1 if our airspeed drops below the
2021-06-04 05:56:59 -03:00
assist speed . We additionally switch to POSITION1 if we are
2021-05-18 20:20:48 -03:00
too far above our desired velocity profile , or our attitude
has deviated too much
*/
const int32_t attitude_error_threshold_cd = 1000 ;
// use at least 1s of airbrake time to ensure motors have a chance to
// properly spin up
const uint32_t min_airbrake_ms = 1000 ;
if ( poscontrol . get_state ( ) = = QPOS_AIRBRAKE & &
poscontrol . time_since_state_start_ms ( ) > min_airbrake_ms & &
( aspeed < aspeed_threshold | |
closing_speed > MAX ( desired_closing_speed * 1.2 , desired_closing_speed + 2 ) | |
labs ( plane . ahrs . roll_sensor - plane . nav_roll_cd ) > attitude_error_threshold_cd | |
labs ( plane . ahrs . pitch_sensor - plane . nav_pitch_cd ) > attitude_error_threshold_cd ) ) {
2021-06-12 04:46:43 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL position1 v=%.1f d=%.1f h=%.1f dc=%.1f " ,
2021-05-18 20:20:48 -03:00
( double ) groundspeed ,
( double ) plane . auto_state . wp_distance ,
2021-06-12 04:46:43 -03:00
plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ,
desired_closing_speed ) ;
2021-05-18 20:20:48 -03:00
poscontrol . set_state ( QPOS_POSITION1 ) ;
// switch to vfwd for throttle control
vel_forward . integrator = SRV_Channels : : get_output_scaled ( SRV_Channel : : k_throttle ) ;
2021-06-04 05:56:59 -03:00
vel_forward . last_ms = now_ms ;
2021-05-18 20:20:48 -03:00
}
2021-06-02 22:04:19 -03:00
if ( tilt . tilt_mask = = 0 & & ! is_tailsitter ( ) ) {
/*
cope with fwd motor thrust loss during approach . We detect
this by looking for the fwd throttle saturating . This only
applies to separate lift - thrust vehicles
*/
bool throttle_saturated = SRV_Channels : : get_output_scaled ( SRV_Channel : : k_throttle ) > = plane . aparm . throttle_max ;
if ( throttle_saturated & &
motors - > get_desired_spool_state ( ) < AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED & &
plane . auto_state . sink_rate > 0.2 & & aspeed < aspeed_threshold + 4 ) {
if ( poscontrol . thrust_loss_start_ms = = 0 ) {
2021-06-04 05:56:59 -03:00
poscontrol . thrust_loss_start_ms = now_ms ;
2021-06-02 22:04:19 -03:00
}
2021-06-04 05:56:59 -03:00
if ( now_ms - poscontrol . thrust_loss_start_ms > 5000 ) {
2021-06-02 22:04:19 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL pos1 thrust loss as=%.1f at=%.1f " ,
aspeed , aspeed_threshold ) ;
poscontrol . set_state ( QPOS_POSITION1 ) ;
}
} else {
poscontrol . thrust_loss_start_ms = 0 ;
2021-06-01 05:10:53 -03:00
}
2021-06-02 22:04:19 -03:00
// handle loss of forward thrust in approach based on low airspeed detection
if ( poscontrol . get_state ( ) = = QPOS_APPROACH & & aspeed < aspeed_threshold & &
motors - > get_desired_spool_state ( ) < AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL pos1 low speed as=%.1f at=%.1f " ,
2021-06-01 05:10:53 -03:00
aspeed , aspeed_threshold ) ;
poscontrol . set_state ( QPOS_POSITION1 ) ;
}
2021-05-19 23:49:44 -03:00
}
2021-05-18 20:20:48 -03:00
if ( poscontrol . get_state ( ) = = QPOS_APPROACH ) {
poscontrol_init_approach ( ) ;
}
break ;
}
2016-05-05 19:27:47 -03:00
case QPOS_POSITION1 : {
2021-05-14 07:54:17 -03:00
setup_target_position ( ) ;
2021-06-11 05:23:15 -03:00
if ( is_tailsitter ( ) ) {
if ( in_tailsitter_vtol_transition ( ) ) {
break ;
}
poscontrol . set_state ( QPOS_POSITION2 ) ;
poscontrol . pilot_correction_done = false ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL position2 started v=%.1f d=%.1f " ,
( double ) ahrs . groundspeed ( ) , ( double ) plane . auto_state . wp_distance ) ;
break ;
}
2019-04-08 10:16:20 -03:00
const Vector2f diff_wp = plane . current_loc . get_distance_NE ( loc ) ;
2019-02-22 02:04:35 -04:00
const float distance = diff_wp . length ( ) ;
2021-06-09 21:41:35 -03:00
// calculate speed we should be at to reach the position2
// target speed at the position2 distance threshold, assuming
// Q_TRANS_DECEL is correct
const float stopping_speed = safe_sqrt ( MAX ( 0 , distance - position2_dist_threshold ) * 2 * transition_decel ) + position2_target_speed ;
float target_speed = stopping_speed ;
// maximum configured VTOL speed
2021-11-05 19:53:25 -03:00
const float wp_speed = wp_nav - > get_default_speed_xy ( ) * 0.01 ;
2021-06-09 21:41:35 -03:00
const float current_speed_sq = plane . ahrs . groundspeed_vector ( ) . length_squared ( ) ;
2021-06-12 00:58:31 -03:00
const float scaled_wp_speed = get_scaled_wp_speed ( degrees ( diff_wp . angle ( ) ) ) ;
2021-11-22 18:53:50 -04:00
// limit target speed to initial pos1 speed, but at least twice the Q_WP_SPEED
target_speed = MIN ( MAX ( poscontrol . pos1_start_speed , 2 * wp_speed ) , target_speed ) ;
2021-06-12 00:58:31 -03:00
if ( poscontrol . reached_wp_speed | |
current_speed_sq < sq ( wp_speed ) | |
wp_speed > 1.35 * scaled_wp_speed ) {
// once we get below the Q_WP_SPEED then we don't want to
// speed up again. At that point we should fly within the
// limits of the configured VTOL controller we also apply
// this limit when we are more than 45 degrees off the
// target in yaw, which is when we start to become
// unstable
target_speed = MIN ( target_speed , scaled_wp_speed ) ;
poscontrol . reached_wp_speed = true ;
2016-04-02 06:54:01 -03:00
}
2016-04-10 07:36:20 -03:00
// run fixed wing navigation
2021-05-19 00:38:12 -03:00
plane . nav_controller - > update_waypoint ( plane . current_loc , loc ) ;
2016-04-10 07:36:20 -03:00
2021-06-09 22:22:51 -03:00
Vector2f target_speed_xy ;
if ( distance > 0.1 ) {
target_speed_xy = diff_wp . normalized ( ) * target_speed ;
}
2021-05-19 11:08:14 -03:00
pos_control - > set_vel_desired_xy_cms ( target_speed_xy * 100 ) ;
2016-04-10 07:36:20 -03:00
2018-02-09 23:52:46 -04:00
// reset position controller xy target to current position
// because we only want velocity control (no position control)
2016-04-10 07:36:20 -03:00
const Vector3f & curr_pos = inertial_nav . get_position ( ) ;
2021-05-03 11:44:05 -03:00
pos_control - > set_pos_target_xy_cm ( curr_pos . x , curr_pos . y ) ;
2021-05-19 11:08:14 -03:00
pos_control - > set_accel_desired_xy_cmss ( Vector2f ( ) ) ;
2018-02-09 23:52:46 -04:00
// run horizontal velocity controller
2021-05-14 05:08:34 -03:00
run_xy_controller ( ) ;
2018-02-09 23:52:46 -04:00
2016-04-02 06:54:01 -03:00
// nav roll and pitch are controller by position controller
2021-05-12 01:32:42 -03:00
plane . nav_roll_cd = pos_control - > get_roll_cd ( ) ;
plane . nav_pitch_cd = pos_control - > get_pitch_cd ( ) ;
2016-04-02 06:54:01 -03:00
2021-10-09 05:33:47 -03:00
if ( ! is_tailsitter ( ) ) {
/*
limit the pitch down with an expanding envelope . This
prevents the velocity controller demanding nose down during
the initial slowdown if the target velocity curve is higher
than the actual velocity curve ( for a high drag
aircraft ) . Nose down will cause a lot of downforce on the
wings which will draw a lot of current and also cause the
aircraft to lose altitude rapidly . pitch limit varies also with speed
to prevent inability to progress to position if moving from a loiter
to landing
*/
float minlimit_cd = linear_interpolate ( - 300 , MAX ( - aparm . angle_max , plane . aparm . pitch_limit_min_cd ) ,
poscontrol . time_since_state_start_ms ( ) ,
0 , 5000 ) ;
if ( plane . nav_pitch_cd < minlimit_cd ) {
plane . nav_pitch_cd = minlimit_cd ;
// tell the pos controller we have limited the pitch to
// stop integrator buildup
pos_control - > set_externally_limited_xy ( ) ;
}
2016-04-10 07:36:20 -03:00
}
2021-06-12 00:58:31 -03:00
2016-04-02 06:54:01 -03:00
// call attitude controller
2016-06-16 23:48:25 -03:00
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( plane . nav_roll_cd ,
2021-05-18 02:15:46 -03:00
plane . nav_pitch_cd ,
desired_auto_yaw_rate_cds ( ) + get_weathervane_yaw_rate_cds ( ) ) ;
2021-06-09 21:41:35 -03:00
if ( plane . auto_state . wp_distance < position2_dist_threshold ) {
2021-05-18 02:15:46 -03:00
poscontrol . set_state ( QPOS_POSITION2 ) ;
2021-05-14 22:12:16 -03:00
poscontrol . pilot_correction_done = false ;
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL position2 started v=%.1f d=%.1f " ,
2016-04-14 20:22:41 -03:00
( double ) ahrs . groundspeed ( ) , ( double ) plane . auto_state . wp_distance ) ;
2016-04-02 06:54:01 -03:00
}
2016-04-28 23:53:20 -03:00
break ;
}
2016-05-05 19:27:47 -03:00
case QPOS_POSITION2 :
2021-05-14 06:16:37 -03:00
case QPOS_LAND_DESCEND : {
2016-01-15 01:49:49 -04:00
/*
2018-02-09 23:52:46 -04:00
for final land repositioning and descent we run the position controller
2016-01-15 01:49:49 -04:00
*/
2021-05-14 22:12:16 -03:00
if ( poscontrol . pilot_correction_done ) {
2021-05-19 11:08:14 -03:00
// if the pilot has repositioned the vehicle then we switch to velocity control. This prevents the vehicle
// shifting position in the event of GPS glitches.
2021-06-21 04:22:56 -03:00
Vector2f zero ;
pos_control - > input_vel_accel_xy ( poscontrol . target_vel_cms . xy ( ) , zero ) ;
2021-05-14 22:12:16 -03:00
} else {
2021-06-21 04:22:56 -03:00
Vector2f zero ;
pos_control - > input_pos_vel_accel_xy ( poscontrol . target_cm . xy ( ) , zero , zero ) ;
2021-05-14 22:12:16 -03:00
}
2018-02-09 23:52:46 -04:00
2016-01-09 18:42:46 -04:00
// also run fixed wing navigation
2021-05-19 00:38:12 -03:00
plane . nav_controller - > update_waypoint ( plane . current_loc , loc ) ;
2016-01-15 01:49:49 -04:00
2021-05-14 07:54:17 -03:00
update_land_positioning ( ) ;
2021-06-12 00:58:31 -03:00
/*
apply the same asymmetric speed limits from POSITION1 , so we
don ' t suddenly speed up when we change to POSITION2 and
LAND_DESCEND
*/
const Vector2f diff_wp = plane . current_loc . get_distance_NE ( loc ) ;
const float scaled_wp_speed = get_scaled_wp_speed ( degrees ( diff_wp . angle ( ) ) ) ;
pos_control - > set_max_speed_accel_xy ( scaled_wp_speed * 100 , wp_nav - > get_wp_acceleration ( ) ) ;
2021-07-08 01:16:41 -03:00
pos_control - > set_correction_speed_accel_xy ( scaled_wp_speed * 100 , wp_nav - > get_wp_acceleration ( ) ) ;
2021-06-12 00:58:31 -03:00
2021-05-14 06:16:37 -03:00
run_xy_controller ( ) ;
2021-05-19 11:08:14 -03:00
// nav roll and pitch are controlled by position controller
2021-05-14 06:16:37 -03:00
plane . nav_roll_cd = pos_control - > get_roll_cd ( ) ;
plane . nav_pitch_cd = pos_control - > get_pitch_cd ( ) ;
2018-02-09 23:52:46 -04:00
2021-05-14 06:16:37 -03:00
// call attitude controller
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( plane . nav_roll_cd ,
plane . nav_pitch_cd ,
get_pilot_input_yaw_rate_cds ( ) + get_weathervane_yaw_rate_cds ( ) ) ;
break ;
}
2018-02-09 23:52:46 -04:00
2021-05-14 06:16:37 -03:00
case QPOS_LAND_FINAL :
2021-05-14 07:54:17 -03:00
update_land_positioning ( ) ;
2021-05-14 06:16:37 -03:00
// relax when close to the ground
2021-05-14 18:20:39 -03:00
if ( should_relax ( ) ) {
2021-05-14 06:16:37 -03:00
pos_control - > relax_velocity_controller_xy ( ) ;
2019-02-22 02:04:35 -04:00
} else {
2021-05-14 18:20:39 -03:00
// we use velocity control in QPOS_LAND_FINAL to allow for GPS glitch handling
2021-06-21 04:22:56 -03:00
Vector2f zero ;
pos_control - > input_vel_accel_xy ( poscontrol . target_vel_cms . xy ( ) , zero ) ;
2019-02-22 02:04:35 -04:00
}
2021-05-14 06:16:37 -03:00
2021-05-14 05:08:34 -03:00
run_xy_controller ( ) ;
2016-01-15 01:49:49 -04:00
// nav roll and pitch are controller by position controller
2021-05-12 01:32:42 -03:00
plane . nav_roll_cd = pos_control - > get_roll_cd ( ) ;
plane . nav_pitch_cd = pos_control - > get_pitch_cd ( ) ;
2016-01-09 18:42:46 -04:00
2016-01-15 01:49:49 -04:00
// call attitude controller
2016-06-16 23:48:25 -03:00
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( plane . nav_roll_cd ,
2018-02-09 23:52:46 -04:00
plane . nav_pitch_cd ,
get_pilot_input_yaw_rate_cds ( ) + get_weathervane_yaw_rate_cds ( ) ) ;
2016-04-28 23:53:20 -03:00
break ;
2016-01-09 18:42:46 -04:00
2016-05-05 19:27:47 -03:00
case QPOS_LAND_COMPLETE :
2016-04-28 23:53:20 -03:00
// nothing to do
break ;
2016-01-02 02:55:56 -04:00
}
2015-12-26 06:40:40 -04:00
2016-04-28 23:53:20 -03:00
// now height control
2021-05-18 02:15:46 -03:00
switch ( poscontrol . get_state ( ) ) {
2021-06-08 21:54:20 -03:00
case QPOS_NONE :
poscontrol . set_state ( QPOS_POSITION1 ) ;
INTERNAL_ERROR ( AP_InternalError : : error_t : : flow_of_control ) ;
break ;
2021-05-18 20:20:48 -03:00
case QPOS_APPROACH :
case QPOS_AIRBRAKE :
// we just want stability from the VTOL controller in these
// phases of landing, so relax the Z controller, unless we are
// providing assistance
if ( transition_state = = TRANSITION_DONE ) {
pos_control - > relax_z_controller ( 0 ) ;
}
break ;
2016-05-05 19:27:47 -03:00
case QPOS_POSITION1 :
2021-06-11 05:23:15 -03:00
if ( in_tailsitter_vtol_transition ( ) ) {
pos_control - > relax_z_controller ( 0 ) ;
break ;
}
FALLTHROUGH ;
2017-10-21 03:30:40 -03:00
case QPOS_POSITION2 : {
bool vtol_loiter_auto = false ;
2019-01-15 13:46:13 -04:00
if ( plane . control_mode = = & plane . mode_auto ) {
2017-10-21 03:30:40 -03:00
switch ( plane . mission . get_current_nav_cmd ( ) . id ) {
case MAV_CMD_NAV_LOITER_UNLIM :
case MAV_CMD_NAV_LOITER_TIME :
case MAV_CMD_NAV_LOITER_TURNS :
case MAV_CMD_NAV_LOITER_TO_ALT :
vtol_loiter_auto = true ;
break ;
}
}
2021-06-04 05:56:59 -03:00
if ( plane . control_mode = = & plane . mode_guided | | vtol_loiter_auto ) {
2016-04-29 02:31:08 -03:00
plane . ahrs . get_position ( plane . current_loc ) ;
2021-04-04 16:37:33 -03:00
int32_t target_altitude_cm ;
if ( ! plane . next_WP_loc . get_alt_cm ( Location : : AltFrame : : ABOVE_HOME , target_altitude_cm ) ) {
break ;
}
2021-05-14 07:18:38 -03:00
if ( poscontrol . slow_descent & &
plane . prev_WP_loc . get_distance ( plane . next_WP_loc ) > 50 ) {
2016-04-29 02:50:45 -03:00
// gradually descend as we approach target
2019-04-12 05:23:04 -03:00
plane . auto_state . wp_proportion = plane . current_loc . line_path_proportion ( plane . prev_WP_loc , plane . next_WP_loc ) ;
2021-04-04 16:37:33 -03:00
int32_t prev_alt ;
if ( plane . prev_WP_loc . get_alt_cm ( Location : : AltFrame : : ABOVE_HOME , prev_alt ) ) {
target_altitude_cm = linear_interpolate ( prev_alt ,
target_altitude_cm ,
plane . auto_state . wp_proportion ,
0 , 1 ) ;
}
}
# if AP_TERRAIN_AVAILABLE
float terrain_altitude_offset_cm ;
if ( plane . next_WP_loc . terrain_alt & & plane . terrain . height_terrain_difference_home ( terrain_altitude_offset_cm , true ) ) {
// Climb if current terrain is above home, target_altitude_cm is reltive to home
target_altitude_cm + = MAX ( terrain_altitude_offset_cm * 100 , 0 ) ;
2016-04-29 02:50:45 -03:00
}
2021-04-04 16:37:33 -03:00
# endif
2021-06-21 04:22:56 -03:00
float zero = 0 ;
float target_z = target_altitude_cm ;
pos_control - > input_pos_vel_accel_z ( target_z , zero , 0 ) ;
2016-04-29 02:31:08 -03:00
} else {
2021-05-14 06:16:37 -03:00
set_climb_rate_cms ( 0 , false ) ;
2016-04-29 02:31:08 -03:00
}
2016-04-28 23:53:20 -03:00
break ;
2017-10-21 03:30:40 -03:00
}
2015-12-26 06:40:40 -04:00
2021-05-14 07:54:17 -03:00
case QPOS_LAND_DESCEND :
case QPOS_LAND_FINAL : {
2016-06-11 01:37:44 -03:00
float height_above_ground = plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ;
2021-05-18 02:15:46 -03:00
if ( poscontrol . get_state ( ) = = QPOS_LAND_FINAL ) {
2021-05-14 07:54:17 -03:00
if ( ( options & OPTION_DISABLE_GROUND_EFFECT_COMP ) = = 0 ) {
2021-05-28 20:54:15 -03:00
ahrs . set_touchdown_expected ( true ) ;
2021-05-14 07:54:17 -03:00
}
}
const float descent_rate_cms = landing_descent_rate_cms ( height_above_ground ) ;
set_climb_rate_cms ( - descent_rate_cms , descent_rate_cms > 0 ) ;
2016-01-01 05:42:10 -04:00
break ;
2016-04-28 23:53:20 -03:00
}
2016-05-05 19:27:47 -03:00
case QPOS_LAND_COMPLETE :
2016-01-01 05:42:10 -04:00
break ;
}
2017-03-13 04:04:09 -03:00
run_z_controller ( ) ;
2021-06-04 05:56:59 -03:00
if ( now_ms - poscontrol . last_log_ms > = 40 ) {
// log poscontrol at 25Hz
poscontrol . last_log_ms = now_ms ;
AP : : logger ( ) . Write ( " QPOS " , " TimeUS,State,Dist " , " QBf " ,
AP_HAL : : micros64 ( ) ,
poscontrol . get_state ( ) ,
plane . auto_state . wp_distance ) ;
}
2015-12-26 06:40:40 -04:00
}
2016-01-01 05:42:10 -04:00
2016-04-28 23:53:20 -03:00
2021-06-12 00:58:31 -03:00
/*
we want to limit WP speed to a lower speed when more than 20 degrees
off pointing at the destination . quadplanes are often
unstable when flying sideways or backwards
*/
float QuadPlane : : get_scaled_wp_speed ( float target_bearing_deg ) const
{
const float yaw_difference = fabsf ( wrap_180 ( degrees ( plane . ahrs . yaw ) - target_bearing_deg ) ) ;
2021-06-12 04:46:43 -03:00
const float wp_speed = wp_nav - > get_default_speed_xy ( ) * 0.01 ;
2021-06-12 00:58:31 -03:00
if ( yaw_difference > 20 ) {
// this gives a factor of 2x reduction in max speed when
// off by 90 degrees, and 3x when off by 180 degrees
const float speed_reduction = linear_interpolate ( 1 , 3 ,
yaw_difference ,
20 , 160 ) ;
return wp_speed / speed_reduction ;
}
return wp_speed ;
}
2016-04-28 23:53:20 -03:00
/*
setup the target position based on plane . next_WP_loc
*/
void QuadPlane : : setup_target_position ( void )
{
const Location & loc = plane . next_WP_loc ;
2019-06-28 18:31:44 -03:00
Location origin ;
if ( ! ahrs . get_origin ( origin ) ) {
origin . zero ( ) ;
}
2021-05-18 20:20:48 -03:00
if ( ! in_vtol_land_approach ( ) | | poscontrol . get_state ( ) > QPOS_APPROACH ) {
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
}
2017-07-10 01:00:45 -03:00
2019-04-08 10:16:20 -03:00
const Vector2f diff2d = origin . get_distance_NE ( loc ) ;
2021-05-14 07:54:17 -03:00
poscontrol . target_cm . x = diff2d . x * 100 ;
poscontrol . target_cm . y = diff2d . y * 100 ;
poscontrol . target_cm . z = plane . next_WP_loc . alt - origin . alt ;
2016-04-28 23:53:20 -03:00
2018-01-24 20:38:07 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
2019-04-08 08:45:29 -03:00
if ( ! loc . same_latlon_as ( last_auto_target ) | |
2016-04-28 23:53:20 -03:00
plane . next_WP_loc . alt ! = last_auto_target . alt | |
2018-01-24 20:38:07 -04:00
now - last_loiter_ms > 500 ) {
2021-06-17 22:20:26 -03:00
wp_nav - > set_wp_destination ( poscontrol . target_cm . tofloat ( ) ) ;
2016-04-28 23:53:20 -03:00
last_auto_target = loc ;
}
2018-01-24 20:38:07 -04:00
last_loiter_ms = now ;
2021-05-19 11:08:14 -03:00
// set vertical speed and acceleration limits
2021-05-03 11:44:05 -03:00
pos_control - > set_max_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2021-07-08 01:16:41 -03:00
pos_control - > set_correction_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2016-04-28 23:53:20 -03:00
}
/*
run takeoff controller to climb vertically
*/
void QuadPlane : : takeoff_controller ( void )
{
/*
2018-02-09 23:52:46 -04:00
for takeoff we use the position controller
2016-04-28 23:53:20 -03:00
*/
2018-09-13 21:02:18 -03:00
check_attitude_relax ( ) ;
2016-04-28 23:53:20 -03:00
setup_target_position ( ) ;
2018-02-09 23:52:46 -04:00
// set position controller desired velocity and acceleration to zero
2021-05-19 11:08:14 -03:00
pos_control - > set_vel_desired_xy_cms ( Vector2f ( ) ) ;
pos_control - > set_accel_desired_xy_cmss ( Vector2f ( ) ) ;
2018-02-09 23:52:46 -04:00
// set position control target and update
2021-06-21 04:22:56 -03:00
Vector2f zero ;
2021-05-14 06:16:37 -03:00
pos_control - > input_vel_accel_xy ( zero , zero ) ;
2021-05-14 05:08:34 -03:00
run_xy_controller ( ) ;
2018-02-09 23:52:46 -04:00
2016-04-28 23:53:20 -03:00
// nav roll and pitch are controller by position controller
2021-05-12 01:32:42 -03:00
plane . nav_roll_cd = pos_control - > get_roll_cd ( ) ;
plane . nav_pitch_cd = pos_control - > get_pitch_cd ( ) ;
2018-02-09 23:52:46 -04:00
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( plane . nav_roll_cd ,
plane . nav_pitch_cd ,
get_pilot_input_yaw_rate_cds ( ) + get_weathervane_yaw_rate_cds ( ) ) ;
2021-09-15 23:53:32 -03:00
float vel_z = wp_nav - > get_default_speed_up ( ) ;
if ( guided_takeoff ) {
// for guided takeoff we aim for a specific height with zero
// velocity at that height
Location origin ;
if ( ahrs . get_origin ( origin ) ) {
// a small margin to ensure we do move to the next takeoff
// stage
const int32_t margin_cm = 5 ;
float pos_z = margin_cm + plane . next_WP_loc . alt - origin . alt ;
vel_z = 0 ;
pos_control - > input_pos_vel_accel_z ( pos_z , vel_z , 0 ) ;
} else {
set_climb_rate_cms ( vel_z , false ) ;
}
} else {
set_climb_rate_cms ( vel_z , false ) ;
}
2017-03-13 04:04:09 -03:00
run_z_controller ( ) ;
2016-04-28 23:53:20 -03:00
}
/*
run waypoint controller between prev_WP_loc and next_WP_loc
*/
void QuadPlane : : waypoint_controller ( void )
{
setup_target_position ( ) ;
2018-09-13 21:02:18 -03:00
check_attitude_relax ( ) ;
2016-04-28 23:53:20 -03:00
/*
this is full copter control of auto flight
*/
// run wpnav controller
wp_nav - > update_wpnav ( ) ;
// call attitude controller
attitude_control - > input_euler_angle_roll_pitch_yaw ( wp_nav - > get_roll ( ) ,
wp_nav - > get_pitch ( ) ,
wp_nav - > get_yaw ( ) ,
2017-07-10 01:00:45 -03:00
true ) ;
2016-04-28 23:53:20 -03:00
// nav roll and pitch are controller by loiter controller
plane . nav_roll_cd = wp_nav - > get_roll ( ) ;
plane . nav_pitch_cd = wp_nav - > get_pitch ( ) ;
// climb based on altitude error
2021-05-14 06:16:37 -03:00
set_climb_rate_cms ( assist_climb_rate_cms ( ) , false ) ;
2017-03-13 04:04:09 -03:00
run_z_controller ( ) ;
2016-04-28 23:53:20 -03:00
}
/*
handle auto - mode when auto_state . vtol_mode is true
*/
2019-10-01 11:11:48 -03:00
void QuadPlane : : control_auto ( void )
2016-04-28 23:53:20 -03:00
{
if ( ! setup ( ) ) {
return ;
}
2021-05-18 20:20:48 -03:00
if ( poscontrol . get_state ( ) > QPOS_APPROACH ) {
if ( ! plane . arming . get_delay_arming ( ) ) {
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
}
}
2016-04-28 23:53:20 -03:00
2017-10-29 03:31:09 -03:00
uint16_t id = plane . mission . get_current_nav_cmd ( ) . id ;
switch ( id ) {
2016-05-11 02:57:41 -03:00
case MAV_CMD_NAV_VTOL_TAKEOFF :
2017-10-29 03:31:09 -03:00
case MAV_CMD_NAV_TAKEOFF :
if ( is_vtol_takeoff ( id ) ) {
takeoff_controller ( ) ;
}
break ;
2018-09-25 01:11:26 -03:00
case MAV_CMD_NAV_VTOL_LAND :
2017-10-29 03:31:09 -03:00
case MAV_CMD_NAV_LAND :
if ( is_vtol_land ( id ) ) {
vtol_position_controller ( ) ;
}
2016-05-11 02:57:41 -03:00
break ;
case MAV_CMD_NAV_LOITER_UNLIM :
case MAV_CMD_NAV_LOITER_TIME :
2017-10-21 03:30:40 -03:00
case MAV_CMD_NAV_LOITER_TURNS :
2021-10-27 15:58:36 -03:00
case MAV_CMD_NAV_LOITER_TO_ALT : {
const uint32_t now = AP_HAL : : millis ( ) ;
if ( now - poscontrol . last_run_ms > 100 ) {
// ensure that poscontrol is reset
poscontrol . set_state ( QPOS_POSITION1 ) ;
}
2016-05-05 19:27:47 -03:00
vtol_position_controller ( ) ;
2021-10-27 15:58:36 -03:00
}
2016-05-11 02:57:41 -03:00
break ;
default :
2016-04-28 23:53:20 -03:00
waypoint_controller ( ) ;
2016-05-11 02:57:41 -03:00
break ;
2016-04-28 23:53:20 -03:00
}
}
2016-04-29 02:31:08 -03:00
/*
handle QRTL mode
*/
void QuadPlane : : control_qrtl ( void )
{
2016-05-05 19:27:47 -03:00
vtol_position_controller ( ) ;
2021-05-18 02:15:46 -03:00
if ( poscontrol . get_state ( ) > = QPOS_POSITION2 ) {
2016-04-29 02:31:08 -03:00
// change target altitude to home alt
plane . next_WP_loc . alt = plane . home . alt ;
verify_vtol_land ( ) ;
}
}
/*
handle QRTL mode
*/
void QuadPlane : : init_qrtl ( void )
{
// use do_RTL() to setup next_WP_loc
plane . do_RTL ( plane . home . alt + qrtl_alt * 100UL ) ;
plane . prev_WP_loc = plane . current_loc ;
2021-05-19 11:08:14 -03:00
pos_control - > set_accel_desired_xy_cmss ( Vector2f ( ) ) ;
2018-08-21 03:26:42 -03:00
pos_control - > init_xy_controller ( ) ;
2021-05-18 20:20:48 -03:00
poscontrol_init_approach ( ) ;
2021-06-01 03:21:26 -03:00
float dist = plane . next_WP_loc . get_distance ( plane . current_loc ) ;
const float radius = MAX ( fabsf ( plane . aparm . loiter_radius ) , fabsf ( plane . g . rtl_radius ) ) ;
if ( dist < 1.5 * radius & &
motors - > get_desired_spool_state ( ) = = AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) {
// we're close to destination and already running VTOL motors, don't transition
2021-06-12 00:58:31 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " VTOL position1 d=%.1f r=%.1f " , dist , radius ) ;
2021-06-01 03:21:26 -03:00
poscontrol . set_state ( QPOS_POSITION1 ) ;
}
2021-04-04 16:37:33 -03:00
int32_t from_alt ;
int32_t to_alt ;
if ( plane . current_loc . get_alt_cm ( Location : : AltFrame : : ABSOLUTE , from_alt ) & & plane . next_WP_loc . get_alt_cm ( Location : : AltFrame : : ABSOLUTE , to_alt ) ) {
poscontrol . slow_descent = from_alt > to_alt ;
return ;
}
// defualt back to old method
poscontrol . slow_descent = ( plane . current_loc . alt > plane . next_WP_loc . alt ) ;
2016-04-29 02:31:08 -03:00
}
2016-01-01 05:42:10 -04:00
/*
start a VTOL takeoff
*/
bool QuadPlane : : do_vtol_takeoff ( const AP_Mission : : Mission_Command & cmd )
{
2016-01-09 01:26:13 -04:00
if ( ! setup ( ) ) {
2016-01-01 05:42:10 -04:00
return false ;
}
2016-04-01 03:29:51 -03:00
2019-01-10 21:22:41 -04:00
// we always use the current location in XY for takeoff. The altitude defaults
// to relative to current height, but if Q_OPTIONS is set to respect takeoff frame
// then it will use normal frame handling for height
Location loc = cmd . content . location ;
loc . lat = 0 ;
loc . lng = 0 ;
plane . set_next_WP ( loc ) ;
2018-08-13 17:16:45 -03:00
if ( options & OPTION_RESPECT_TAKEOFF_FRAME ) {
if ( plane . current_loc . alt > = plane . next_WP_loc . alt ) {
// we are above the takeoff already, no need to do anything
return false ;
}
} else {
plane . next_WP_loc . alt = plane . current_loc . alt + cmd . content . location . alt ;
}
2016-01-01 05:42:10 -04:00
throttle_wait = false ;
2021-05-19 11:08:14 -03:00
// set vertical speed and acceleration limits
2021-05-03 11:44:05 -03:00
pos_control - > set_max_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2021-07-08 01:16:41 -03:00
pos_control - > set_correction_speed_accel_z ( - get_pilot_velocity_z_max_dn ( ) , pilot_velocity_z_max_up , pilot_accel_z ) ;
2021-05-19 11:08:14 -03:00
// initialise the vertical position controller
2021-05-03 11:44:05 -03:00
pos_control - > init_z_controller ( ) ;
2016-01-01 05:42:10 -04:00
// also update nav_controller for status output
2021-05-19 00:38:12 -03:00
plane . nav_controller - > update_waypoint ( plane . current_loc , plane . next_WP_loc ) ;
2018-12-15 01:23:56 -04:00
// calculate the time required to complete a takeoff
// this may be conservative and accept extra time due to clamping
// derived from the following latex equations if you want a nicely formatted view
// t_{accel} = \frac{V_max - V_z}{a}
// d_{accel} = V_z*t_{accel} + \frac{1}{2}*a*t_{accel}^2
// d_{remaining} = d_{total} - d_{accel}
// t_{constant} = \frac{d_{remaining}}{V_z}
// t = max(t_{accel}, 0) + max(t_{constant}, 0)
const float d_total = ( plane . next_WP_loc . alt - plane . current_loc . alt ) * 0.01f ;
const float accel_m_s_s = MAX ( 10 , pilot_accel_z ) * 0.01f ;
2021-03-16 18:54:03 -03:00
const float vel_max = MAX ( 10 , pilot_velocity_z_max_up ) * 0.01f ;
2018-12-15 01:23:56 -04:00
const float vel_z = inertial_nav . get_velocity_z ( ) * 0.01f ;
const float t_accel = ( vel_max - vel_z ) / accel_m_s_s ;
const float d_accel = vel_z * t_accel + 0.5f * accel_m_s_s * sq ( t_accel ) ;
const float d_remaining = d_total - d_accel ;
const float t_constant = d_remaining / vel_max ;
const float travel_time = MAX ( t_accel , 0 ) + MAX ( t_constant , 0 ) ;
// setup the takeoff failure handling code
takeoff_start_time_ms = millis ( ) ;
takeoff_time_limit_ms = MAX ( travel_time * takeoff_failure_scalar * 1000 , 5000 ) ; // minimum time 5 seconds
2016-01-01 05:42:10 -04:00
return true ;
}
/*
start a VTOL landing
*/
bool QuadPlane : : do_vtol_land ( const AP_Mission : : Mission_Command & cmd )
{
2016-01-09 01:26:13 -04:00
if ( ! setup ( ) ) {
2016-01-01 05:42:10 -04:00
return false ;
}
2018-09-17 21:35:24 -03:00
attitude_control - > reset_rate_controller_I_terms ( ) ;
2016-01-15 01:49:49 -04:00
2016-01-01 06:57:31 -04:00
plane . set_next_WP ( cmd . content . location ) ;
2016-01-02 02:55:56 -04:00
// initially aim for current altitude
plane . next_WP_loc . alt = plane . current_loc . alt ;
2021-05-18 02:15:46 -03:00
poscontrol . set_state ( QPOS_POSITION1 ) ;
2021-05-03 11:44:05 -03:00
2021-05-19 11:08:14 -03:00
// initialise the position controller
2018-08-21 03:26:42 -03:00
pos_control - > init_xy_controller ( ) ;
2021-05-03 11:44:05 -03:00
pos_control - > init_z_controller ( ) ;
2016-04-10 07:36:20 -03:00
2016-01-15 01:49:49 -04:00
throttle_wait = false ;
2016-06-16 05:17:46 -03:00
landing_detect . lower_limit_start_ms = 0 ;
2019-02-01 23:49:02 -04:00
landing_detect . land_start_ms = 0 ;
2018-09-25 01:11:26 -03:00
plane . crash_state . is_crashed = false ;
2016-01-01 07:09:11 -04:00
2016-01-01 05:42:10 -04:00
// also update nav_controller for status output
2021-05-19 00:38:12 -03:00
plane . nav_controller - > update_waypoint ( plane . current_loc , plane . next_WP_loc ) ;
2021-05-18 20:20:48 -03:00
poscontrol_init_approach ( ) ;
2016-01-01 05:42:10 -04:00
return true ;
}
/*
check if a VTOL takeoff has completed
*/
bool QuadPlane : : verify_vtol_takeoff ( const AP_Mission : : Mission_Command & cmd )
{
2016-01-01 07:09:11 -04:00
if ( ! available ( ) ) {
return true ;
}
2018-12-15 01:23:56 -04:00
const uint32_t now = millis ( ) ;
2021-12-03 18:31:48 -04:00
// reset takeoff if we aren't armed
2018-12-15 01:23:56 -04:00
if ( ! hal . util - > get_soft_armed ( ) ) {
2021-12-03 18:31:48 -04:00
do_vtol_takeoff ( cmd ) ;
return false ;
2018-12-15 01:23:56 -04:00
}
2020-09-22 18:05:38 -03:00
if ( now - takeoff_start_time_ms < 3000 & &
( options & OPTION_DISABLE_GROUND_EFFECT_COMP ) = = 0 ) {
2021-05-28 20:54:15 -03:00
ahrs . set_takeoff_expected ( true ) ;
2020-09-22 18:05:38 -03:00
}
2018-12-15 01:23:56 -04:00
// check for failure conditions
if ( is_positive ( takeoff_failure_scalar ) & & ( ( now - takeoff_start_time_ms ) > takeoff_time_limit_ms ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " Failed to complete takeoff within time limit " ) ;
2019-10-17 00:49:32 -03:00
plane . set_mode ( plane . mode_qland , ModeReason : : VTOL_FAILED_TAKEOFF ) ;
2018-12-15 01:23:56 -04:00
return false ;
}
2018-12-15 16:47:49 -04:00
if ( is_positive ( maximum_takeoff_airspeed ) & & ( plane . airspeed . get_airspeed ( ) > maximum_takeoff_airspeed ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " Failed to complete takeoff, excessive wind " ) ;
2019-10-17 00:49:32 -03:00
plane . set_mode ( plane . mode_qland , ModeReason : : VTOL_FAILED_TAKEOFF ) ;
2018-12-15 16:47:49 -04:00
return false ;
}
2016-01-08 19:47:41 -04:00
if ( plane . current_loc . alt < plane . next_WP_loc . alt ) {
2016-01-01 05:42:10 -04:00
return false ;
}
2017-11-19 01:05:01 -04:00
transition_state = is_tailsitter ( ) ? TRANSITION_ANGLE_WAIT_FW : TRANSITION_AIRSPEED_WAIT ;
2016-04-02 05:53:48 -03:00
plane . TECS_controller . set_pitch_max_limit ( transition_pitch_max ) ;
2021-05-03 11:44:05 -03:00
// todo: why are you doing this, I want to delete it.
2017-09-05 19:22:38 -03:00
set_alt_target_current ( ) ;
2016-08-29 05:04:42 -03:00
2021-03-03 07:49:04 -04:00
# if AC_FENCE == ENABLED
plane . fence . auto_enable_fence_after_takeoff ( ) ;
# endif
2020-02-06 00:44:26 -04:00
if ( plane . control_mode = = & plane . mode_auto ) {
// we reset TECS so that the target height filter is not
// constrained by the climb and sink rates from the initial
// takeoff height.
plane . SpdHgt_Controller - > reset ( ) ;
}
2020-08-01 07:31:53 -03:00
// don't crosstrack on next WP
plane . auto_state . next_wp_crosstrack = false ;
2016-01-01 05:42:10 -04:00
return true ;
}
2016-06-16 05:17:46 -03:00
/*
2019-02-01 23:49:02 -04:00
a landing detector based on change in altitude over a timeout
2016-06-16 05:17:46 -03:00
*/
2019-02-01 23:49:02 -04:00
bool QuadPlane : : land_detector ( uint32_t timeout_ms )
2016-03-09 03:20:41 -04:00
{
2018-01-24 20:38:07 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
2016-06-16 05:17:46 -03:00
bool might_be_landed = ( landing_detect . lower_limit_start_ms ! = 0 & &
now - landing_detect . lower_limit_start_ms > 1000 ) ;
if ( ! might_be_landed ) {
landing_detect . land_start_ms = 0 ;
2019-02-01 23:49:02 -04:00
return false ;
2016-06-16 05:17:46 -03:00
}
float height = inertial_nav . get_altitude ( ) * 0.01f ;
if ( landing_detect . land_start_ms = = 0 ) {
landing_detect . land_start_ms = now ;
landing_detect . vpos_start_m = height ;
}
2019-02-01 23:49:02 -04:00
2016-06-16 05:17:46 -03:00
// we only consider the vehicle landed when the motors have been
2019-02-01 23:49:02 -04:00
// at minimum for timeout_ms+1000 and the vertical position estimate has not
// changed by more than 20cm for timeout_ms
2016-07-08 05:38:43 -03:00
if ( fabsf ( height - landing_detect . vpos_start_m ) > 0.2 ) {
2016-06-16 05:17:46 -03:00
// height has changed, call off landing detection
landing_detect . land_start_ms = 0 ;
2019-02-01 23:49:02 -04:00
return false ;
2016-06-16 05:17:46 -03:00
}
2019-02-01 23:49:02 -04:00
if ( ( now - landing_detect . land_start_ms ) < timeout_ms | |
( now - landing_detect . lower_limit_start_ms ) < ( timeout_ms + 1000 ) ) {
2016-06-16 05:17:46 -03:00
// not landed yet
2019-02-01 23:49:02 -04:00
return false ;
}
return true ;
}
/*
check if a landing is complete
*/
2020-03-03 23:59:05 -04:00
bool QuadPlane : : check_land_complete ( void )
2019-02-01 23:49:02 -04:00
{
2021-05-18 02:15:46 -03:00
if ( poscontrol . get_state ( ) ! = QPOS_LAND_FINAL ) {
2019-02-01 23:49:02 -04:00
// only apply to final landing phase
2020-03-03 23:59:05 -04:00
return false ;
2016-03-09 03:20:41 -04:00
}
2019-02-01 23:49:02 -04:00
if ( land_detector ( 4000 ) ) {
2021-05-18 02:15:46 -03:00
poscontrol . set_state ( QPOS_LAND_COMPLETE ) ;
2019-02-01 23:49:02 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Land complete " ) ;
2020-03-03 23:59:05 -04:00
if ( plane . control_mode ! = & plane . mode_auto | |
! plane . mission . continue_after_land ( ) ) {
// disarm on land unless we have MIS_OPTIONS setup to
// continue after land in AUTO
plane . arming . disarm ( AP_Arming : : Method : : LANDED ) ;
}
return true ;
2019-02-01 23:49:02 -04:00
}
2020-03-03 23:59:05 -04:00
return false ;
2019-02-01 23:49:02 -04:00
}
/*
check if we should switch from QPOS_LAND_DESCEND to QPOS_LAND_FINAL
*/
bool QuadPlane : : check_land_final ( void )
{
float height_above_ground = plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ;
2020-01-11 00:17:23 -04:00
// we require 2 readings at 10Hz to be within 5m of each other to
// trigger the switch to land final. This prevents a short term
// glitch at high altitude from triggering land final
const float max_change = 5 ;
if ( height_above_ground < land_final_alt & &
fabsf ( height_above_ground - last_land_final_agl ) < max_change ) {
2019-02-01 23:49:02 -04:00
return true ;
}
2020-01-11 00:17:23 -04:00
last_land_final_agl = height_above_ground ;
2019-02-01 23:49:02 -04:00
/*
also apply landing detector , in case we have landed in descent
phase . Use a longer threshold
*/
return land_detector ( 6000 ) ;
2016-03-09 03:20:41 -04:00
}
2016-01-01 05:42:10 -04:00
/*
check if a VTOL landing has completed
*/
2016-04-29 02:31:08 -03:00
bool QuadPlane : : verify_vtol_land ( void )
2016-01-01 05:42:10 -04:00
{
2016-01-01 07:09:11 -04:00
if ( ! available ( ) ) {
return true ;
}
2021-06-12 04:46:43 -03:00
if ( poscontrol . get_state ( ) = = QPOS_POSITION2 ) {
// see if we should move onto the descend stage of landing
const float descend_dist_threshold = 2.0 ;
const float descend_speed_threshold = 3.0 ;
bool reached_position = false ;
if ( poscontrol . pilot_correction_done ) {
reached_position = ! poscontrol . pilot_correction_active ;
} else {
2021-06-17 22:20:26 -03:00
const float dist = ( inertial_nav . get_position ( ) . topostype ( ) - poscontrol . target_cm ) . xy ( ) . length ( ) * 0.01 ;
2021-06-12 04:46:43 -03:00
reached_position = dist < descend_dist_threshold ;
}
if ( reached_position & &
plane . ahrs . groundspeed ( ) < descend_speed_threshold ) {
poscontrol . set_state ( QPOS_LAND_DESCEND ) ;
poscontrol . pilot_correction_done = false ;
2021-01-13 19:36:42 -04:00
# if AC_FENCE == ENABLED
2021-06-12 04:46:43 -03:00
plane . fence . auto_disable_fence_for_landing ( ) ;
2021-01-13 19:36:42 -04:00
# endif
2020-03-31 22:42:16 -03:00
# if LANDING_GEAR_ENABLED == ENABLED
2021-06-12 04:46:43 -03:00
plane . g2 . landing_gear . deploy_for_landing ( ) ;
2020-03-31 22:42:16 -03:00
# endif
2021-06-12 04:46:43 -03:00
last_land_final_agl = plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Land descend started " ) ;
if ( plane . control_mode = = & plane . mode_auto ) {
// set height to mission height, so we can use the mission
// WP height for triggering land final if no rangefinder
// available
plane . set_next_WP ( plane . mission . get_current_nav_cmd ( ) . content . location ) ;
} else {
plane . set_next_WP ( plane . next_WP_loc ) ;
plane . next_WP_loc . alt = ahrs . get_home ( ) . alt ;
}
2019-02-23 22:53:52 -04:00
}
2016-01-02 02:55:56 -04:00
}
// at land_final_alt begin final landing
2021-05-18 02:15:46 -03:00
if ( poscontrol . get_state ( ) = = QPOS_LAND_DESCEND & & check_land_final ( ) ) {
poscontrol . set_state ( QPOS_LAND_FINAL ) ;
2016-07-25 02:46:17 -03:00
// cut IC engine if enabled
if ( land_icengine_cut ! = 0 ) {
plane . g2 . ice_control . engine_control ( 0 , 0 , 0 ) ;
}
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Land final started " ) ;
2016-01-02 02:55:56 -04:00
}
2020-03-03 23:59:05 -04:00
if ( check_land_complete ( ) & & plane . mission . continue_after_land ( ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Mission continue " ) ;
return true ;
}
2016-01-01 05:42:10 -04:00
return false ;
}
2016-03-24 22:33:19 -03:00
// Write a control tuning packet
void QuadPlane : : Log_Write_QControl_Tuning ( )
{
2018-12-21 03:42:59 -04:00
float des_alt_m = 0.0f ;
int16_t target_climb_rate_cms = 0 ;
2019-01-15 13:46:13 -04:00
if ( plane . control_mode ! = & plane . mode_qstabilize ) {
2021-05-03 11:44:05 -03:00
des_alt_m = pos_control - > get_pos_target_z_cm ( ) / 100.0f ;
target_climb_rate_cms = pos_control - > get_vel_target_z_cms ( ) ;
2018-12-21 03:42:59 -04:00
}
2016-03-24 22:33:19 -03:00
struct log_QControl_Tuning pkt = {
LOG_PACKET_HEADER_INIT ( LOG_QTUN_MSG ) ,
time_us : AP_HAL : : micros64 ( ) ,
2018-12-21 03:42:59 -04:00
throttle_in : attitude_control - > get_throttle_in ( ) ,
2016-03-24 22:33:19 -03:00
angle_boost : attitude_control - > angle_boost ( ) ,
throttle_out : motors - > get_throttle ( ) ,
2018-12-21 03:42:59 -04:00
throttle_hover : motors - > get_throttle_hover ( ) ,
desired_alt : des_alt_m ,
2016-03-24 22:33:19 -03:00
inav_alt : inertial_nav . get_altitude ( ) / 100.0f ,
2018-12-21 03:42:59 -04:00
baro_alt : int32_t ( plane . barometer . get_altitude ( ) * 100 ) ,
target_climb_rate : target_climb_rate_cms ,
climb_rate : int16_t ( inertial_nav . get_velocity_z ( ) ) ,
2017-04-18 09:25:14 -03:00
throttle_mix : attitude_control - > get_throttle_mix ( ) ,
2020-11-01 18:36:55 -04:00
speed_scaler : log_spd_scaler ,
2020-11-16 13:28:09 -04:00
transition_state : static_cast < uint8_t > ( transition_state ) ,
assist : assisted_flight ,
2016-03-24 22:33:19 -03:00
} ;
2019-01-18 00:23:42 -04:00
plane . logger . WriteBlock ( & pkt , sizeof ( pkt ) ) ;
2018-03-01 01:44:44 -04:00
// write multicopter position control message
pos_control - > write_log ( ) ;
2016-03-24 22:33:19 -03:00
}
2016-04-20 02:13:20 -03:00
/*
calculate the forward throttle percentage . The forward throttle can
be used to assist with position hold and with landing approach . It
reduces the need for down pitch which reduces load on the vertical
lift motors .
*/
2020-06-09 19:05:07 -03:00
int8_t QuadPlane : : forward_throttle_pct ( )
2016-04-20 02:13:20 -03:00
{
/*
2020-06-09 19:05:07 -03:00
Unless an RC channel is assigned for manual forward throttle control ,
we don ' t use forward throttle in QHOVER or QSTABILIZE as they are the primary
2016-04-20 02:13:20 -03:00
recovery modes for a quadplane and need to be as simple as
2020-06-09 19:05:07 -03:00
possible . They will drift with the wind .
2016-04-20 02:13:20 -03:00
*/
2020-06-09 19:05:07 -03:00
if ( plane . control_mode = = & plane . mode_qacro | |
2019-01-15 13:46:13 -04:00
plane . control_mode = = & plane . mode_qstabilize | |
2020-06-09 19:05:07 -03:00
plane . control_mode = = & plane . mode_qhover ) {
if ( rc_fwd_thr_ch = = nullptr ) {
return 0 ;
} else {
// calculate fwd throttle demand from manual input
float fwd_thr = rc_fwd_thr_ch - > percent_input ( ) ;
// set forward throttle to fwd_thr_max * (manual input + mix): range [0,100]
fwd_thr * = .01f * constrain_float ( fwd_thr_max , 0 , 100 ) ;
return fwd_thr ;
}
}
/*
in qautotune mode or modes without a velocity controller
*/
if ( vel_forward . gain < = 0 | |
plane . control_mode = = & plane . mode_qautotune ) {
2016-04-20 02:13:20 -03:00
return 0 ;
}
2020-06-09 19:05:07 -03:00
/*
in modes with a velocity controller
*/
2016-06-02 18:57:10 -03:00
float deltat = ( AP_HAL : : millis ( ) - vel_forward . last_ms ) * 0.001f ;
2016-04-20 02:13:20 -03:00
if ( deltat > 1 | | deltat < 0 ) {
vel_forward . integrator = 0 ;
deltat = 0.1 ;
}
if ( deltat < 0.1 ) {
// run at 10Hz
return vel_forward . last_pct ;
}
2016-06-02 18:57:10 -03:00
vel_forward . last_ms = AP_HAL : : millis ( ) ;
2016-04-20 02:13:20 -03:00
// work out the desired speed in forward direction
2021-06-08 03:02:15 -03:00
Vector3f desired_velocity_cms = pos_control - > get_vel_desired_cms ( ) ;
// convert to NED m/s
desired_velocity_cms . z * = - 1 ;
2016-04-20 02:13:20 -03:00
Vector3f vel_ned ;
if ( ! plane . ahrs . get_velocity_NED ( vel_ned ) ) {
// we don't know our velocity? EKF must be pretty sick
vel_forward . last_pct = 0 ;
2016-06-03 19:35:09 -03:00
vel_forward . integrator = 0 ;
2016-04-20 02:13:20 -03:00
return 0 ;
}
2020-06-09 19:05:07 -03:00
// get component of velocity error in fwd body frame direction
2016-04-20 02:13:20 -03:00
Vector3f vel_error_body = ahrs . get_rotation_body_to_ned ( ) . transposed ( ) * ( ( desired_velocity_cms * 0.01f ) - vel_ned ) ;
2020-06-09 19:05:07 -03:00
float fwd_vel_error = vel_error_body . x ;
2016-04-20 02:13:20 -03:00
// scale forward velocity error by maximum airspeed
2016-08-07 21:48:36 -03:00
fwd_vel_error / = MAX ( plane . aparm . airspeed_max , 5 ) ;
2016-04-20 02:13:20 -03:00
// add in a component from our current pitch demand. This tends to
// move us to zero pitch. Assume that LIM_PITCH would give us the
// WP nav speed.
2019-01-24 01:03:12 -04:00
fwd_vel_error - = ( wp_nav - > get_default_speed_xy ( ) * 0.01f ) * plane . nav_pitch_cd / ( float ) plane . aparm . pitch_limit_max_cd ;
2016-04-20 02:13:20 -03:00
if ( should_relax ( ) & & vel_ned . length ( ) < 1 ) {
// we may be landed
fwd_vel_error = 0 ;
vel_forward . integrator * = 0.95f ;
}
// integrator as throttle percentage (-100 to 100)
vel_forward . integrator + = fwd_vel_error * deltat * vel_forward . gain * 100 ;
2016-06-13 18:31:50 -03:00
// inhibit reverse throttle and allow petrol engines with min > 0
2018-11-09 18:38:43 -04:00
int8_t fwd_throttle_min = plane . have_reverse_thrust ( ) ? 0 : plane . aparm . throttle_min ;
2021-11-22 18:15:34 -04:00
vel_forward . integrator = constrain_float ( vel_forward . integrator , fwd_throttle_min , plane . aparm . throttle_cruise ) ;
2019-02-22 20:20:20 -04:00
2019-02-23 22:53:52 -04:00
if ( in_vtol_land_approach ( ) ) {
2019-02-22 20:20:20 -04:00
// when we are doing horizontal positioning in a VTOL land
// we always allow the fwd motor to run. Otherwise a bad
// lidar could cause the aircraft not to be able to
// approach the landing point when landing below the takeoff point
vel_forward . last_pct = vel_forward . integrator ;
2021-02-26 13:43:38 -04:00
} else if ( ( in_vtol_land_final ( ) & & motors - > limit . throttle_lower ) | |
( plane . g . rangefinder_landing & & ( plane . rangefinder . status_orient ( ROTATION_PITCH_270 ) = = RangeFinder : : Status : : OutOfRangeLow ) ) ) {
// we're in the settling phase of landing or using a rangefinder that is out of range low, disable fwd motor
2019-09-11 01:02:49 -03:00
vel_forward . last_pct = 0 ;
vel_forward . integrator = 0 ;
2019-02-22 20:20:20 -04:00
} else {
// If we are below alt_cutoff then scale down the effect until
// it turns off at alt_cutoff and decay the integrator
float alt_cutoff = MAX ( 0 , vel_forward_alt_cutoff ) ;
float height_above_ground = plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ;
vel_forward . last_pct = linear_interpolate ( 0 , vel_forward . integrator ,
height_above_ground , alt_cutoff , alt_cutoff + 2 ) ;
}
2016-06-03 19:35:09 -03:00
if ( vel_forward . last_pct = = 0 ) {
// if the percent is 0 then decay the integrator
vel_forward . integrator * = 0.95f ;
}
2016-04-20 02:13:20 -03:00
return vel_forward . last_pct ;
}
2016-04-20 03:23:17 -03:00
/*
get weathervaning yaw rate in cd / s
*/
float QuadPlane : : get_weathervane_yaw_rate_cds ( void )
{
/*
we only do weathervaning in modes where we are doing VTOL
position control . We also don ' t do it if the pilot has given any
yaw input in the last 3 seconds .
*/
if ( ! in_vtol_mode ( ) | |
! motors - > armed ( ) | |
weathervane . gain < = 0 | |
2019-01-15 13:46:13 -04:00
plane . control_mode = = & plane . mode_qstabilize | |
plane . control_mode = = & plane . mode_qhover | |
plane . control_mode = = & plane . mode_qautotune ) {
2016-04-20 03:23:17 -03:00
weathervane . last_output = 0 ;
return 0 ;
}
2018-08-24 02:42:37 -03:00
const uint32_t tnow = millis ( ) ;
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
if ( plane . channel_rudder - > get_control_in ( ) ! = 0 ) {
2018-08-24 02:42:37 -03:00
weathervane . last_pilot_input_ms = tnow ;
2016-04-20 03:23:17 -03:00
weathervane . last_output = 0 ;
return 0 ;
}
2018-08-24 02:42:37 -03:00
if ( tnow - weathervane . last_pilot_input_ms < 3000 ) {
2016-04-20 03:23:17 -03:00
weathervane . last_output = 0 ;
return 0 ;
}
2016-04-21 08:52:25 -03:00
float roll = wp_nav - > get_roll ( ) / 100.0f ;
if ( fabsf ( roll ) < weathervane . min_roll ) {
weathervane . last_output = 0 ;
return 0 ;
}
if ( roll > 0 ) {
roll - = weathervane . min_roll ;
} else {
roll + = weathervane . min_roll ;
}
float output = constrain_float ( ( roll / 45.0f ) * weathervane . gain , - 1 , 1 ) ;
2016-04-20 03:23:17 -03:00
if ( should_relax ( ) ) {
output = 0 ;
}
weathervane . last_output = 0.98f * weathervane . last_output + 0.02f * output ;
// scale over half of yaw_rate_max. This gives the pilot twice the
// authority of the weathervane controller
return weathervane . last_output * ( yaw_rate_max / 2 ) * 100 ;
}
2016-05-05 22:28:26 -03:00
/*
start guided mode control
*/
void QuadPlane : : guided_start ( void )
{
2017-09-06 04:58:08 -03:00
guided_takeoff = false ;
2016-05-05 22:28:26 -03:00
setup_target_position ( ) ;
2021-04-04 16:37:33 -03:00
int32_t from_alt ;
int32_t to_alt ;
if ( plane . current_loc . get_alt_cm ( Location : : AltFrame : : ABSOLUTE , from_alt ) & & plane . next_WP_loc . get_alt_cm ( Location : : AltFrame : : ABSOLUTE , to_alt ) ) {
poscontrol . slow_descent = from_alt > to_alt ;
2021-06-04 05:56:59 -03:00
} else {
// default back to old method
poscontrol . slow_descent = ( plane . current_loc . alt > plane . next_WP_loc . alt ) ;
2021-04-04 16:37:33 -03:00
}
2021-05-18 20:20:48 -03:00
poscontrol_init_approach ( ) ;
2016-05-05 22:28:26 -03:00
}
/*
update guided mode control
*/
void QuadPlane : : guided_update ( void )
{
2019-01-15 13:46:13 -04:00
if ( plane . control_mode = = & plane . mode_guided & & guided_takeoff & & plane . current_loc . alt < plane . next_WP_loc . alt ) {
2017-09-06 04:58:08 -03:00
throttle_wait = false ;
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2017-09-06 04:58:08 -03:00
takeoff_controller ( ) ;
} else {
2021-09-15 23:53:32 -03:00
if ( guided_takeoff ) {
poscontrol . set_state ( QPOS_POSITION2 ) ;
}
2017-09-06 04:58:08 -03:00
guided_takeoff = false ;
// run VTOL position controller
vtol_position_controller ( ) ;
}
2016-05-05 22:28:26 -03:00
}
2016-09-26 22:46:51 -03:00
void QuadPlane : : afs_terminate ( void )
{
if ( available ( ) ) {
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : SHUT_DOWN ) ;
2016-09-26 22:46:51 -03:00
motors - > output ( ) ;
}
}
2016-09-30 19:35:58 -03:00
/*
return true if we should do guided mode loitering using VTOL motors
*/
bool QuadPlane : : guided_mode_enabled ( void )
{
if ( ! available ( ) ) {
return false ;
}
// only use quadplane guided when in AUTO or GUIDED mode
2019-01-15 13:46:13 -04:00
if ( plane . control_mode ! = & plane . mode_guided & & plane . control_mode ! = & plane . mode_auto ) {
2016-09-30 19:35:58 -03:00
return false ;
}
2020-02-10 23:36:02 -04:00
if ( plane . control_mode = = & plane . mode_auto & &
plane . mission . get_current_nav_cmd ( ) . id = = MAV_CMD_NAV_LOITER_TURNS ) {
// loiter turns is a fixed wing only operation
return false ;
}
2016-09-30 19:35:58 -03:00
return guided_mode ! = 0 ;
}
2017-09-05 19:22:38 -03:00
/*
set altitude target to current altitude
*/
void QuadPlane : : set_alt_target_current ( void )
{
2021-05-03 11:44:05 -03:00
pos_control - > set_pos_target_z_cm ( inertial_nav . get_altitude ( ) ) ;
2017-09-05 19:22:38 -03:00
}
2017-09-06 04:58:08 -03:00
// user initiated takeoff for guided mode
bool QuadPlane : : do_user_takeoff ( float takeoff_altitude )
{
2019-01-15 13:46:13 -04:00
if ( plane . control_mode ! = & plane . mode_guided ) {
2017-09-06 04:58:08 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " User Takeoff only in GUIDED mode " ) ;
return false ;
}
if ( ! hal . util - > get_soft_armed ( ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Must be armed for takeoff " ) ;
return false ;
}
if ( is_flying ( ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Already flying - no takeoff " ) ;
return false ;
}
plane . auto_state . vtol_loiter = true ;
plane . prev_WP_loc = plane . current_loc ;
plane . next_WP_loc = plane . current_loc ;
plane . next_WP_loc . alt + = takeoff_altitude * 100 ;
2021-05-18 20:20:48 -03:00
set_desired_spool_state ( AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED ) ;
2017-09-06 04:58:08 -03:00
guided_start ( ) ;
guided_takeoff = true ;
2020-09-22 18:05:38 -03:00
if ( ( options & OPTION_DISABLE_GROUND_EFFECT_COMP ) = = 0 ) {
2021-05-28 20:54:15 -03:00
ahrs . set_takeoff_expected ( true ) ;
2020-09-22 18:05:38 -03:00
}
2017-09-11 16:08:29 -03:00
return true ;
2017-09-06 04:58:08 -03:00
}
2017-10-20 01:39:44 -03:00
2018-07-25 18:32:42 -03:00
// return true if the wp_nav controller is being updated
bool QuadPlane : : using_wp_nav ( void ) const
{
2021-05-19 23:58:58 -03:00
if ( plane . control_mode = = & plane . mode_qloiter | | plane . control_mode = = & plane . mode_qland ) {
return true ;
}
if ( plane . control_mode = = & plane . mode_qrtl & & poscontrol . get_state ( ) > = QPOS_POSITION2 ) {
return true ;
}
return false ;
2018-07-25 18:32:42 -03:00
}
2017-10-20 01:39:44 -03:00
/*
return mav_type for heartbeat
*/
2018-03-22 06:16:35 -03:00
MAV_TYPE QuadPlane : : get_mav_type ( void ) const
2017-10-20 01:39:44 -03:00
{
if ( mav_type . get ( ) = = 0 ) {
return MAV_TYPE_FIXED_WING ;
}
2018-03-22 06:16:35 -03:00
return MAV_TYPE ( mav_type . get ( ) ) ;
2017-10-20 01:39:44 -03:00
}
2017-10-29 03:31:09 -03:00
/*
return true if current mission item is a vtol takeoff
*/
bool QuadPlane : : is_vtol_takeoff ( uint16_t id ) const
{
if ( id = = MAV_CMD_NAV_VTOL_TAKEOFF ) {
return true ;
}
if ( id = = MAV_CMD_NAV_TAKEOFF & & available ( ) & & ( options & OPTION_ALLOW_FW_TAKEOFF ) = = 0 ) {
// treat fixed wing takeoff as VTOL takeoff
return true ;
}
return false ;
}
/*
return true if current mission item is a vtol land
*/
bool QuadPlane : : is_vtol_land ( uint16_t id ) const
{
if ( id = = MAV_CMD_NAV_VTOL_LAND ) {
2018-09-25 01:11:26 -03:00
if ( options & QuadPlane : : OPTION_MISSION_LAND_FW_APPROACH ) {
return plane . vtol_approach_s . approach_stage = = Plane : : Landing_ApproachStage : : VTOL_LANDING ;
} else {
return true ;
}
2017-10-29 03:31:09 -03:00
}
if ( id = = MAV_CMD_NAV_LAND & & available ( ) & & ( options & OPTION_ALLOW_FW_LAND ) = = 0 ) {
// treat fixed wing land as VTOL land
return true ;
}
return false ;
}
/*
return true if we are in a transition to fwd flight from hover
*/
bool QuadPlane : : in_transition ( void ) const
{
return available ( ) & & assisted_flight & &
( transition_state = = TRANSITION_AIRSPEED_WAIT | |
transition_state = = TRANSITION_TIMER ) ;
}
2017-11-05 05:44:42 -04:00
/*
calculate current stopping distance for a quadplane in fixed wing flight
*/
2021-10-28 17:36:09 -03:00
float QuadPlane : : stopping_distance ( float ground_speed_squared )
2017-11-05 05:44:42 -04:00
{
// use v^2/(2*accel). This is only quite approximate as the drag
// varies with pitch, but it gives something for the user to
// control the transition distance in a reasonable way
2021-10-28 17:36:09 -03:00
return ground_speed_squared / ( 2 * transition_decel ) ;
}
/*
calculate current stopping distance for a quadplane in fixed wing flight
*/
float QuadPlane : : stopping_distance ( void )
{
return stopping_distance ( plane . ahrs . groundspeed_vector ( ) . length_squared ( ) ) ;
}
/*
distance below which we don ' t do approach , based on stopping
distance for cruise speed
*/
float QuadPlane : : transition_threshold ( void )
{
// 1.5 times stopping distance for cruise speed
return 1.5 * stopping_distance ( sq ( plane . aparm . airspeed_cruise_cm * 0.01 ) ) ;
2017-11-05 05:44:42 -04:00
}
2019-01-26 19:30:53 -04:00
# define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing
# define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing
# define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity
2020-02-21 02:51:36 -04:00
void QuadPlane : : update_throttle_mix ( void )
2019-01-26 19:30:53 -04:00
{
2020-03-04 02:05:13 -04:00
// update filtered acceleration
Vector3f accel_ef = ahrs . get_accel_ef_blended ( ) ;
accel_ef . z + = GRAVITY_MSS ;
throttle_mix_accel_ef_filter . apply ( accel_ef , plane . scheduler . get_loop_period_s ( ) ) ;
2019-01-26 19:30:53 -04:00
// transition will directly manage the mix
if ( in_transition ( ) ) {
return ;
}
// if disarmed or landed prioritise throttle
2020-02-21 02:51:36 -04:00
if ( ! motors - > armed ( ) ) {
2019-01-26 19:30:53 -04:00
attitude_control - > set_throttle_mix_min ( ) ;
return ;
}
2020-07-07 21:22:56 -03:00
if ( plane . control_mode - > is_vtol_man_throttle ( ) ) {
2019-01-26 19:30:53 -04:00
// manual throttle
2020-07-07 21:22:56 -03:00
if ( ( plane . get_throttle_input ( ) < = 0 ) & & ( air_mode = = AirMode : : OFF ) ) {
2019-01-26 19:30:53 -04:00
attitude_control - > set_throttle_mix_min ( ) ;
} else {
attitude_control - > set_throttle_mix_man ( ) ;
}
} else {
// autopilot controlled throttle
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
const Vector3f angle_target = attitude_control - > get_att_target_euler_cd ( ) ;
bool large_angle_request = ( norm ( angle_target . x , angle_target . y ) > LAND_CHECK_LARGE_ANGLE_CD ) ;
// check for large external disturbance - angle error over 30 degrees
const float angle_error = attitude_control - > get_att_error_angle_deg ( ) ;
bool large_angle_error = ( angle_error > LAND_CHECK_ANGLE_ERROR_DEG ) ;
// check for large acceleration - falling or high turbulence
2020-03-04 02:05:13 -04:00
bool accel_moving = ( throttle_mix_accel_ef_filter . get ( ) . length ( ) > LAND_CHECK_ACCEL_MOVING ) ;
2019-01-26 19:30:53 -04:00
2021-05-19 11:08:14 -03:00
// check for requested descent
2021-05-03 11:44:05 -03:00
bool descent_not_demanded = pos_control - > get_vel_desired_cms ( ) . z > = 0.0f ;
2019-01-26 19:30:53 -04:00
2020-02-21 02:51:36 -04:00
if ( large_angle_request | | large_angle_error | | accel_moving | | descent_not_demanded ) {
2019-06-17 12:58:17 -03:00
attitude_control - > set_throttle_mix_max ( 1.0 ) ;
2019-01-26 19:30:53 -04:00
} else {
attitude_control - > set_throttle_mix_min ( ) ;
}
}
}
2019-02-23 22:53:52 -04:00
/*
see if we are in the approach phase of a VTOL landing
*/
bool QuadPlane : : in_vtol_land_approach ( void ) const
{
2021-05-18 20:20:48 -03:00
if ( plane . control_mode = = & plane . mode_qrtl & &
poscontrol . get_state ( ) < = QPOS_POSITION2 ) {
2019-02-23 22:53:52 -04:00
return true ;
}
2021-05-18 20:20:48 -03:00
if ( in_vtol_auto ( ) ) {
if ( is_vtol_land ( plane . mission . get_current_nav_cmd ( ) . id ) & &
( poscontrol . get_state ( ) = = QPOS_APPROACH | |
poscontrol . get_state ( ) = = QPOS_AIRBRAKE | |
poscontrol . get_state ( ) = = QPOS_POSITION1 | |
poscontrol . get_state ( ) = = QPOS_POSITION2 ) ) {
return true ;
}
}
2019-02-23 22:53:52 -04:00
return false ;
}
/*
see if we are in the descent phase of a VTOL landing
*/
bool QuadPlane : : in_vtol_land_descent ( void ) const
{
if ( in_vtol_auto ( ) & & is_vtol_land ( plane . mission . get_current_nav_cmd ( ) . id ) & &
2021-05-18 02:15:46 -03:00
( poscontrol . get_state ( ) = = QPOS_LAND_DESCEND | | poscontrol . get_state ( ) = = QPOS_LAND_FINAL ) ) {
2019-02-23 22:53:52 -04:00
return true ;
}
return false ;
}
2019-09-11 04:20:04 -03:00
/*
see if we are in the final phase of a VTOL landing
*/
bool QuadPlane : : in_vtol_land_final ( void ) const
{
2021-05-18 02:15:46 -03:00
return in_vtol_land_descent ( ) & & poscontrol . get_state ( ) = = QPOS_LAND_FINAL ;
2019-09-11 04:20:04 -03:00
}
2019-12-27 17:13:27 -04:00
/*
see if we are in any of the phases of a VTOL landing
*/
bool QuadPlane : : in_vtol_land_sequence ( void ) const
{
return in_vtol_land_approach ( ) | | in_vtol_land_descent ( ) | | in_vtol_land_final ( ) ;
}
2020-09-25 18:16:38 -03:00
2021-06-08 03:00:54 -03:00
/*
see if we are in the VTOL position control phase of a landing
*/
bool QuadPlane : : in_vtol_land_poscontrol ( void ) const
{
if ( in_vtol_auto ( ) & & is_vtol_land ( plane . mission . get_current_nav_cmd ( ) . id ) & &
poscontrol . get_state ( ) > = QPOS_POSITION1 ) {
return true ;
}
return false ;
}
2020-09-25 18:16:38 -03:00
// return true if we should show VTOL view
bool QuadPlane : : show_vtol_view ( ) const
{
bool show_vtol = in_vtol_mode ( ) ;
2021-03-26 21:00:11 -03:00
if ( is_tailsitter ( ) ) {
2020-09-25 18:16:38 -03:00
if ( show_vtol & & ( transition_state = = TRANSITION_ANGLE_WAIT_VTOL ) ) {
// in a vtol mode but still transitioning from forward flight
return false ;
}
if ( ! show_vtol & & ( transition_state = = TRANSITION_ANGLE_WAIT_FW ) ) {
// not in VTOL mode but still transitioning from VTOL
return true ;
}
}
2020-12-15 00:03:42 -04:00
if ( ! show_vtol & & tilt . is_vectored & & transition_state < = TRANSITION_TIMER ) {
// we use multirotor controls during fwd transition for
// vectored yaw vehicles
return true ;
}
2020-09-25 18:16:38 -03:00
return show_vtol ;
}
2020-11-06 12:20:02 -04:00
2021-03-16 18:54:03 -03:00
// return the PILOT_VELZ_MAX_DN value if non zero, otherwise returns the PILOT_VELZ_MAX value.
uint16_t QuadPlane : : get_pilot_velocity_z_max_dn ( ) const
{
if ( pilot_velocity_z_max_dn = = 0 ) {
return abs ( pilot_velocity_z_max_up ) ;
}
return abs ( pilot_velocity_z_max_dn ) ;
}
2021-05-18 00:44:57 -03:00
/*
should we use the fixed wing attitude controllers for roll / pitch control
*/
bool QuadPlane : : use_fw_attitude_controllers ( void ) const
{
if ( available ( ) & &
motors - > armed ( ) & &
motors - > get_desired_spool_state ( ) > = AP_Motors : : DesiredSpoolState : : THROTTLE_UNLIMITED & &
in_vtol_mode ( ) & &
2021-06-18 22:55:06 -03:00
! is_tailsitter ( ) & &
poscontrol . get_state ( ) ! = QPOS_AIRBRAKE ) {
2021-05-18 00:44:57 -03:00
// we want the desired rates for fixed wing slaved to the
// multicopter rates
return false ;
}
return true ;
}
2021-05-18 20:20:48 -03:00
/*
calculate our closing velocity vector on the landing point . In the
future this will take account of the landing point having a
velocity
*/
Vector2f QuadPlane : : landing_closing_velocity ( )
{
Vector2f vel = ahrs . groundspeed_vector ( ) ;
return vel ;
}
/*
calculate our desired closing velocity vector on the landing point .
*/
Vector2f QuadPlane : : landing_desired_closing_velocity ( )
{
if ( poscontrol . get_state ( ) > = QPOS_LAND_DESCEND ) {
return Vector2f ( 0 , 0 ) ;
}
const Vector2f diff_wp = plane . current_loc . get_distance_NE ( plane . next_WP_loc ) ;
float dist = diff_wp . length ( ) ;
if ( dist < 1 ) {
return Vector2f ( 0 , 0 ) ;
}
// base target speed based on sqrt of distance
float target_speed = safe_sqrt ( 2 * transition_decel * dist ) ;
Vector2f target_speed_xy = diff_wp . normalized ( ) * target_speed ;
return target_speed_xy ;
}
/*
get target airspeed for landing , for use by TECS
*/
float QuadPlane : : get_land_airspeed ( void )
{
if ( poscontrol . get_state ( ) = = QPOS_APPROACH | |
plane . control_mode = = & plane . mode_rtl ) {
float land_airspeed = plane . SpdHgt_Controller - > get_land_airspeed ( ) ;
if ( ! is_positive ( land_airspeed ) ) {
land_airspeed = plane . aparm . airspeed_cruise_cm * 0.01 ;
}
float cruise_airspeed = plane . aparm . airspeed_cruise_cm * 0.01 ;
float time_to_landing = plane . auto_state . wp_distance / MAX ( land_airspeed , 5 ) ;
/*
slow down to landing approach speed as we get closer to landing
*/
land_airspeed = linear_interpolate ( land_airspeed , cruise_airspeed ,
time_to_landing ,
20 , 60 ) ;
return land_airspeed ;
}
Vector2f vel = landing_desired_closing_velocity ( ) ;
const float eas2tas = plane . ahrs . get_EAS2TAS ( ) ;
const Vector3f wind = plane . ahrs . wind_estimate ( ) ;
vel . x - = wind . x ;
vel . y - = wind . y ;
vel / = eas2tas ;
return vel . length ( ) ;
}
void QuadPlane : : set_desired_spool_state ( AP_Motors : : DesiredSpoolState state )
{
if ( motors - > get_desired_spool_state ( ) ! = state ) {
motors - > set_desired_spool_state ( state ) ;
}
}
2020-11-06 12:20:02 -04:00
QuadPlane * QuadPlane : : _singleton = nullptr ;