2015-11-24 04:24:04 -04:00
# include "Plane.h"
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
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
2015-12-26 03:45:42 -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
// @DisplayName: Pilot maximum vertical speed
// @Description: The maximum 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
2015-12-26 06:40:40 -04:00
AP_GROUPINFO ( " VELZ_MAX " , 18 , QuadPlane , pilot_velocity_z_max , 250 ) ,
2015-11-24 04:24:04 -04:00
2015-12-26 03:45:42 -04:00
// @Param: ACCEL_Z
// @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 ) ,
// @Param: THR_MIN_PWM
// @DisplayName: Minimum PWM output
// @Description: This is the minimum PWM output for the quad motors
2017-09-18 15:05:45 -03:00
// @Units: PWM
2015-12-26 06:40:40 -04:00
// @Range: 800 2200
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " THR_MIN_PWM " , 22 , QuadPlane , thr_min_pwm , 1000 ) ,
// @Param: THR_MAX_PWM
// @DisplayName: Maximum PWM output
// @Description: This is the maximum PWM output for the quad motors
2017-09-18 15:05:45 -03:00
// @Units: PWM
2015-12-26 06:40:40 -04:00
// @Range: 800 2200
// @Increment: 1
// @User: Standard
2016-01-01 20:56:31 -04:00
AP_GROUPINFO ( " THR_MAX_PWM " , 23 , QuadPlane , thr_max_pwm , 2000 ) ,
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
2017-06-10 05:41:22 -03:00
// @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 7:Tri, 10: TailSitter
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
2017-10-05 21:36:34 -03:00
// @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F
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
2017-10-30 02:51:28 -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
2016-04-29 03:31:22 -03:00
// @Values: 0:Disabled,1:Enabled
// @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
// @Description: This is used to calibrate the throttle range of the VTOL motors. Please read http://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.
// @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.
// @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
// @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 1 second. 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
2017-04-22 22:35: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
// @Values: 0:Continuous,1:Binary,2:VectoredYaw
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
2017-02-11 04:12:56 -04:00
// @DisplayName: Tailsitter transition angle
// @Description: This is the angle at which tailsitter aircraft will change from VTOL control to fixed wing control.
// @Range: 5 80
2017-11-05 02:25:00 -04:00
AP_GROUPINFO ( " TAILSIT_ANGLE " , 48 , QuadPlane , tailsitter . transition_angle , 45 ) ,
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
// @DisplayName: Tailsitter input type
// @Description: This controls whether stick input when hovering as a tailsitter follows the conventions for fixed wing hovering or multicopter hovering. When multicopter input is selected the roll stick will roll the aircraft in earth frame and yaw stick will yaw in earth frame. When using fixed wing input the roll and yaw sticks will control the aircraft in body frame.
// @Values: 0:MultiCopterInput,1:FixedWingInput
AP_GROUPINFO ( " TAILSIT_INPUT " , 50 , QuadPlane , tailsitter . input_type , TAILSITTER_INPUT_MULTICOPTER ) ,
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
// @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.
// @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
2017-10-31 19:32:10 -03:00
// @Description: This provides a set of additional control options for quadplanes. LevelTransition means that the wings should be held level to within LEVEL_ROLL_LIMIT degrees during transition to fixed wing flight. If AllowFWTakeoff bit is not set then fixed wing takeoff on quadplanes will instead perform a VTOL takeoff. If AllowFWLand bit is not set then fixed wing land on quadplanes will instead perform a VTOL land.
2017-10-29 03:31:09 -03:00
// @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand
AP_GROUPINFO ( " OPTIONS " , 58 , QuadPlane , options , 0 ) ,
2017-11-05 05:44:42 -04:00
AP_SUBGROUPEXTENSION ( " " , 59 , QuadPlane , var_info2 ) ,
2017-02-24 03:02:34 -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 ) ,
2018-04-15 08:31:53 -03:00
// @Param: TAILSIT_THSCMX
// @DisplayName: Maximum control throttle scaling value
// @Description: Maximum value of throttle scaling for tailsitter velocity scaling, reduce this value to remove low thorottle D ossilaitons
// @Range: 1 5
// @User: Standard
AP_GROUPINFO ( " TAILSIT_THSCMX " , 3 , QuadPlane , tailsitter . throttle_scale_max , 5 ) ,
2017-11-05 05:44:42 -04:00
AP_GROUPEND
} ;
2017-04-03 19:18:30 -03:00
struct defaults_struct {
2016-04-01 02:40:06 -03:00
const char * name ;
float value ;
2017-04-03 19:18:30 -03:00
} ;
/*
defaults for all quadplanes
*/
static const struct defaults_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 } ,
{ " Q_A_RAT_RLL_FILT " , 10.0 } ,
{ " Q_A_RAT_PIT_P " , 0.25 } ,
{ " Q_A_RAT_PIT_I " , 0.25 } ,
{ " Q_A_RAT_PIT_FILT " , 10.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 } ,
2016-04-01 02:40:06 -03:00
} ;
2017-04-03 19:18:30 -03:00
/*
extra defaults for tailsitters
*/
static const struct defaults_struct defaults_table_tailsitter [ ] = {
{ " KFF_RDDRMIX " , 0.02 } ,
{ " Q_A_RAT_PIT_FF " , 0.2 } ,
{ " Q_A_RAT_YAW_FF " , 0.2 } ,
{ " Q_A_RAT_YAW_I " , 0.18 } ,
{ " 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 } ,
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
{ Parameters : : k_param_quadplane , 206 , AP_PARAM_FLOAT , " Q_P_VELXY_FILT " } , // Q_VXY_FILT_HZ
{ 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
{ Parameters : : k_param_quadplane , 400 , AP_PARAM_FLOAT , " Q_P_ACCZ_FILT " } , // Q_AZ_FILT
{ 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
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 ) ;
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-01-09 04:19:42 -04:00
enum AP_Motors : : motor_frame_class motor_class ;
2017-02-11 04:12:56 -04:00
enum Rotation rotation = ROTATION_NONE ;
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 ( ) <
4096 + sizeof ( * motors ) + sizeof ( * attitude_control ) + sizeof ( * pos_control ) + sizeof ( * wp_nav ) ) {
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Not enough memory for quadplane " ) ;
2016-01-06 03:17:08 -04:00
goto failed ;
}
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
*/
2017-01-09 04:19:42 -04:00
motor_class = ( enum AP_Motors : : motor_frame_class ) frame_class . get ( ) ;
switch ( motor_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 :
break ;
2016-02-07 20:00:19 -04:00
default :
2017-03-18 08:10:20 -03:00
hal . console - > printf ( " Unknown frame class %u - using QUAD \n " , ( unsigned ) frame_class . get ( ) ) ;
frame_class . set ( AP_Motors : : MOTOR_FRAME_QUAD ) ;
setup_default_channels ( 4 ) ;
break ;
2016-02-07 20:00:19 -04:00
}
2017-02-11 01:50:03 -04:00
switch ( motor_class ) {
case AP_Motors : : MOTOR_FRAME_TRI :
2018-07-28 00:43:26 -03:00
motors = new AP_MotorsTri ( plane . scheduler . get_loop_rate_hz ( ) , rc_speed ) ;
2017-02-27 02:39:46 -04:00
motors_var_info = AP_MotorsTri : : var_info ;
2017-02-11 01:50:03 -04:00
break ;
case AP_Motors : : MOTOR_FRAME_TAILSITTER :
2018-07-28 00:43:26 -03:00
motors = new AP_MotorsTailsitter ( plane . scheduler . get_loop_rate_hz ( ) , rc_speed ) ;
2017-02-27 02:39:46 -04:00
motors_var_info = AP_MotorsTailsitter : : var_info ;
2017-02-11 04:12:56 -04:00
rotation = ROTATION_PITCH_90 ;
2017-02-11 01:50:03 -04:00
break ;
default :
2018-07-28 00:43:26 -03:00
motors = new AP_MotorsMatrix ( plane . scheduler . get_loop_rate_hz ( ) , rc_speed ) ;
2017-02-27 02:39:46 -04:00
motors_var_info = AP_MotorsMatrix : : var_info ;
2017-02-11 01:50:03 -04:00
break ;
2017-01-09 04:19:42 -04:00
}
2016-10-06 09:47:33 -03:00
const static char * strUnableToAllocate = " Unable to allocate " ;
2015-12-26 06:40:40 -04:00
if ( ! motors ) {
2016-10-06 09:47:33 -03:00
hal . console - > printf ( " %s motors \n " , strUnableToAllocate ) ;
2016-01-06 03:17:08 -04:00
goto failed ;
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
ahrs_view = ahrs . create_view ( rotation ) ;
if ( ahrs_view = = nullptr ) {
goto failed ;
}
attitude_control = new AC_AttitudeControl_Multi ( * ahrs_view , aparm , * motors , loop_delta_t ) ;
2015-12-26 06:40:40 -04:00
if ( ! attitude_control ) {
2016-10-06 09:47:33 -03:00
hal . console - > printf ( " %s attitude_control \n " , strUnableToAllocate ) ;
2016-01-06 03:17:08 -04:00
goto failed ;
2015-12-26 06:40:40 -04:00
}
AP_Param : : load_object_from_eeprom ( attitude_control , attitude_control - > var_info ) ;
2018-01-15 07:59:09 -04:00
pos_control = new AC_PosControl ( * ahrs_view , inertial_nav , * motors , * attitude_control ) ;
2015-12-26 06:40:40 -04:00
if ( ! pos_control ) {
2016-10-06 09:47:33 -03:00
hal . console - > printf ( " %s pos_control \n " , strUnableToAllocate ) ;
2016-01-06 03:17:08 -04:00
goto failed ;
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 ) {
2016-10-06 09:47:33 -03:00
hal . console - > printf ( " %s wp_nav \n " , strUnableToAllocate ) ;
2016-01-06 03:17:08 -04:00
goto failed ;
2015-12-26 06:40:40 -04:00
}
AP_Param : : load_object_from_eeprom ( wp_nav , wp_nav - > var_info ) ;
2018-03-27 23:24:05 -03:00
loiter_nav = new AC_Loiter ( inertial_nav , * ahrs_view , * pos_control , * attitude_control ) ;
if ( ! loiter_nav ) {
hal . console - > printf ( " %s loiter_nav \n " , strUnableToAllocate ) ;
goto failed ;
}
AP_Param : : load_object_from_eeprom ( loiter_nav , loiter_nav - > var_info ) ;
2016-12-12 09:08:16 -04:00
motors - > init ( ( AP_Motors : : motor_frame_class ) frame_class . get ( ) , ( AP_Motors : : motor_frame_type ) frame_type . get ( ) ) ;
2016-06-09 10:19:18 -03:00
motors - > set_throttle_range ( thr_min_pwm , thr_max_pwm ) ;
2015-12-26 06:40:40 -04:00
motors - > set_update_rate ( rc_speed ) ;
motors - > set_interlock ( true ) ;
2016-04-02 17:49:38 -03:00
pos_control - > set_dt ( loop_delta_t ) ;
2016-09-01 08:32:24 -03:00
attitude_control - > parameter_sanity_check ( ) ;
2015-12-26 06:40:40 -04:00
2016-01-01 22:29:05 -04:00
// setup the trim of any motors used by AP_Motors so px4io
// 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 ) ;
2016-10-22 07:27:57 -03:00
SRV_Channels : : set_failsafe_pwm ( func , thr_min_pwm ) ;
2016-01-01 22:29:05 -04:00
}
2016-09-10 19:19:22 -03:00
# if HAVE_PX4_MIXER
2016-01-01 22:29:05 -04:00
// redo failsafe mixing on px4
plane . setup_failsafe_mixing ( ) ;
# endif
2015-12-26 06:40:40 -04:00
transition_state = TRANSITION_DONE ;
2017-04-22 22:35:25 -03:00
if ( tilt . tilt_mask ! = 0 & & 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 ) ;
}
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 ) ) ;
2016-04-01 02:40:06 -03:00
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " QuadPlane initialised " ) ;
2016-01-06 03:17:08 -04:00
initialised = true ;
return true ;
failed :
initialised = false ;
enable . set ( 0 ) ;
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " QuadPlane setup failed " ) ;
2016-01-06 03:17:08 -04:00
return false ;
2015-11-24 04:24:04 -04:00
}
2016-04-01 02:40:06 -03:00
/*
2017-04-03 19:18:30 -03:00
setup default parameters from a defaults_struct table
2016-04-01 02:40:06 -03:00
*/
2017-04-03 19:18:30 -03:00
void QuadPlane : : setup_defaults_table ( const struct defaults_struct * table , uint8_t count )
2016-04-01 02:40:06 -03:00
{
2017-04-03 19:18:30 -03:00
for ( uint8_t i = 0 ; i < count ; i + + ) {
if ( ! AP_Param : : set_default_by_name ( table [ i ] . name , table [ i ] . value ) ) {
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " QuadPlane setup failure for %s " ,
2017-04-03 19:18:30 -03:00
table [ i ] . name ) ;
AP_HAL : : panic ( " quadplane bad default %s " , table [ i ] . name ) ;
2016-04-01 02:40:06 -03:00
}
}
2017-04-03 19:18:30 -03:00
}
2016-06-02 00:10:39 -03:00
2017-04-03 19:18:30 -03:00
/*
setup default parameters from defaults_table
*/
void QuadPlane : : setup_defaults ( void )
{
setup_defaults_table ( defaults_table , ARRAY_SIZE ( defaults_table ) ) ;
enum AP_Motors : : motor_frame_class motor_class ;
motor_class = ( enum AP_Motors : : motor_frame_class ) frame_class . get ( ) ;
if ( motor_class = = AP_Motors : : MOTOR_FRAME_TAILSITTER ) {
setup_defaults_table ( defaults_table_tailsitter , ARRAY_SIZE ( defaults_table_tailsitter ) ) ;
}
2016-06-02 00:10:39 -03:00
// reset ESC calibration
if ( esc_calibration ! = 0 ) {
esc_calibration . set_and_save ( 0 ) ;
}
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
motors - > set_throttle_passthrough_for_esc_calibration ( plane . channel_throttle - > get_control_in ( ) * 0.01f ) ;
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
}
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 ( ) ;
2017-03-13 04:45:46 -03:00
if ( in_vtol_mode ( ) | | is_tailsitter ( ) ) {
// use euler angle attitude control
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
yaw_rate_cds ) ;
2017-03-13 04:45:46 -03:00
} else {
// use the fixed wing desired rates
float roll_rate = plane . rollController . get_pid_info ( ) . desired ;
float pitch_rate = plane . pitchController . get_pid_info ( ) . desired ;
2018-03-09 05:48:55 -04:00
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
2016-01-05 19:32:25 -04:00
if ( throttle_in < = 0 ) {
2016-02-04 00:03:39 -04:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
2017-04-13 23:48:08 -03:00
if ( is_tailsitter ( ) ) {
// always stabilize with tailsitters so we can do belly takeoffs
attitude_control - > set_throttle_out ( 0 , true , 0 ) ;
} else {
attitude_control - > set_throttle_out_unstabilized ( 0 , true , 0 ) ;
}
2016-01-05 19:32:25 -04:00
} else {
2016-02-04 00:03:39 -04:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2016-01-05 19:32:25 -04:00
attitude_control - > set_throttle_out ( throttle_in , true , 0 ) ;
}
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
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
float pilot_throttle_scaled = plane . channel_throttle - > get_control_in ( ) / 100.0f ;
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 ( ) ;
2017-03-13 04:04:09 -03:00
if ( now - last_pidz_active_ms > 2000 ) {
2017-04-07 22:49:18 -03:00
// set alt target to current height on transition. This
// starts the Z controller off with the right values
2017-09-18 15:21:01 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Reset alt target to %.1f " , ( double ) inertial_nav . get_altitude ( ) / 100 ) ;
2017-09-05 19:22:38 -03:00
set_alt_target_current ( ) ;
2017-04-07 22:49:18 -03:00
pos_control - > set_desired_velocity_z ( inertial_nav . get_velocity_z ( ) ) ;
// initialize vertical speeds and leash lengths
pos_control - > set_speed_z ( - pilot_velocity_z_max , pilot_velocity_z_max ) ;
pos_control - > set_accel_z ( pilot_accel_z ) ;
2017-03-13 04:04:09 -03:00
// it has been two seconds since we last ran the Z
// controller. We need to assume the integrator may be way off
2017-04-05 07:00:29 -03:00
// the base throttle we start at is the current throttle we are using
2017-04-23 23:05:24 -03:00
float base_throttle = constrain_float ( motors - > get_throttle ( ) - motors - > get_throttle_hover ( ) , - 1 , 1 ) * 1000 ;
2018-01-15 07:59:09 -04:00
pos_control - > get_accel_z_pid ( ) . set_integrator ( base_throttle ) ;
2017-04-08 01:39:55 -03:00
last_pidz_init_ms = now ;
2017-03-13 04:04:09 -03:00
}
last_pidz_active_ms = now ;
pos_control - > update_z_controller ( ) ;
}
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 ) {
2018-09-13 21:02:18 -03:00
attitude_control - > relax_attitude_controllers ( ) ;
}
last_att_control_ms = now ;
}
2015-12-26 03:45:42 -04:00
// init quadplane hover mode
void QuadPlane : : init_hover ( void )
{
// initialize vertical speeds and leash lengths
2015-12-26 06:40:40 -04:00
pos_control - > set_speed_z ( - pilot_velocity_z_max , pilot_velocity_z_max ) ;
pos_control - > set_accel_z ( pilot_accel_z ) ;
2015-12-26 03:45:42 -04:00
// initialise position and desired velocity
2017-09-05 19:22:38 -03:00
set_alt_target_current ( ) ;
2015-12-26 06:40:40 -04:00
pos_control - > set_desired_velocity_z ( inertial_nav . get_velocity_z ( ) ) ;
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 ;
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF yaw reset %.2f " , ( double ) degrees ( yaw_angle_change_rad ) ) ;
2017-04-18 09:40:06 -03:00
}
}
2015-12-26 04:27:13 -04:00
/*
hold hover with target climb rate
*/
void QuadPlane : : hold_hover ( float target_climb_rate )
2015-12-26 03:45:42 -04:00
{
2016-02-04 00:03:39 -04:00
// motors use full range
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2015-12-26 03:45:42 -04:00
// initialize vertical speeds and acceleration
2015-12-26 06:40:40 -04:00
pos_control - > set_speed_z ( - pilot_velocity_z_max , pilot_velocity_z_max ) ;
pos_control - > set_accel_z ( 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
2015-12-26 06:40:40 -04:00
pos_control - > set_alt_target_from_climb_rate_ff ( target_climb_rate , plane . G_Dt , false ) ;
2017-03-13 04:04:09 -03:00
run_z_controller ( ) ;
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 ) {
2016-02-04 00:03:39 -04:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
2015-12-26 06:40:40 -04:00
attitude_control - > set_throttle_out_unstabilized ( 0 , true , 0 ) ;
pos_control - > relax_alt_hold_controllers ( 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-19 00:22:42 -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
// initialize vertical speed and acceleration
2015-12-26 06:40:40 -04:00
pos_control - > set_speed_z ( - pilot_velocity_z_max , pilot_velocity_z_max ) ;
pos_control - > set_accel_z ( pilot_accel_z ) ;
2015-12-26 04:51:05 -04:00
// initialise position and desired velocity
2017-09-05 19:22:38 -03:00
set_alt_target_current ( ) ;
2015-12-26 06:40:40 -04:00
pos_control - > set_desired_velocity_z ( inertial_nav . get_velocity_z ( ) ) ;
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 ;
2016-05-05 19:27:47 -03:00
poscontrol . state = QPOS_LAND_DESCEND ;
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 ;
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 ;
}
2017-09-06 04:58:08 -03:00
if ( plane . control_mode = = GUIDED & & guided_takeoff ) {
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 )
{
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 ;
}
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 ;
2015-12-26 06:40:40 -04:00
}
2016-06-16 05:17:46 -03:00
if ( motor_at_lower_limit & & landing_detect . lower_limit_start_ms = = 0 ) {
landing_detect . lower_limit_start_ms = millis ( ) ;
2015-12-26 06:40:40 -04:00
}
2016-06-16 05:17:46 -03:00
bool relax_loiter = landing_detect . lower_limit_start_ms ! = 0 & &
( millis ( ) - landing_detect . lower_limit_start_ms ) > 1000 ;
2015-12-26 06:40:40 -04:00
return relax_loiter ;
}
2016-04-22 07:20:06 -03:00
// see if we are flying in vtol
bool QuadPlane : : is_flying_vtol ( void )
{
2016-07-26 18:43:16 -03:00
if ( ! available ( ) ) {
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 ;
}
2017-09-06 04:58:08 -03:00
if ( plane . control_mode = = GUIDED & & guided_takeoff ) {
return true ;
}
2016-07-24 17:08:36 -03:00
if ( plane . control_mode = = QSTABILIZE | | plane . control_mode = = QHOVER | | plane . control_mode = = QLOITER ) {
// in manual flight modes only consider aircraft landed when pilot demanded throttle is zero
return plane . channel_throttle - > get_control_in ( ) > 0 ;
}
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 .
*/
float QuadPlane : : landing_descent_rate_cms ( float height_above_ground )
{
float ret = linear_interpolate ( land_speed_cms , wp_nav - > get_speed_down ( ) ,
height_above_ground ,
2017-09-08 03:43:32 -03:00
land_final_alt , land_final_alt + 6 ) ;
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 ) {
2016-02-04 00:03:39 -04:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
2015-12-26 06:40:40 -04:00
attitude_control - > set_throttle_out_unstabilized ( 0 , true , 0 ) ;
pos_control - > relax_alt_hold_controllers ( 0 ) ;
2018-09-19 00:22:42 -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 ;
}
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-09-19 00:22:42 -03:00
const uint32_t now = AP_HAL : : millis ( ) ;
if ( now - last_loiter_ms > 500 ) {
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-09-19 00:22:42 -03:00
last_loiter_ms = now ;
2016-02-04 00:03:39 -04:00
// motors use full range
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2015-12-26 04:51:05 -04:00
// initialize vertical speed and acceleration
2015-12-26 06:40:40 -04:00
pos_control - > set_speed_z ( - pilot_velocity_z_max , pilot_velocity_z_max ) ;
pos_control - > set_accel_z ( pilot_accel_z ) ;
2015-12-26 04:51:05 -04:00
// process pilot's roll and pitch input
2018-03-27 23:24:05 -03:00
loiter_nav - > set_pilot_desired_acceleration ( plane . channel_roll - > get_control_in ( ) ,
plane . channel_pitch - > get_control_in ( ) ,
plane . G_Dt ) ;
2015-12-26 04:51:05 -04:00
// Update EKF speed limit - used to limit speed when we are using optical flow
float ekfGndSpdLimit , ekfNavVelGainScaler ;
ahrs . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
// run loiter controller
2018-03-27 23:24:05 -03:00
loiter_nav - > update ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
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 ;
pos_control - > set_limit_accel_xy ( ) ;
}
}
// 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
2016-03-09 03:20:41 -04:00
if ( plane . control_mode = = QLAND ) {
2019-02-01 23:49:02 -04:00
if ( poscontrol . state < QPOS_LAND_FINAL & & check_land_final ( ) ) {
2016-05-05 19:27:47 -03:00
poscontrol . state = QPOS_LAND_FINAL ;
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 ) ;
2016-05-05 19:27:47 -03:00
float descent_rate = ( poscontrol . state = = QPOS_LAND_FINAL ) ? land_speed_cms : landing_descent_rate_cms ( height_above_ground ) ;
2016-03-09 03:20:41 -04:00
pos_control - > set_alt_target_from_climb_rate ( - descent_rate , plane . G_Dt , true ) ;
check_land_complete ( ) ;
2017-09-06 04:58:08 -03:00
} else if ( plane . control_mode = = GUIDED & & guided_takeoff ) {
pos_control - > set_alt_target_from_climb_rate_ff ( 0 , plane . G_Dt , false ) ;
2016-03-09 03:20:41 -04:00
} else {
// update altitude target and call position controller
pos_control - > set_alt_target_from_climb_rate_ff ( get_pilot_desired_climb_rate_cms ( ) , plane . G_Dt , false ) ;
}
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
*/
2016-01-09 18:42:46 -04:00
float QuadPlane : : get_pilot_input_yaw_rate_cds ( void )
{
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_throttle - > get_control_in ( ) < = 0 & & ! plane . auto_throttle_mode ) {
2016-01-09 18:42:46 -04:00
// the user may be trying to disarm
return 0 ;
}
// add in rudder input
2017-02-24 01:47:09 -04:00
return plane . channel_rudder - > get_control_in ( ) * yaw_rate_max / 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
}
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_throttle - > get_control_in ( ) < = 0 & & ! plane . auto_throttle_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
float QuadPlane : : get_pilot_desired_climb_rate_cms ( void )
{
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 ;
2015-12-26 06:40:40 -04:00
return pilot_velocity_z_max * plane . channel_throttle - > pwm_to_angle_dz_trim ( dead_zone , trim ) / 100.0f ;
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 )
{
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_throttle - > get_control_in ( ) > = 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 )
*/
float QuadPlane : : assist_climb_rate_cms ( void )
{
2016-01-20 02:21:54 -04:00
float climb_rate ;
2015-12-26 06:40:40 -04:00
if ( plane . auto_throttle_mode ) {
2016-01-20 02:21:54 -04:00
// use altitude_error_cm, spread over 10s interval
2016-12-04 11:27:33 -04:00
climb_rate = plane . altitude_error_cm / 10.0f ;
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 ) ;
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
climb_rate * = plane . channel_throttle - > get_control_in ( ) ;
2015-11-24 04:24:04 -04:00
}
2016-01-20 02:21:54 -04:00
climb_rate = constrain_float ( climb_rate , - wp_nav - > get_speed_down ( ) , wp_nav - > get_speed_up ( ) ) ;
2017-04-08 01:39:55 -03:00
// bring in the demanded climb rate over 2 seconds
2018-01-24 20:38:07 -04:00
const uint32_t dt_since_start = last_pidz_active_ms - last_pidz_init_ms ;
2017-04-08 01:39:55 -03:00
if ( dt_since_start < 2000 ) {
climb_rate = linear_interpolate ( 0 , climb_rate , dt_since_start , 0 , 2000 ) ;
}
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
*/
2016-01-09 18:42:46 -04:00
float QuadPlane : : desired_auto_yaw_rate_cds ( void )
2015-12-26 06:40:40 -04:00
{
float aspeed ;
2016-08-07 21:48:36 -03:00
if ( ! ahrs . airspeed_estimate ( & aspeed ) | | aspeed < plane . aparm . airspeed_min ) {
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
*/
bool QuadPlane : : assistance_needed ( float aspeed )
{
if ( assist_speed < = 0 ) {
// assistance disabled
in_angle_assist = false ;
angle_error_start_ms = 0 ;
return false ;
}
if ( aspeed < assist_speed ) {
// assistance due to Q_ASSIST_SPEED
in_angle_assist = false ;
angle_error_start_ms = 0 ;
return true ;
}
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 ;
}
2018-01-24 20:38:07 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
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
}
2018-01-24 20:38:07 -04:00
bool ret = ( now - angle_error_start_ms ) > = 1000U ;
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 ;
}
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 )
{
2016-01-03 01:46:34 -04:00
if ( plane . control_mode = = MANUAL | |
plane . control_mode = = ACRO | |
plane . control_mode = = TRAINING ) {
// in manual modes quad motors are always off
2017-02-11 01:50:03 -04:00
if ( ! tilt . motors_active & & ! is_tailsitter ( ) ) {
2016-05-01 20:33:54 -03:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SHUT_DOWN ) ;
motors - > output ( ) ;
}
2015-12-26 04:27:13 -04:00
transition_state = TRANSITION_DONE ;
2016-10-11 03:04:22 -03:00
assisted_flight = false ;
2015-12-26 04:27:13 -04:00
return ;
}
2015-12-26 06:40:40 -04:00
float aspeed ;
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 ;
}
2015-12-26 06:40:40 -04:00
/*
see if we should provide some assistance
*/
2016-08-29 03:44:54 -03:00
if ( have_airspeed & &
assistance_needed ( aspeed ) & &
2017-02-11 04:12:56 -04:00
! is_tailsitter ( ) & &
2017-07-30 01:53:38 -03:00
hal . util - > get_soft_armed ( ) & &
2017-10-20 01:13:52 -03:00
( ( plane . auto_throttle_mode & & ! plane . throttle_suppressed ) | |
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
plane . channel_throttle - > get_control_in ( ) > 0 | |
2016-01-01 03:18:53 -04:00
plane . is_flying ( ) ) ) {
2015-12-26 06:40:40 -04:00
// the quad should provide some assistance to the plane
2016-06-10 04:37:14 -03:00
if ( transition_state ! = TRANSITION_AIRSPEED_WAIT ) {
2017-07-09 00:49:39 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Transition started airspeed %.1f " , ( double ) aspeed ) ;
2016-06-10 04:37:14 -03:00
}
2015-12-26 06:40:40 -04:00
transition_state = TRANSITION_AIRSPEED_WAIT ;
transition_start_ms = millis ( ) ;
assisted_flight = true ;
} 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 ( ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Transition FW done " ) ;
2017-02-11 04:12:56 -04:00
transition_state = TRANSITION_DONE ;
}
}
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 till decrease rapidly)
if ( tiltrotor_fully_fwd ( ) & & transition_state ! = TRANSITION_AIRSPEED_WAIT ) {
2017-01-22 22:06:31 -04:00
transition_state = TRANSITION_DONE ;
}
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 : {
2016-04-01 03:29:51 -03:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_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 " ) ;
2015-12-26 05:13:20 -04:00
transition_start_ms = millis ( ) ;
}
2016-08-07 21:48:36 -03:00
if ( have_airspeed & & aspeed > plane . aparm . airspeed_min & & ! assisted_flight ) {
2015-12-26 04:51:05 -04:00
transition_start_ms = millis ( ) ;
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 ;
2015-12-26 06:40:40 -04:00
hold_hover ( assist_climb_rate_cms ( ) ) ;
2016-09-20 22:46:15 -03:00
run_rate_controller ( ) ;
2016-03-24 22:11:56 -03:00
motors_output ( ) ;
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
attitude_control - > set_throttle_mix_max ( ) ;
2015-12-26 04:27:13 -04:00
break ;
}
case TRANSITION_TIMER : {
2016-04-01 03:29:51 -03:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_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
2015-12-26 04:51:05 -04:00
if ( millis ( ) - transition_start_ms > ( unsigned ) transition_time_ms ) {
2015-12-26 04:27:13 -04:00
transition_state = TRANSITION_DONE ;
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 ( ) ;
2017-01-28 19:40:56 -04:00
float transition_scale = ( trans_time_ms - ( millis ( ) - transition_start_ms ) ) / trans_time_ms ;
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 ) ;
2016-09-20 22:46:15 -03:00
run_rate_controller ( ) ;
2016-03-24 22:11:56 -03:00
motors_output ( ) ;
2015-12-26 04:27:13 -04:00
break ;
}
2017-10-30 01:19:38 -03:00
case TRANSITION_ANGLE_WAIT_FW : {
2017-02-11 04:12:56 -04:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
assisted_flight = true ;
2017-11-05 02:25:00 -04:00
// calculate transition rate in degrees per
// millisecond. Assume we want to get to the transition angle
// in half the transition time
float transition_rate = tailsitter . transition_angle / float ( transition_time_ms / 2 ) ;
uint32_t dt = AP_HAL : : millis ( ) - transition_start_ms ;
plane . nav_pitch_cd = constrain_float ( ( - transition_rate * dt ) * 100 , - 8500 , 0 ) ;
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 ) ;
2017-10-30 01:19:38 -03:00
attitude_control - > set_throttle_out ( motors - > get_throttle_hover ( ) , true , 0 ) ;
2017-02-11 04:12:56 -04:00
run_rate_controller ( ) ;
motors_output ( ) ;
break ;
}
2017-10-30 01:19:38 -03:00
case TRANSITION_ANGLE_WAIT_VTOL :
// nothing to do, this is handled in the fw attitude controller
break ;
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 ( ) ) {
2016-05-01 20:33:54 -03:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SHUT_DOWN ) ;
motors - > output ( ) ;
}
2015-12-26 04:27:13 -04:00
break ;
2015-11-24 04:24:04 -04:00
}
}
2016-09-20 22:46:15 -03:00
/*
run multicopter rate controller
*/
void QuadPlane : : run_rate_controller ( void )
{
attitude_control - > rate_controller_run ( ) ;
}
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 ;
}
2017-04-18 09:40:06 -03:00
2016-07-22 05:36:28 -03:00
if ( plane . afs . should_crash_vehicle ( ) ) {
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SHUT_DOWN ) ;
motors - > output ( ) ;
return ;
}
2016-03-12 19:05:10 -04:00
if ( motor_test . running ) {
motor_test_output ( ) ;
return ;
}
2017-01-28 19:40:56 -04:00
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
*/
attitude_control - > relax_attitude_controllers ( ) ;
pos_control - > relax_alt_hold_controllers ( 0 ) ;
}
2016-03-09 03:20:41 -04:00
if ( ! in_vtol_mode ( ) ) {
2015-11-24 04:24:04 -04:00
update_transition ( ) ;
} else {
2018-01-24 20:38:07 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
2017-10-30 01:19:38 -03:00
2015-12-26 06:40:40 -04:00
assisted_flight = false ;
2017-01-28 19:40:56 -04:00
// give full authority to attitude control
attitude_control - > set_throttle_mix_max ( ) ;
2015-12-26 05:13:20 -04:00
// run low level rate controllers
2016-09-20 22:46:15 -03:00
run_rate_controller ( ) ;
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 ;
} else if ( is_tailsitter ( ) & &
transition_state = = TRANSITION_ANGLE_WAIT_VTOL ) {
if ( tailsitter_transition_vtol_complete ( ) ) {
/*
we have completed transition to VTOL as a tailsitter ,
setup for the back transition when needed
*/
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Transition VTOL done " ) ;
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 ;
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 ;
} 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 & &
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
( plane . channel_throttle - > get_control_in ( ) > 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 ( ) ;
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
AP_Motors : : DESIRED_SHUT_DOWN
This is a safety check to prevent accidental motor runs on the
ground , such as if RC fails and QRTL is started
*/
void QuadPlane : : check_throttle_suppression ( void )
{
// 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
if ( motors - > get_desired_spool_state ( ) < AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) {
return ;
}
// if the users throttle is above zero then allow motors to run
if ( plane . channel_throttle - > get_control_in ( ) ! = 0 ) {
return ;
}
// if we are in a fixed wing auto throttle mode and we have
// unsuppressed the throttle then allow motors to run
if ( plane . auto_throttle_mode & & ! plane . throttle_suppressed ) {
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
2017-10-29 03:31:09 -03:00
if ( plane . control_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
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
motors - > set_throttle ( 0 ) ;
last_motors_active_ms = 0 ;
}
2016-03-24 22:11:56 -03:00
/*
output motors and do any copter needed
*/
void QuadPlane : : motors_output ( void )
{
2016-08-29 04:55:27 -03:00
if ( ! hal . util - > get_soft_armed ( ) | | plane . afs . should_crash_vehicle ( ) ) {
2016-07-22 05:36:28 -03:00
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SHUT_DOWN ) ;
motors - > output ( ) ;
return ;
}
2016-06-02 00:10:39 -03:00
if ( esc_calibration & & AP_Notify : : flags . esc_calibration & & plane . control_mode = = QSTABILIZE ) {
// output is direct from run_esc_calibration()
return ;
}
2016-08-01 19:21:44 -03:00
2017-10-30 01:19:38 -03:00
if ( in_tailsitter_vtol_transition ( ) ) {
/*
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
check_throttle_suppression ( ) ;
2016-03-24 22:11:56 -03:00
motors - > output ( ) ;
2016-04-03 20:50:15 -03:00
if ( motors - > armed ( ) ) {
plane . DataFlash . Log_Write_Rate ( plane . ahrs , * motors , * attitude_control , * pos_control ) ;
Log_Write_QControl_Tuning ( ) ;
2018-01-24 20:38:07 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
2016-05-27 01:16:44 -03:00
if ( now - last_ctrl_log_ms > 100 ) {
attitude_control - > control_monitor_log ( ) ;
}
2016-04-03 20:50:15 -03:00
}
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 ( ) ;
}
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 ;
}
switch ( plane . control_mode ) {
case QSTABILIZE :
control_stabilize ( ) ;
break ;
case QHOVER :
control_hover ( ) ;
break ;
case QLOITER :
2016-03-09 03:20:41 -04:00
case QLAND :
2015-12-26 06:40:40 -04:00
control_loiter ( ) ;
2016-04-29 02:31:08 -03:00
break ;
case QRTL :
control_qrtl ( ) ;
break ;
2015-12-26 06:40:40 -04:00
default :
break ;
}
// we also stabilize using fixed wing surfaces
float speed_scaler = plane . get_speed_scaler ( ) ;
plane . stabilize_roll ( speed_scaler ) ;
plane . stabilize_pitch ( speed_scaler ) ;
}
/*
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 ;
2015-12-26 06:40:40 -04:00
switch ( plane . control_mode ) {
case QSTABILIZE :
init_stabilize ( ) ;
break ;
case QHOVER :
init_hover ( ) ;
break ;
case QLOITER :
init_loiter ( ) ;
break ;
2016-03-09 03:20:41 -04:00
case QLAND :
2019-02-22 02:04:35 -04:00
init_qland ( ) ;
2016-03-09 03:20:41 -04:00
break ;
2016-04-29 02:31:08 -03:00
case QRTL :
init_qrtl ( ) ;
break ;
2017-09-06 04:58:08 -03:00
case GUIDED :
guided_takeoff = false ;
break ;
2015-12-26 06:40:40 -04:00
default :
break ;
}
return true ;
}
/*
handle a MAVLink DO_VTOL_TRANSITION
*/
2016-04-22 05:22:31 -03:00
bool QuadPlane : : handle_do_vtol_transition ( enum MAV_VTOL_STATE state )
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
}
if ( plane . control_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
{
2016-05-05 22:28:26 -03:00
if ( ! enable ) {
return false ;
}
2016-05-05 22:50:26 -03:00
if ( plane . control_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_LAND :
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 :
2016-05-11 02:57:41 -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 ) ;
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
/*
are we in a VTOL mode ?
*/
2017-10-30 01:19:38 -03:00
bool QuadPlane : : in_vtol_mode ( void ) const
2016-02-20 05:20:27 -04:00
{
2016-04-09 05:48:22 -03:00
if ( ! enable ) {
return false ;
}
2016-02-20 05:20:27 -04:00
return ( plane . control_mode = = QSTABILIZE | |
plane . control_mode = = QHOVER | |
plane . control_mode = = QLOITER | |
2016-03-09 03:20:41 -04:00
plane . control_mode = = QLAND | |
2016-04-29 02:31:08 -03:00
plane . control_mode = = QRTL | |
2016-08-12 17:27:48 -03:00
( ( plane . control_mode = = GUIDED | | plane . control_mode = = AVOID_ADSB ) & & plane . auto_state . vtol_loiter ) | |
2016-02-20 05:20:27 -04:00
in_vtol_auto ( ) ) ;
}
2016-04-28 23:53:20 -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
setup_target_position ( ) ;
2015-12-26 06:40:40 -04:00
2016-04-28 23:53:20 -03:00
const Location & loc = plane . next_WP_loc ;
float ekfGndSpdLimit , ekfNavVelGainScaler ;
ahrs . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
2017-04-22 23:49:48 -03:00
2018-09-13 21:02:18 -03:00
check_attitude_relax ( ) ;
2018-03-09 02:28:02 -04:00
// horizontal position control
2018-02-09 23:52:46 -04:00
switch ( poscontrol . state ) {
2016-04-28 23:53:20 -03:00
2016-05-05 19:27:47 -03:00
case QPOS_POSITION1 : {
2016-04-10 07:36:20 -03:00
Vector2f diff_wp = location_diff ( plane . current_loc , loc ) ;
2019-02-22 02:04:35 -04:00
const float distance = diff_wp . length ( ) ;
2016-04-02 06:54:01 -03:00
2016-05-05 19:27:47 -03:00
if ( poscontrol . speed_scale < = 0 ) {
2016-04-02 06:54:01 -03:00
// initialise scaling so we start off targeting our
2016-04-03 20:50:15 -03:00
// current linear speed towards the target. If this is
// less than the wpnav speed then the wpnav speed is used
// land_speed_scale is then used to linearly change
// velocity as we approach the waypoint, aiming for zero
// speed at the waypoint
Vector2f groundspeed = ahrs . groundspeed_vector ( ) ;
2016-05-07 18:34:30 -03:00
float speed_towards_target = distance > 1 ? ( diff_wp . normalized ( ) * groundspeed ) : 0 ;
// setup land_speed_scale so at current distance we
// maintain speed towards target, and slow down as we
// approach
2016-04-20 05:07:04 -03:00
// max_speed will control how fast we will fly. It will always decrease
2016-05-05 19:27:47 -03:00
poscontrol . max_speed = MAX ( speed_towards_target , wp_nav - > get_speed_xy ( ) * 0.01 ) ;
poscontrol . speed_scale = poscontrol . max_speed / MAX ( distance , 1 ) ;
2016-04-02 06:54:01 -03:00
}
2016-04-10 07:36:20 -03:00
// run fixed wing navigation
2016-04-28 23:53:20 -03:00
plane . nav_controller - > update_waypoint ( plane . prev_WP_loc , loc ) ;
2016-04-10 07:36:20 -03:00
/*
calculate target velocity , not dropping it below 2 m / s
*/
const float final_speed = 2.0f ;
2016-05-05 19:27:47 -03:00
Vector2f target_speed_xy = diff_wp * poscontrol . speed_scale ;
2016-04-20 05:07:04 -03:00
float target_speed = target_speed_xy . length ( ) ;
2016-05-07 18:34:30 -03:00
if ( distance < 1 ) {
// prevent numerical error before switching to POSITION2
target_speed_xy ( 0.1 , 0.1 ) ;
}
2016-04-20 05:07:04 -03:00
if ( target_speed < final_speed ) {
// until we enter the loiter we always aim for at least 2m/s
target_speed_xy = target_speed_xy . normalized ( ) * final_speed ;
2016-05-05 19:27:47 -03:00
poscontrol . max_speed = final_speed ;
} else if ( target_speed > poscontrol . max_speed ) {
2016-04-20 05:07:04 -03:00
// we never speed up during landing approaches
2016-05-05 19:27:47 -03:00
target_speed_xy = target_speed_xy . normalized ( ) * poscontrol . max_speed ;
2016-04-20 05:07:04 -03:00
} else {
2016-05-05 19:27:47 -03:00
poscontrol . max_speed = target_speed ;
2016-04-10 07:36:20 -03:00
}
2016-04-20 05:07:04 -03:00
pos_control - > set_desired_velocity_xy ( target_speed_xy . x * 100 ,
target_speed_xy . y * 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 ( ) ;
pos_control - > set_xy_target ( curr_pos . x , curr_pos . y ) ;
2018-02-09 23:52:46 -04:00
pos_control - > set_desired_accel_xy ( 0.0f , 0.0f ) ;
// run horizontal velocity controller
pos_control - > update_vel_controller_xy ( ekfNavVelGainScaler ) ;
2016-04-02 06:54:01 -03:00
// nav roll and pitch are controller by position controller
plane . nav_roll_cd = pos_control - > get_roll ( ) ;
plane . nav_pitch_cd = pos_control - > get_pitch ( ) ;
2016-04-10 07:36:20 -03:00
/*
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 .
*/
float pitch_limit_cd = linear_interpolate ( - 300 , plane . aparm . pitch_limit_min_cd ,
plane . auto_state . wp_proportion , 0 , 1 ) ;
if ( plane . nav_pitch_cd < pitch_limit_cd ) {
plane . nav_pitch_cd = pitch_limit_cd ;
// tell the pos controller we have limited the pitch to
// stop integrator buildup
pos_control - > set_limit_accel_xy ( ) ;
}
2016-04-02 07:19:06 -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 ,
2016-04-02 06:54:01 -03:00
plane . nav_pitch_cd ,
2017-07-10 01:00:45 -03:00
desired_auto_yaw_rate_cds ( ) + get_weathervane_yaw_rate_cds ( ) ) ;
2016-04-10 07:36:20 -03:00
if ( plane . auto_state . wp_proportion > = 1 | |
plane . auto_state . wp_distance < 5 ) {
2016-05-05 19:27:47 -03:00
poscontrol . state = QPOS_POSITION2 ;
2018-09-19 00:22:42 -03:00
loiter_nav - > clear_pilot_desired_acceleration ( ) ;
2018-03-27 23:24:05 -03:00
loiter_nav - > init_target ( ) ;
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 :
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
*/
2018-02-09 23:52:46 -04:00
2016-01-09 18:42:46 -04:00
// also run fixed wing navigation
2016-04-28 23:53:20 -03:00
plane . nav_controller - > update_waypoint ( plane . prev_WP_loc , loc ) ;
2018-02-09 23:52:46 -04:00
FALLTHROUGH ;
2016-01-15 01:49:49 -04:00
2018-02-09 23:52:46 -04:00
case QPOS_LAND_FINAL :
// set position controller desired velocity and acceleration to zero
pos_control - > set_desired_velocity_xy ( 0.0f , 0.0f ) ;
pos_control - > set_desired_accel_xy ( 0.0f , 0.0f ) ;
// set position control target and update
2019-02-22 02:04:35 -04:00
if ( should_relax ( ) ) {
loiter_nav - > soften_for_landing ( ) ;
} else {
pos_control - > set_xy_target ( poscontrol . target . x , poscontrol . target . y ) ;
}
2018-02-09 23:52:46 -04:00
pos_control - > update_xy_controller ( ekfNavVelGainScaler ) ;
2016-01-15 01:49:49 -04:00
// nav roll and pitch are controller by position controller
2018-02-09 23:52:46 -04:00
plane . nav_roll_cd = pos_control - > get_roll ( ) ;
plane . nav_pitch_cd = pos_control - > get_pitch ( ) ;
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
2016-05-05 19:27:47 -03:00
switch ( poscontrol . state ) {
case QPOS_POSITION1 :
2017-10-21 03:30:40 -03:00
case QPOS_POSITION2 : {
bool vtol_loiter_auto = false ;
if ( plane . control_mode = = AUTO ) {
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 ;
}
}
if ( plane . control_mode = = QRTL | | plane . control_mode = = GUIDED | | vtol_loiter_auto ) {
2016-04-29 02:31:08 -03:00
plane . ahrs . get_position ( plane . current_loc ) ;
2016-04-29 02:50:45 -03:00
float target_altitude = plane . next_WP_loc . alt ;
2016-05-05 19:27:47 -03:00
if ( poscontrol . slow_descent ) {
2016-04-29 02:50:45 -03:00
// gradually descend as we approach target
plane . auto_state . wp_proportion = location_path_proportion ( plane . current_loc ,
plane . prev_WP_loc , plane . next_WP_loc ) ;
target_altitude = linear_interpolate ( plane . prev_WP_loc . alt ,
plane . next_WP_loc . alt ,
plane . auto_state . wp_proportion ,
0 , 1 ) ;
}
2017-09-05 19:22:38 -03:00
adjust_alt_target ( target_altitude - plane . home . alt ) ;
2016-04-29 02:31:08 -03:00
} else {
pos_control - > set_alt_target_from_climb_rate ( 0 , plane . G_Dt , false ) ;
}
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
2016-05-05 19:27:47 -03:00
case QPOS_LAND_DESCEND : {
2016-06-11 01:37:44 -03:00
float height_above_ground = plane . relative_ground_altitude ( plane . g . rangefinder_landing ) ;
2016-04-28 23:53:20 -03:00
pos_control - > set_alt_target_from_climb_rate ( - landing_descent_rate_cms ( height_above_ground ) ,
plane . G_Dt , true ) ;
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_FINAL :
2016-04-28 23:53:20 -03:00
pos_control - > set_alt_target_from_climb_rate ( - land_speed_cms , plane . G_Dt , true ) ;
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 ( ) ;
2015-12-26 06:40:40 -04:00
}
2016-01-01 05:42:10 -04:00
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 ;
Location origin = inertial_nav . get_origin ( ) ;
Vector2f diff2d ;
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2017-07-10 01:00:45 -03:00
2016-04-28 23:53:20 -03:00
diff2d = location_diff ( origin , loc ) ;
2016-05-05 19:27:47 -03:00
poscontrol . target . x = diff2d . x * 100 ;
poscontrol . target . y = diff2d . y * 100 ;
poscontrol . target . 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 ( ) ;
2016-04-28 23:53:20 -03:00
if ( ! locations_are_same ( loc , last_auto_target ) | |
plane . next_WP_loc . alt ! = last_auto_target . alt | |
2018-01-24 20:38:07 -04:00
now - last_loiter_ms > 500 ) {
2016-05-05 19:27:47 -03:00
wp_nav - > set_wp_destination ( poscontrol . target ) ;
2016-04-28 23:53:20 -03:00
last_auto_target = loc ;
}
2018-01-24 20:38:07 -04:00
last_loiter_ms = now ;
2016-04-28 23:53:20 -03:00
// setup vertical speed and acceleration
pos_control - > set_speed_z ( - pilot_velocity_z_max , pilot_velocity_z_max ) ;
pos_control - > set_accel_z ( pilot_accel_z ) ;
}
/*
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
*/
float ekfGndSpdLimit , ekfNavVelGainScaler ;
ahrs . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
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
pos_control - > set_desired_velocity_xy ( 0.0f , 0.0f ) ;
pos_control - > set_desired_accel_xy ( 0.0f , 0.0f ) ;
// set position control target and update
pos_control - > set_xy_target ( poscontrol . target . x , poscontrol . target . y ) ;
pos_control - > update_xy_controller ( ekfNavVelGainScaler ) ;
2016-04-28 23:53:20 -03:00
// nav roll and pitch are controller by position controller
plane . nav_roll_cd = pos_control - > get_roll ( ) ;
plane . nav_pitch_cd = pos_control - > get_pitch ( ) ;
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 ( ) ) ;
2016-04-28 23:53:20 -03:00
pos_control - > set_alt_target_from_climb_rate ( wp_nav - > get_speed_up ( ) , plane . G_Dt , true ) ;
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
pos_control - > set_alt_target_from_climb_rate_ff ( assist_climb_rate_cms ( ) , plane . G_Dt , 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
*/
void QuadPlane : : control_auto ( const Location & loc )
{
if ( ! setup ( ) ) {
return ;
}
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
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 ;
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_VTOL_LAND :
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 :
2016-05-05 19:27:47 -03:00
vtol_position_controller ( ) ;
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 ( ) ;
if ( poscontrol . 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 ;
2016-05-05 19:27:47 -03:00
poscontrol . slow_descent = ( plane . current_loc . alt > plane . next_WP_loc . alt ) ;
poscontrol . state = QPOS_POSITION1 ;
poscontrol . speed_scale = 0 ;
2018-02-09 23:52:46 -04:00
pos_control - > set_desired_accel_xy ( 0.0f , 0.0f ) ;
2018-08-21 03:26:42 -03:00
pos_control - > init_xy_controller ( ) ;
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:42:18 -04:00
// we always use the current location in XY for takeoff.
Location loc = cmd . content . location ;
loc . lat = 0 ;
loc . lng = 0 ;
plane . set_next_WP ( loc ) ;
2016-01-01 05:42:10 -04:00
plane . next_WP_loc . alt = plane . current_loc . alt + cmd . content . location . alt ;
throttle_wait = false ;
2016-01-08 19:47:41 -04:00
// set target to current position
2018-09-19 00:22:42 -03:00
loiter_nav - > clear_pilot_desired_acceleration ( ) ;
2018-03-27 23:24:05 -03:00
loiter_nav - > init_target ( ) ;
2016-01-08 19:47:41 -04:00
// initialize vertical speed and acceleration
pos_control - > set_speed_z ( - pilot_velocity_z_max , pilot_velocity_z_max ) ;
pos_control - > set_accel_z ( pilot_accel_z ) ;
// initialise position and desired velocity
2017-09-05 19:22:38 -03:00
set_alt_target_current ( ) ;
2016-01-08 19:47:41 -04:00
pos_control - > set_desired_velocity_z ( inertial_nav . get_velocity_z ( ) ) ;
2016-01-01 05:42:10 -04:00
// also update nav_controller for status output
plane . nav_controller - > update_waypoint ( plane . prev_WP_loc , plane . next_WP_loc ) ;
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 ;
}
2016-02-15 04:45:56 -04:00
attitude_control - > get_rate_roll_pid ( ) . reset_I ( ) ;
attitude_control - > get_rate_pitch_pid ( ) . reset_I ( ) ;
2016-04-01 02:45:01 -03:00
attitude_control - > get_rate_yaw_pid ( ) . reset_I ( ) ;
2018-01-15 07:59:09 -04:00
pos_control - > get_accel_z_pid ( ) . reset_I ( ) ;
2017-11-20 09:02:22 -04:00
pos_control - > get_vel_xy_pid ( ) . reset_I ( ) ;
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 ;
2016-05-05 19:27:47 -03:00
poscontrol . state = QPOS_POSITION1 ;
poscontrol . speed_scale = 0 ;
2018-02-09 23:52:46 -04:00
pos_control - > set_desired_accel_xy ( 0.0f , 0.0f ) ;
2018-08-21 03:26:42 -03:00
pos_control - > init_xy_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 ;
2017-09-05 19:22:38 -03:00
set_alt_target_current ( ) ;
2016-01-01 07:09:11 -04:00
2016-01-01 05:42:10 -04:00
// also update nav_controller for status output
plane . nav_controller - > update_waypoint ( plane . prev_WP_loc , plane . next_WP_loc ) ;
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 ;
}
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 ) ;
2017-09-05 19:22:38 -03:00
set_alt_target_current ( ) ;
2016-08-29 05:04:42 -03:00
plane . complete_auto_takeoff ( ) ;
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
*/
void QuadPlane : : check_land_complete ( void )
{
if ( poscontrol . state ! = QPOS_LAND_FINAL ) {
// only apply to final landing phase
2016-06-16 05:17:46 -03:00
return ;
2016-03-09 03:20:41 -04:00
}
2019-02-01 23:49:02 -04:00
if ( land_detector ( 4000 ) ) {
plane . disarm_motors ( ) ;
poscontrol . state = QPOS_LAND_COMPLETE ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Land complete " ) ;
// reload target airspeed which could have been modified by the mission
plane . aparm . airspeed_cruise_cm . load ( ) ;
}
}
/*
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 ) ;
if ( height_above_ground < land_final_alt ) {
return true ;
}
/*
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 ;
}
2016-05-05 19:27:47 -03:00
if ( poscontrol . state = = QPOS_POSITION2 & &
2016-01-02 02:55:56 -04:00
plane . auto_state . wp_distance < 2 ) {
2016-05-05 19:27:47 -03:00
poscontrol . state = QPOS_LAND_DESCEND ;
2017-07-08 22:15:58 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Land descend started " ) ;
2016-04-29 02:31:08 -03:00
plane . set_next_WP ( plane . next_WP_loc ) ;
2016-01-02 02:55:56 -04:00
}
// at land_final_alt begin final landing
2019-02-01 23:49:02 -04:00
if ( poscontrol . state = = QPOS_LAND_DESCEND & & check_land_final ( ) ) {
2016-05-05 19:27:47 -03:00
poscontrol . 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
}
2016-03-09 03:20:41 -04:00
check_land_complete ( ) ;
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 ( )
{
2016-04-03 20:50:15 -03:00
const Vector3f & desired_velocity = pos_control - > get_desired_velocity ( ) ;
2016-04-10 07:36:20 -03:00
const Vector3f & accel_target = pos_control - > get_accel_target ( ) ;
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 ( ) ,
angle_boost : attitude_control - > angle_boost ( ) ,
throttle_out : motors - > get_throttle ( ) ,
desired_alt : pos_control - > get_alt_target ( ) / 100.0f ,
inav_alt : inertial_nav . get_altitude ( ) / 100.0f ,
desired_climb_rate : ( int16_t ) pos_control - > get_vel_target_z ( ) ,
2016-04-03 20:50:15 -03:00
climb_rate : ( int16_t ) inertial_nav . get_velocity_z ( ) ,
dvx : desired_velocity . x * 0.01f ,
dvy : desired_velocity . y * 0.01f ,
2016-04-10 07:36:20 -03:00
dax : accel_target . x * 0.01f ,
day : accel_target . y * 0.01f ,
2017-04-18 09:25:14 -03:00
throttle_mix : attitude_control - > get_throttle_mix ( ) ,
2016-03-24 22:33:19 -03:00
} ;
plane . DataFlash . 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 .
*/
int8_t QuadPlane : : forward_throttle_pct ( void )
{
/*
in non - VTOL modes or modes without a velocity controller . We
don ' t use it in QHOVER or QSTABILIZE as they are the primary
recovery modes for a quadplane and need to be as simple as
possible . They will drift with the wind
*/
if ( ! in_vtol_mode ( ) | |
! motors - > armed ( ) | |
vel_forward . gain < = 0 | |
plane . control_mode = = QSTABILIZE | |
plane . control_mode = = QHOVER ) {
return 0 ;
}
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
const Vector3f & desired_velocity_cms = pos_control - > get_desired_velocity ( ) ;
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 ;
}
Vector3f vel_error_body = ahrs . get_rotation_body_to_ned ( ) . transposed ( ) * ( ( desired_velocity_cms * 0.01f ) - vel_ned ) ;
// find component of velocity error in fwd body frame direction
float fwd_vel_error = vel_error_body * Vector3f ( 1 , 0 , 0 ) ;
// 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.
fwd_vel_error - = ( wp_nav - > get_speed_xy ( ) * 0.01f ) * plane . nav_pitch_cd / ( float ) plane . aparm . pitch_limit_max_cd ;
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
int8_t fwd_throttle_min = ( plane . aparm . throttle_min < = 0 ) ? 0 : plane . aparm . throttle_min ;
vel_forward . integrator = constrain_float ( vel_forward . integrator , fwd_throttle_min , plane . aparm . throttle_max ) ;
2019-02-22 20:20:20 -04:00
if ( in_vtol_auto ( ) & & is_vtol_land ( plane . mission . get_current_nav_cmd ( ) . id ) & &
( poscontrol . state = = QPOS_POSITION1 | | poscontrol . state = = QPOS_POSITION2 ) ) {
// 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 ;
} 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 | |
plane . control_mode = = QSTABILIZE | |
plane . control_mode = = QHOVER ) {
weathervane . last_output = 0 ;
return 0 ;
}
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 ) {
2016-04-20 03:23:17 -03:00
weathervane . last_pilot_input_ms = AP_HAL : : millis ( ) ;
weathervane . last_output = 0 ;
return 0 ;
}
if ( AP_HAL : : millis ( ) - weathervane . last_pilot_input_ms < 3000 ) {
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 )
{
poscontrol . state = QPOS_POSITION1 ;
poscontrol . speed_scale = 0 ;
2017-09-06 04:58:08 -03:00
guided_takeoff = false ;
2016-05-05 22:28:26 -03:00
setup_target_position ( ) ;
poscontrol . slow_descent = ( plane . current_loc . alt > plane . next_WP_loc . alt ) ;
}
/*
update guided mode control
*/
void QuadPlane : : guided_update ( void )
{
2017-09-06 04:58:08 -03:00
if ( plane . control_mode = = GUIDED & & guided_takeoff & & plane . current_loc . alt < plane . next_WP_loc . alt ) {
throttle_wait = false ;
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
takeoff_controller ( ) ;
} else {
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 ( ) ) {
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_SHUT_DOWN ) ;
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
if ( plane . control_mode ! = GUIDED & & plane . control_mode ! = AUTO ) {
return false ;
}
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 )
{
pos_control - > set_alt_target ( inertial_nav . get_altitude ( ) ) ;
}
/*
adjust the altitude target to the given target , moving it slowly
*/
void QuadPlane : : adjust_alt_target ( float altitude_cm )
{
float current_alt = inertial_nav . get_altitude ( ) ;
// don't let it get beyond 50cm from current altitude
float target_cm = constrain_float ( altitude_cm , current_alt - 50 , current_alt + 50 ) ;
pos_control - > set_alt_target ( target_cm ) ;
}
2017-09-06 04:58:08 -03:00
// user initiated takeoff for guided mode
bool QuadPlane : : do_user_takeoff ( float takeoff_altitude )
{
if ( plane . control_mode ! = GUIDED ) {
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 ;
motors - > set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
guided_start ( ) ;
guided_takeoff = true ;
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
/*
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 ) {
return true ;
}
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
*/
float QuadPlane : : stopping_distance ( void )
{
// 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
return plane . ahrs . groundspeed_vector ( ) . length_squared ( ) / ( 2 * transition_decel ) ;
}