Line data Source code
1 : #include "Plane.h"
2 :
3 : #if HAL_QUADPLANE_ENABLED
4 :
5 : #include "AC_AttitudeControl/AC_AttitudeControl_TS.h"
6 :
7 : const AP_Param::GroupInfo QuadPlane::var_info[] = {
8 :
9 : // @Param: ENABLE
10 : // @DisplayName: Enable QuadPlane
11 : // @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.
12 : // @Values: 0:Disable,1:Enable,2:Enable VTOL AUTO
13 : // @User: Standard
14 : // @RebootRequired: True
15 : AP_GROUPINFO_FLAGS("ENABLE", 1, QuadPlane, enable, 0, AP_PARAM_FLAG_ENABLE),
16 :
17 : // @Group: M_
18 : // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp
19 : AP_SUBGROUPVARPTR(motors, "M_", 2, QuadPlane, plane.quadplane.motors_var_info),
20 :
21 : // 3 ~ 8 were used by quadplane attitude control PIDs
22 :
23 : // 10 was ANGLE_MAX
24 :
25 : // @Param: TRANSITION_MS
26 : // @DisplayName: Transition time
27 : // @Description: Transition time in milliseconds after minimum airspeed is reached
28 : // @Units: ms
29 : // @Range: 500 30000
30 : // @User: Advanced
31 : AP_GROUPINFO("TRANSITION_MS", 11, QuadPlane, transition_time_ms, 5000),
32 :
33 : // 12 ~ 16 were used by position, velocity and acceleration PIDs
34 :
35 : // @Group: P
36 : // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
37 : AP_SUBGROUPPTR(pos_control, "P", 17, QuadPlane, AC_PosControl),
38 :
39 : // @Param: PILOT_SPD_UP
40 : // @DisplayName: Pilot maximum vertical speed up
41 : // @Description: The maximum ascending vertical velocity the pilot may request in m/s
42 : // @Units: m/s
43 : // @Range: 0.5 5
44 : // @Increment: 0.1
45 : // @User: Standard
46 : AP_GROUPINFO("PILOT_SPD_UP", 18, QuadPlane, pilot_speed_z_max_up_ms, 2.50),
47 :
48 : // @Param: PILOT_SPD_DN
49 : // @DisplayName: Pilot maximum vertical speed down
50 : // @Description: The maximum vertical velocity the pilot may request in m/s going down. If 0, uses Q_PILOT_SPD_UP value.
51 : // @Units: m/s
52 : // @Range: 0.5 5
53 : // @Increment: 0.1
54 : // @User: Standard
55 : AP_GROUPINFO("PILOT_SPD_DN", 60, QuadPlane, pilot_speed_z_max_dn_ms, 0),
56 :
57 : // @Param: PILOT_ACCEL_Z
58 : // @DisplayName: Pilot vertical acceleration
59 : // @Description: The vertical acceleration used when pilot is controlling the altitude
60 : // @Units: m/s/s
61 : // @Range: 0.5 5
62 : // @Increment: 0.1
63 : // @User: Standard
64 : AP_GROUPINFO("PILOT_ACCEL_Z", 19, QuadPlane, pilot_accel_z_mss, 2.5),
65 :
66 : // @Group: WP_
67 : // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
68 : AP_SUBGROUPPTR(wp_nav, "WP_", 20, QuadPlane, AC_WPNav),
69 :
70 : // @Param: RC_SPEED
71 : // @DisplayName: RC output speed in Hz
72 : // @Description: This is the PWM refresh rate in Hz for QuadPlane quad motors
73 : // @Units: Hz
74 : // @Range: 50 500
75 : // @Increment: 10
76 : // @User: Standard
77 : AP_GROUPINFO("RC_SPEED", 21, QuadPlane, rc_speed, 490),
78 :
79 : // @Param: THR_MIN_PWM
80 : // @DisplayName: Minimum PWM output
81 : // @Description: This is the minimum PWM output for the quad motors
82 : // @Units: PWM
83 : // @Range: 800 2200
84 : // @Increment: 1
85 : // @User: Standard
86 : // 22: THR_MIN_PWM
87 :
88 : // @Param: THR_MAX_PWM
89 : // @DisplayName: Maximum PWM output
90 : // @Description: This is the maximum PWM output for the quad motors
91 : // @Units: PWM
92 : // @Range: 800 2200
93 : // @Increment: 1
94 : // @User: Standard
95 : // 23: THR_MAX_PWM
96 :
97 : // @Param: ASSIST_SPEED
98 : // @DisplayName: Quadplane assistance speed
99 : // @Description: This is the speed below which the quad motors will provide stability and lift assistance in fixed wing modes. The default value of 0 disables assistance but will generate a pre-arm failure to encourage users to set this parameter to -1, or a positive, non-zero value. If this is set to -1 then all Q_ASSIST features are disabled except during transitions. A high non-zero,positive value will lead to more false positives which can waste battery. A lower value will result in less false positive, but will result in assistance taking longer to trigger. If unsure then set to 3 m/s below the minimum airspeed you will fly at. If you don't have an airspeed sensor then use 5 m/s below the minimum airspeed you fly at.
100 : // @Units: m/s
101 : // @Range: 0 100
102 : // @Increment: 0.1
103 : // @User: Standard
104 : AP_GROUPINFO("ASSIST_SPEED", 24, QuadPlane, assist.speed, 0),
105 :
106 : // @Param: YAW_RATE_MAX
107 : // @DisplayName: Maximum yaw rate
108 : // @Description: This is the maximum yaw rate for pilot input on rudder stick in degrees/second and weathervaning is limited to half of this value
109 : // @Units: deg/s
110 : // @Range: 50 500
111 : // @Increment: 1
112 : // @User: Standard
113 :
114 : // YAW_RATE_MAX index 25
115 :
116 : // @Param: LAND_FINAL_SPD
117 : // @DisplayName: Land final speed
118 : // @Description: The descent speed for the final stage of landing in m/s
119 : // @Units: m/s
120 : // @Range: 0.3 2
121 : // @Increment: 0.1
122 : // @User: Standard
123 : AP_GROUPINFO("LAND_FINAL_SPD", 26, QuadPlane, land_final_speed_ms, 0.5),
124 :
125 : // @Param: LAND_FINAL_ALT
126 : // @DisplayName: Land final altitude
127 : // @Description: The altitude at which we should switch to Q_LAND_FINAL_SPD descent rate
128 : // @Units: m
129 : // @Range: 0.5 50
130 : // @Increment: 0.1
131 : // @User: Standard
132 : AP_GROUPINFO("LAND_FINAL_ALT", 27, QuadPlane, land_final_alt_m, 6),
133 :
134 : // 28 was used by THR_MID
135 :
136 : // @Param: TRAN_PIT_MAX
137 : // @DisplayName: Transition max pitch
138 : // @Description: Maximum pitch during transition to auto fixed wing flight
139 : // @User: Standard
140 : // @Range: 0 30
141 : // @Units: deg
142 : // @Increment: 1
143 : AP_GROUPINFO("TRAN_PIT_MAX", 29, QuadPlane, transition_pitch_max, 3),
144 :
145 : // frame class was moved from 30 when consolidating AP_Motors classes
146 :
147 : // @Param: FRAME_CLASS
148 : // @DisplayName: Frame Class
149 : // @Description: Controls major frame class for multicopter component
150 : // @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 7:Tri, 10: Single/Dual, 12:DodecaHexa, 14:Deca, 15:Scripting Matrix, 17:Dynamic Scripting Matrix
151 : // @User: Standard
152 : AP_GROUPINFO("FRAME_CLASS", 46, QuadPlane, frame_class, 1),
153 :
154 : // @Param: FRAME_TYPE
155 : // @DisplayName: Frame Type (+, X or V)
156 : // @Description: Controls motor mixing for multicopter component
157 : // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15:I, 16:MOTOR_FRAME_TYPE_NYT_PLUS, 17:MOTOR_FRAME_TYPE_NYT_X, 18: BetaFlightXReversed, 19: Y4
158 : // @User: Standard
159 : AP_GROUPINFO("FRAME_TYPE", 31, QuadPlane, frame_type, 1),
160 :
161 : // @Param: VFWD_GAIN
162 : // @DisplayName: Forward velocity hold gain
163 : // @Description: The use of this parameter is no longer recommended and has been superseded by a method that works in all VTOL modes with the exception of QAUTOTUNE which is controlled by the Q_FWD_THR_USE parameter. This Q_VFD_GAIN parameter controls use of the forward motor in VTOL modes that use the velocity controller. Set to 0 to disable this function. 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 with QLOITER for position hold with the forward motor.
164 : // @Range: 0 0.5
165 : // @Increment: 0.01
166 : // @User: Standard
167 : AP_GROUPINFO("VFWD_GAIN", 32, QuadPlane, vel_forward.gain, 0),
168 :
169 : // 33 was used by WVANE_GAIN
170 :
171 : // 34 was used by WVANE_MINROLL
172 :
173 : // @Param: RTL_ALT
174 : // @DisplayName: QRTL return altitude
175 : // @Description: The altitude which QRTL mode heads to initially
176 : // @Units: m
177 : // @Range: 1 200
178 : // @Increment: 1
179 : // @User: Standard
180 : AP_GROUPINFO("RTL_ALT", 35, QuadPlane, qrtl_alt_m, 15),
181 :
182 : // @Param: RTL_MODE
183 : // @DisplayName: VTOL RTL mode
184 : // @Description: If this is set to 1 then an RTL will change to QRTL when within RTL_RADIUS meters of the RTL destination, VTOL approach: vehicle will RTL at RTL alt and circle with a radius of Q_FW_LND_APR_RAD down to Q_RTL_ALT and then transition into the wind and QRTL, see 'AUTO VTOL Landing', QRTL Always: do a QRTL instead of RTL
185 : // @Values: 0:Disabled,1:Enabled,2:VTOL approach,3:QRTL Always
186 : // @User: Standard
187 : AP_GROUPINFO("RTL_MODE", 36, QuadPlane, rtl_mode, 0),
188 :
189 : // 37: TILT_MASK
190 : // 38: TILT_RATE_UP
191 : // 39: TILT_MAX
192 :
193 : // @Param: GUIDED_MODE
194 : // @DisplayName: Enable VTOL in GUIDED mode
195 : // @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.
196 : // @Values: 0:Disabled,1:Enabled
197 : // @User: Standard
198 : AP_GROUPINFO("GUIDED_MODE", 40, QuadPlane, guided_mode, 0),
199 :
200 : // 41 was used by THR_MIN
201 :
202 : // @Param: ESC_CAL
203 : // @DisplayName: ESC Calibration
204 : // @Description: This is used to calibrate the throttle range of the VTOL motors. Please read https://ardupilot.club/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.
205 : // @Values: 0:Disabled,1:ThrottleInput,2:FullInput
206 : // @User: Standard
207 : AP_GROUPINFO("ESC_CAL", 42, QuadPlane, esc_calibration, 0),
208 :
209 : // @Param: VFWD_ALT
210 : // @DisplayName: Forward velocity alt cutoff
211 : // @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.
212 : // @Units: m
213 : // @Range: 0 10
214 : // @Increment: 0.25
215 : // @User: Standard
216 : AP_GROUPINFO("VFWD_ALT", 43, QuadPlane, vel_forward_alt_cutoff_m, 0),
217 :
218 : // @Param: LAND_ICE_CUT
219 : // @DisplayName: Cut IC engine on landing
220 : // @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.
221 : // @Values: 0:Disabled,1:Enabled
222 : // @User: Standard
223 : AP_GROUPINFO("LAND_ICE_CUT", 44, QuadPlane, land_icengine_cut, 1),
224 :
225 : // @Param: ASSIST_ANGLE
226 : // @DisplayName: Quadplane assistance angle
227 : // @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 positive and non-zero. Assistance will be given if the attitude is outside the normal attitude limits by at least 5 degrees and the angular error in roll or pitch is greater than this angle for at least Q_ASSIST_DELAY seconds. Set to zero to disable angle assistance.
228 : // @Units: deg
229 : // @Range: 0 90
230 : // @Increment: 1
231 : // @User: Standard
232 : AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist.angle, 30),
233 :
234 : // @Param: ASSIST_OPTIONS
235 : // @DisplayName: Quadplane assistance options
236 : // @Description: Options for special QAssist features
237 : // @Bitmask: 0: Disable force fixed wing controller recovery
238 : // @Bitmask: 1: Disable quadplane spin recovery
239 : // @User: Standard
240 : AP_GROUPINFO("ASSIST_OPTIONS", 47, QuadPlane, assist.options, 0),
241 :
242 : // 47: TILT_TYPE // was AP_Int8, re-used by AP_Int16 ASSIST_OPTIONS
243 : // 48: TAILSIT_ANGLE
244 : // 61: TAILSIT_ANG_VT
245 : // 49: TILT_RATE_DN
246 : // 50: TAILSIT_INPUT
247 : // 51: TAILSIT_MASK
248 : // 52: TAILSIT_MASKCH
249 : // 53: TAILSIT_VFGAIN
250 : // 54: TAILSIT_VHGAIN
251 : // 56: TAILSIT_VHPOW
252 :
253 : // @Param: MAV_TYPE
254 : // @DisplayName: MAVLink type identifier
255 : // @Description: This controls the mavlink type given in HEARTBEAT messages. For some GCS types a particular setting will be needed for correct operation.
256 : // @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
257 : AP_GROUPINFO("MAV_TYPE", 57, QuadPlane, mav_type, 0),
258 :
259 : // @Param: OPTIONS
260 : // @DisplayName: quadplane options
261 : // @Description: See description for each bitmask bit description
262 : // @Bitmask: 0: Level Transition-keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition
263 : // @Bitmask: 1: Allow FW Takeoff-if bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff
264 : // @Bitmask: 2: Allow FW Land-if bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND
265 : // @Bitmask: 3: Vtol Takeoff Frame-command NAV_VTOL_TAKEOFF alt set by the command's reference frame not above current location
266 : // @Bitmask: 4: Always use FW spiral approach-always use Use a fixed wing spiral approach for VTOL landings
267 : // @Bitmask: 5: USE QRTL-instead of QLAND for rc failsafe when in VTOL modes
268 : // @Bitmask: 6: Use Governor-use ICE Idle Governor in MANUAL for forward motor
269 : // @Bitmask: 7: Force Qassist-on always
270 : // @Bitmask: 8: Mtrs_Only_Qassist-in tailsitters only uses VTOL motors and not flying surfaces for QASSIST
271 : // @Bitmask: 10: Disarmed Yaw Tilt-enable motor tilt for yaw when disarmed
272 : // @Bitmask: 11: Delay Spoolup-delay VTOL spoolup for 2 seconds after arming
273 : // @Bitmask: 12: Disable speed based Qassist when using synthetic airspeed estimates
274 : // @Bitmask: 13: Disable Ground Effect Compensation-on baro altitude reports
275 : // @Bitmask: 14: Ignore forward flight angle limits-in Qmodes and use Q_A_ANGLE_MAX exclusively
276 : // @Bitmask: 15: ThrLandControl-enable throttle stick control of landing rate
277 : // @Bitmask: 16: DisableApproach-disable use of approach and airbrake stages in VTOL landing
278 : // @Bitmask: 17: EnableLandResposition-enable pilot controlled repositioning in AUTO land.Descent will pause while repositioning
279 : // @Bitmask: 18: ARMVTOL-arm only in VTOL modes (or AUTO mode when current nav cmd is VTOL Takeoff)
280 : // @Bitmask: 19: CompleteTransition-to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND
281 : // @Bitmask: 20: Force RTL mode-forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL)
282 : // @Bitmask: 21: Tilt rotor-tilt motors up when disarmed in FW modes (except manual) to prevent ground strikes.
283 : // @Bitmask: 22: Scale FF by the ratio of VTOL to plane angle P gains in Position 1 phase of transition into VTOL flight as well as reducing VTOL angle P based on airspeed.
284 : AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
285 :
286 : AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
287 :
288 : // 60 is used above for VELZ_MAX_DN
289 : // 61 was used above for TAILSIT_ANG_VT
290 :
291 : AP_GROUPEND
292 : };
293 :
294 : // second table of user settable parameters for quadplanes, this
295 : // allows us to go beyond the 64 parameter limit
296 : const AP_Param::GroupInfo QuadPlane::var_info2[] = {
297 : // @Param: TRANS_DECEL
298 : // @DisplayName: Transition deceleration
299 : // @Description: This is deceleration rate that will be used in calculating the stopping distance when transitioning from fixed wing flight to multicopter flight.
300 : // @Units: m/s/s
301 : // @Increment: 0.1
302 : // @Range: 0.2 5
303 : // @User: Standard
304 : AP_GROUPINFO("TRANS_DECEL", 1, QuadPlane, transition_decel_mss, 2.0),
305 :
306 : // @Group: LOIT_
307 : // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
308 : AP_SUBGROUPPTR(loiter_nav, "LOIT_", 2, QuadPlane, AC_Loiter),
309 :
310 : // 3: TAILSIT_GSCMAX
311 :
312 : // @Param: TRIM_PITCH
313 : // @DisplayName: Quadplane AHRS trim pitch
314 : // @Description: This sets the compensation for the pitch angle trim difference between calibrated AHRS level and vertical flight pitch. NOTE! this is relative to calibrated AHRS trim, not forward flight trim which includes PTCH_TRIM_DEG. For tailsitters, this is relative to a baseline of 90 degrees in AHRS.
315 : // @Units: deg
316 : // @Range: -10 +10
317 : // @Increment: 0.1
318 : // @User: Advanced
319 : // @RebootRequired: True
320 : AP_GROUPINFO("TRIM_PITCH", 4, QuadPlane, ahrs_trim_pitch, 0),
321 :
322 : // 5: TAILSIT_RLL_MX
323 :
324 : #if QAUTOTUNE_ENABLED
325 : // @Group: AUTOTUNE_
326 : // @Path: ../libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
327 : AP_SUBGROUPINFO(qautotune, "AUTOTUNE_", 6, QuadPlane, QAutoTune),
328 : #endif
329 :
330 : // @Param: FW_LND_APR_RAD
331 : // @DisplayName: Quadplane fixed wing landing approach radius
332 : // @Description: This provides the radius used, when using a fixed wing landing approach. If set to 0 then the WP_LOITER_RAD will be selected.
333 : // @Units: m
334 : // @Range: 0 200
335 : // @Increment: 5
336 : // @User: Advanced
337 : AP_GROUPINFO("FW_LND_APR_RAD", 7, QuadPlane, fw_land_approach_radius_m, 0),
338 :
339 : // @Param: TRANS_FAIL
340 : // @DisplayName: Quadplane transition failure time
341 : // @Description: Maximum time allowed for forward transitions, exceeding this time will cancel the transition and the aircraft will immediately change to the mode set by Q_TRANS_FAIL_ACT or finish the transition depending on Q_OPTIONS bit 19. 0 for no limit.
342 : // @Units: s
343 : // @Range: 0 20
344 : // @Increment: 1
345 : // @User: Advanced
346 : AP_GROUPINFO("TRANS_FAIL", 8, QuadPlane, transition_failure.timeout, 0),
347 :
348 : // 9: TAILSIT_MOTMX
349 :
350 : // @Param: THROTTLE_EXPO
351 : // @DisplayName: Throttle expo strength
352 : // @Description: Amount of curvature in throttle curve: 0 is linear, 1 is cubic
353 : // @Range: 0 1
354 : // @Increment: 0.1
355 : // @User: Advanced
356 : AP_GROUPINFO("THROTTLE_EXPO", 10, QuadPlane, throttle_expo, 0.2),
357 :
358 : // @Param: ACRO_RLL_RATE
359 : // @DisplayName: QACRO mode roll rate
360 : // @Description: The maximum roll rate at full stick deflection in QACRO mode
361 : // @Units: deg/s
362 : // @Range: 10 500
363 : // @Increment: 1
364 : // @User: Standard
365 : AP_GROUPINFO("ACRO_RLL_RATE", 11, QuadPlane, acro_roll_rate, 360),
366 :
367 : // @Param: ACRO_PIT_RATE
368 : // @DisplayName: QACRO mode pitch rate
369 : // @Description: The maximum pitch rate at full stick deflection in QACRO mode
370 : // @Units: deg/s
371 : // @Range: 10 500
372 : // @Increment: 1
373 : // @User: Standard
374 : AP_GROUPINFO("ACRO_PIT_RATE", 12, QuadPlane, acro_pitch_rate, 180),
375 :
376 : // @Param: ACRO_YAW_RATE
377 : // @DisplayName: QACRO mode yaw rate
378 : // @Description: The maximum yaw rate at full stick deflection in QACRO mode
379 : // @Units: deg/s
380 : // @Range: 10 500
381 : // @Increment: 1
382 : // @User: Standard
383 : AP_GROUPINFO("ACRO_YAW_RATE", 13, QuadPlane, acro_yaw_rate, 90),
384 :
385 : // @Param: TKOFF_FAIL_SCL
386 : // @DisplayName: Takeoff time failure scalar
387 : // @Description: Scalar for how long past the expected takeoff time a takeoff should be considered as failed and the vehicle will switch to QLAND. If set to 0 there is no limit on takeoff time.
388 : // @Range: 1.1 5.0
389 : // @Increment: 5.1
390 : // @User: Advanced
391 : AP_GROUPINFO("TKOFF_FAIL_SCL", 14, QuadPlane, takeoff_failure_scalar, 0),
392 :
393 : // @Param: TKOFF_ARSP_LIM
394 : // @DisplayName: Takeoff airspeed limit
395 : // @Description: Airspeed limit during takeoff. If the airspeed exceeds this level the vehicle will switch to QLAND. This is useful for ensuring that you don't takeoff into excessively strong wind. If set to 0 there is no limit on airspeed during takeoff.
396 : // @Units: m/s
397 : // @Range: 0 20
398 : // @Increment: 1
399 : // @User: Advanced
400 : AP_GROUPINFO("TKOFF_ARSP_LIM", 15, QuadPlane, maximum_takeoff_airspeed_ms, 0),
401 :
402 : // @Param: ASSIST_ALT
403 : // @DisplayName: Quadplane assistance altitude
404 : // @Description: This is the altitude below which quadplane assistance will be triggered. This acts the same way as Q_ASSIST_ANGLE and Q_ASSIST_SPEED, but triggers if the aircraft drops below the given altitude while the VTOL motors are not running. A value of zero disables this feature. The altitude is calculated as being above ground level. The height above ground is given from a Lidar used if available and RNGFND_LANDING=1. Otherwise it comes from terrain data if TERRAIN_FOLLOW=1 and comes from height above home otherwise.
405 : // @Units: m
406 : // @Range: 0 120
407 : // @Increment: 1
408 : // @User: Standard
409 : AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist.alt, 0),
410 :
411 : // 17: TAILSIT_GSCMSK
412 : // 18: TAILSIT_GSCMIN
413 :
414 : // @Param: ASSIST_DELAY
415 : // @DisplayName: Quadplane assistance delay
416 : // @Description: This is delay between the assistance thresholds being met and the assistance starting.
417 : // @Units: s
418 : // @Range: 0 2
419 : // @Increment: 0.1
420 : // @User: Standard
421 : AP_GROUPINFO("ASSIST_DELAY", 19, QuadPlane, assist.delay, 0.5),
422 :
423 : // @Param: FWD_MANTHR_MAX
424 : // @DisplayName: VTOL manual forward throttle max percent
425 : // @Description: Maximum value for manual forward throttle; used with RC option FWD_THR (209)
426 : // @Range: 0 100
427 : AP_GROUPINFO("FWD_MANTHR_MAX", 20, QuadPlane, fwd_thr_max, 0),
428 :
429 : // 21: TAILSIT_DSKLD
430 : // 22: TILT_FIX_ANGLE
431 : // 23: TILT_FIX_GAIN
432 : // 24: TAILSIT_RAT_FW
433 : // 25: TAILSIT_RAT_VT
434 :
435 : // @Group: TAILSIT_
436 : // @Path: tailsitter.cpp
437 : AP_SUBGROUPINFO(tailsitter, "TAILSIT_", 26, QuadPlane, Tailsitter),
438 :
439 : // @Group: TILT_
440 : // @Path: tiltrotor.cpp
441 : AP_SUBGROUPINFO(tiltrotor, "TILT_", 27, QuadPlane, Tiltrotor),
442 :
443 : // @Param: BACKTRANS_MS
444 : // @DisplayName: SLT and Tiltrotor back transition pitch limit duration
445 : // @Description: Pitch angle will increase from 0 to angle max over this duration when switching into VTOL flight in a position control mode. 0 Disables.
446 : // @Units: ms
447 : // @Range: 0 10000
448 : AP_GROUPINFO("BACKTRANS_MS", 28, QuadPlane, back_trans_pitch_limit_ms, 3000),
449 :
450 : // @Param: TRANS_FAIL_ACT
451 : // @DisplayName: Quadplane transition failure action
452 : // @Description: This sets the mode that is changed to when Q_TRANS_FAIL time elapses, if set. See also Q_OPTIONS bit 19: CompleteTransition if Q_TRANS_FAIL
453 : // @Values: -1:Warn only, 0:QLand, 1:QRTL
454 : AP_GROUPINFO("TRANS_FAIL_ACT", 29, QuadPlane, transition_failure.action, 0),
455 :
456 : // @Group: WVANE_
457 : // @Path: ../libraries/AC_AttitudeControl/AC_WeatherVane.cpp
458 : AP_SUBGROUPPTR(weathervane, "WVANE_", 30, QuadPlane, AC_WeatherVane),
459 :
460 : // @Param: LAND_ALTCHG
461 : // @DisplayName: Land detection altitude change threshold
462 : // @Description: The maximum altitude change allowed during land detection. You can raise this value if you find that landing detection takes a long time to complete. It is the maximum change in altitude over a period of 4 seconds for landing to be detected
463 : // @Units: m
464 : // @Range: 0.1 0.6
465 : // @Increment: 0.05
466 : // @User: Standard
467 : AP_GROUPINFO("LAND_ALTCHG", 31, QuadPlane, landing_detect.detect_alt_change_m, 0.2),
468 :
469 : // @Param: NAVALT_MIN
470 : // @DisplayName: Minimum navigation altitude
471 : // @Description: This is the altitude in meters above which navigation begins in auto takeoff. Below this altitude the target roll and pitch will be zero. A value of zero disables the feature
472 : // @Units: m
473 : // @Range: 0 5
474 : // @User: Advanced
475 : AP_GROUPINFO("NAVALT_MIN", 32, QuadPlane, takeoff_navalt_min_m, 0),
476 :
477 : // @Param: PLT_Y_RATE
478 : // @DisplayName: Pilot controlled yaw rate
479 : // @Description: Pilot controlled yaw rate max. Used in all pilot controlled modes except QAcro
480 : // @Units: deg/s
481 : // @Range: 1 360
482 : // @User: Standard
483 :
484 : // @Param: PLT_Y_EXPO
485 : // @DisplayName: Pilot controlled yaw expo
486 : // @Description: Pilot controlled yaw expo to allow faster rotation when stick at edges
487 : // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
488 : // @Range: -0.5 1.0
489 : // @User: Advanced
490 :
491 : // @Param: PLT_Y_RATE_TC
492 : // @DisplayName: Pilot yaw rate control input time constant
493 : // @Description: Pilot yaw rate control input time constant. Low numbers lead to sharper response, higher numbers to softer response.
494 : // @Units: s
495 : // @Range: 0 1
496 : // @Increment: 0.01
497 : // @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
498 : // @User: Standard
499 : AP_SUBGROUPINFO(command_model_pilot, "PLT_Y_", 33, QuadPlane, AC_CommandModel),
500 :
501 : // @Param: RTL_ALT_MIN
502 : // @DisplayName: QRTL minimum altitude
503 : // @Description: If VTOL motors are active QRTL mode will VTOL climb to at least this altitude before returning home. If outside 150% the larger of WP_LOITER_RAD and RTL_RADIUS the vehicle will VTOL climb to Q_RTL_ALT. This parameter has no effect if the vehicle is in forward flight. Should be between Q_LAND_FINAL_ALT and Q_RTL_ALT
504 : // @Units: m
505 : // @Range: 1 200
506 : // @Increment: 1
507 : // @User: Standard
508 : AP_GROUPINFO("RTL_ALT_MIN", 34, QuadPlane, qrtl_alt_min_m, 10),
509 :
510 : // @Param: FWD_THR_GAIN
511 : // @DisplayName: Q mode fwd throttle gain
512 : // @Description: This parameter sets the gain from forward accel/tilt to forward throttle in certain Q modes. The Q modes this feature operates in is controlled by the Q_FWD_THR_USE parameter. Vehicles using separate forward thrust motors, eg quadplanes, should set this parameter to (all up weight) / (maximum combined thrust of forward motors) with a value of 2 being typical. Vehicles that tilt lifting rotors to provide forward thrust should set this parameter to (all up weight) / (weight lifted by tilting rotors) which for most aircraft can be approximated as (total number of lifting rotors) / (number of lifting rotors that tilt). When using this method of forward throttle control, the forward tilt angle limit is controlled by the Q_FWD_PIT_LIM parameter.
513 : // @Range: 0.0 5.0
514 : // @Increment: 0.1
515 : // @User: Standard
516 : AP_GROUPINFO("FWD_THR_GAIN", 35, QuadPlane, q_fwd_thr_gain, 2.0f),
517 :
518 : // @Param: FWD_PIT_LIM
519 : // @DisplayName: Q mode forward pitch limit
520 : // @Description: When forward throttle is being controlled by the Q_FWD_THR_GAIN parameter in Q modes, the vehicle forward (nose down) pitch rotation will be limited to the value specified by this parameter and the any additional forward acceleration required will be produced by use of the forward thrust motor(s) or tilting of moveable rotors. Larger values allow the vehicle to pitch more nose down. Set initially to the amount of nose down pitch required to remove wing lift.
521 : // @Units: deg
522 : // @Range: 0.0 5.0
523 : // @Increment: 0.1
524 : // @User: Standard
525 : AP_GROUPINFO("FWD_PIT_LIM", 36, QuadPlane, q_fwd_pitch_lim, 3.0f),
526 :
527 : // @Param: FWD_THR_USE
528 : // @DisplayName: Q mode forward throttle use
529 : // @Description: This parameter determines when the feature that uses forward throttle instead of forward tilt is used. The amount of forward throttle is controlled by the Q_FWD_THR_GAIN parameter. The maximum amount of forward pitch allowed is controlled by the Q_FWD_PIT_LIM parameter. Q_FWD_THR_USE = 0 disables the feature. Q_FWD_THR_USE = 1 enables the feature in all position controlled modes such as QLOITER, QLAND, QRTL and VTOL TAKEOFF. Q_FWD_THR_USE = 2 enables the feature in all Q modes except QAUTOTUNE and QACRO. When enabling the feature, the legacy method of controlling forward throttle use via velocity controller error should be disabled by setting Q_VFWD_GAIN to 0. Do not use this feature with tailsitters.
530 : // @Values: 0:Off,1:On in all position controlled Q modes,2:On in all Q modes except QAUTOTUNE and QACRO
531 : // @User: Standard
532 : AP_GROUPINFO("FWD_THR_USE", 37, QuadPlane, q_fwd_thr_use, uint8_t(FwdThrUse::OFF)),
533 :
534 : // @Param: BCK_PIT_LIM
535 : // @DisplayName: Q mode rearward pitch limit
536 : // @Description: This sets the maximum number of degrees of back or pitch up in Q modes when the airspeed is at AIRSPEED_MIN, and is used to prevent excessive sutructural loads when pitching up decelerate. If airspeed is above or below AIRSPEED_MIN, the pitch up/back will be adjusted according to the formula pitch_limit = Q_BCK_PIT_LIM * (AIRSPEED_MIN / IAS)^2. The backwards/up pitch limit controlled by this parameter is in addition to limiting applied by PTCH_LIM_MAX_DEG and Q_A_ANGLE_MAX. The BCK_PIT_LIM limit is only applied when Q_FWD_THR_USE is set to 1 or 2 and the vehicle is flying in a mode that uses forward throttle instead of forward tilt to generate forward speed. Set to a non positive value 0 to deactivate this limit.
537 : // @Units: deg
538 : // @Range: 0.0 15.0
539 : // @Increment: 0.1
540 : // @User: Standard
541 : AP_GROUPINFO("BCK_PIT_LIM", 38, QuadPlane, q_bck_pitch_lim, 10.0f),
542 :
543 : // @Param: APPROACH_DIST
544 : // @DisplayName: Q mode approach distance
545 : // @Description: The minimum distance from the destination to use the fixed wing airbrake and approach code for landing approach. This is useful if you don't want the fixed wing approach logic to be used when you are close to the destination. Set to zero to always use fixed wing approach.
546 : // @Units: m
547 : // @Range: 0.0 1000
548 : // @Increment: 1
549 : // @User: Standard
550 : AP_GROUPINFO("APPROACH_DIST", 39, QuadPlane, approach_distance_m, 0),
551 :
552 : #if HAL_WITH_ESC_TELEM
553 : // @Param: TKOFF_RPM_MIN
554 : // @DisplayName: Auto Takeoff Check RPM minimum
555 : // @Description: Auto takeoff (in Auto or Guided) is not permitted until motors report at least this RPM. Set to zero to disable check (which also disables Q_TKOFF_RPM_MAX).
556 : // @Range: 0 10000
557 : // @User: Standard
558 : AP_GROUPINFO("TKOFF_RPM_MIN", 40, QuadPlane, takeoff_rpm_min, 0),
559 :
560 : // @Param: TKOFF_RPM_MAX
561 : // @DisplayName: Auto Takeoff Check RPM maximum
562 : // @Description: Auto takeoff (in Auto or Guided) is not permitted until motors report no more than this RPM. Set to zero to disable check. Q_TKOFF_RPM_MIN must also be set to a non-zero value to enable this check.
563 : // @Range: 0 10000
564 : // @User: Standard
565 : AP_GROUPINFO("TKOFF_RPM_MAX", 41, QuadPlane, takeoff_rpm_max, 0),
566 : #endif
567 :
568 : // @Param: THRST_LOSS_OPT
569 : // @DisplayName: VTOL thrust loss detection options
570 : // @Description: Configuration for thrust loss detection detection, this results in "Potential VTOL Thrust Loss" warnings. This detection also allows the motor mixer to ignore the failed motor allowing better use of the remaining motors.
571 : // @Bitmask: 0: Disable thrust loss detection.
572 : // @Bitmask: 1: Disable thrust loss detection in transtions and fixed wing modes. Thrust loss detection will only run in VTOL modes.
573 : AP_GROUPINFO("THRST_LOSS_OPT", 42, QuadPlane, thrust_loss.options, 0),
574 :
575 : AP_GROUPEND
576 : };
577 :
578 : /*
579 : defaults for all quadplanes
580 : */
581 : static const struct AP_Param::defaults_table_struct defaults_table[] = {
582 : { "Q_A_RAT_RLL_P", 0.25 },
583 : { "Q_A_RAT_RLL_I", 0.25 },
584 : { "Q_A_RAT_RLL_FLTD", 10.0 },
585 : { "Q_A_RAT_RLL_SMAX", 50.0 },
586 : { "Q_A_RAT_PIT_P", 0.25 },
587 : { "Q_A_RAT_PIT_I", 0.25 },
588 : { "Q_A_RAT_PIT_FLTD", 10.0 },
589 : { "Q_A_RAT_PIT_SMAX", 50.0 },
590 : { "Q_A_RAT_YAW_SMAX", 50.0 },
591 : { "Q_A_RATE_R_MAX", 75.0 },
592 : { "Q_A_RATE_P_MAX", 75.0 },
593 : { "Q_A_RATE_Y_MAX", 75.0 },
594 : { "Q_M_SPOOL_TIME", 0.25 },
595 : { "Q_LOIT_ANG_MAX", 15.0 },
596 : { "Q_LOIT_ACC_MAX_M", 2.50 },
597 : { "Q_LOIT_BRK_ACC_M", 0.5 },
598 : { "Q_LOIT_BRK_JRK_M", 2.5 },
599 : { "Q_LOIT_SPEED_MS", 5.0 },
600 : { "Q_WP_SPD", 5.0 },
601 : { "Q_WP_ACC", 1.0 },
602 : { "Q_P_NE_JERK", 2 },
603 : // lower rotational accel limits
604 : { "Q_A_ACC_R_MAX", 400 },
605 : { "Q_A_ACC_P_MAX", 400 },
606 : { "Q_A_ACC_Y_MAX", 100 },
607 : };
608 :
609 692 : QuadPlane::QuadPlane(AP_AHRS &_ahrs) :
610 692 : ahrs(_ahrs)
611 : {
612 692 : AP_Param::setup_object_defaults(this, var_info);
613 692 : AP_Param::setup_object_defaults(this, var_info2);
614 :
615 692 : if (_singleton != nullptr) {
616 0 : AP_HAL::panic("Can only be one Quadplane");
617 : }
618 692 : _singleton = this;
619 692 : }
620 :
621 :
622 : // setup default motors for the frame class
623 191 : void QuadPlane::setup_default_channels(uint8_t num_motors)
624 : {
625 961 : for (uint8_t i=0; i<num_motors; i++) {
626 770 : SRV_Channels::set_aux_channel_default(SRV_Channels::get_motor_function(i), CH_5+i);
627 : }
628 191 : }
629 :
630 :
631 10606814 : bool QuadPlane::setup(void)
632 : {
633 10606814 : if (initialised) {
634 8732932 : return true;
635 : }
636 1873882 : if (!enable || hal.util->get_soft_armed()) {
637 1873681 : return false;
638 : }
639 :
640 201 : if (hal.util->available_memory() <
641 : 4096 + sizeof(*motors) + sizeof(*attitude_control) + sizeof(*pos_control) + sizeof(*wp_nav) + sizeof(*ahrs_view) + sizeof(*loiter_nav) + sizeof(*weathervane)) {
642 0 : AP_BoardConfig::config_error("Not enough memory for quadplane");
643 : }
644 :
645 : /*
646 : dynamically allocate the key objects for quadplane. This ensures
647 : that the objects don't affect the vehicle unless enabled and
648 : also saves memory when not in use
649 : */
650 201 : switch ((AP_Motors::motor_frame_class)frame_class) {
651 189 : case AP_Motors::MOTOR_FRAME_QUAD:
652 189 : setup_default_channels(4);
653 189 : break;
654 0 : case AP_Motors::MOTOR_FRAME_HEXA:
655 0 : setup_default_channels(6);
656 0 : break;
657 0 : case AP_Motors::MOTOR_FRAME_OCTA:
658 : case AP_Motors::MOTOR_FRAME_OCTAQUAD:
659 0 : setup_default_channels(8);
660 0 : break;
661 2 : case AP_Motors::MOTOR_FRAME_Y6:
662 2 : setup_default_channels(7);
663 2 : break;
664 0 : case AP_Motors::MOTOR_FRAME_DECA:
665 0 : setup_default_channels(10);
666 0 : break;
667 9 : case AP_Motors::MOTOR_FRAME_TRI:
668 9 : SRV_Channels::set_default_function(CH_5, SRV_Channel::k_motor1);
669 9 : SRV_Channels::set_default_function(CH_6, SRV_Channel::k_motor2);
670 9 : SRV_Channels::set_default_function(CH_8, SRV_Channel::k_motor4);
671 9 : SRV_Channels::set_default_function(CH_11, SRV_Channel::k_motor7);
672 9 : AP_Param::set_frame_type_flags(AP_PARAM_FRAME_TRICOPTER);
673 9 : break;
674 1 : case AP_Motors::MOTOR_FRAME_TAILSITTER:
675 : case AP_Motors::MOTOR_FRAME_SCRIPTING_MATRIX:
676 : case AP_Motors::MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX:
677 1 : break;
678 0 : default:
679 0 : AP_BoardConfig::config_error("Unsupported Q_FRAME_CLASS %u", (unsigned int)(frame_class.get()));
680 : }
681 :
682 : // Make sure not both a tailsiter and tiltrotor
683 201 : if ((tailsitter.enable > 0) && (tiltrotor.enable > 0)) {
684 0 : AP_BoardConfig::config_error("set TAILSIT_ENABLE 0 or TILT_ENABLE 0");
685 : }
686 :
687 201 : switch ((AP_Motors::motor_frame_class)frame_class) {
688 : #if AP_MOTORS_TRI_ENABLED
689 9 : case AP_Motors::MOTOR_FRAME_TRI:
690 9 : motors = NEW_NOTHROW AP_MotorsTri(rc_speed);
691 9 : motors_var_info = AP_MotorsTri::var_info;
692 9 : break;
693 : #endif // AP_MOTORS_TRI_ENABLED
694 1 : case AP_Motors::MOTOR_FRAME_TAILSITTER:
695 : // this is a duo-motor tailsitter
696 1 : tailsitter.tailsitter_motors = NEW_NOTHROW AP_MotorsTailsitter(rc_speed);
697 1 : motors = tailsitter.tailsitter_motors;
698 1 : motors_var_info = AP_MotorsTailsitter::var_info;
699 1 : break;
700 0 : case AP_Motors::MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX:
701 : #if AP_SCRIPTING_ENABLED
702 0 : motors = NEW_NOTHROW AP_MotorsMatrix_Scripting_Dynamic(plane.scheduler.get_loop_rate_hz());
703 0 : motors_var_info = AP_MotorsMatrix_Scripting_Dynamic::var_info;
704 : #endif // AP_SCRIPTING_ENABLED
705 0 : break;
706 191 : default:
707 191 : motors = NEW_NOTHROW AP_MotorsMatrix(rc_speed);
708 191 : motors_var_info = AP_MotorsMatrix::var_info;
709 191 : break;
710 : }
711 :
712 201 : if (!motors) {
713 0 : AP_BoardConfig::allocation_error("motors");
714 : }
715 :
716 201 : AP_Param::load_object_from_eeprom(motors, motors_var_info);
717 :
718 : // create the attitude view used by the VTOL code
719 201 : ahrs_view = ahrs.create_view((tailsitter.enable > 0) ? ROTATION_PITCH_90 : ROTATION_NONE, ahrs_trim_pitch);
720 201 : if (ahrs_view == nullptr) {
721 0 : AP_BoardConfig::allocation_error("ahrs_view");
722 : }
723 :
724 201 : attitude_control = NEW_NOTHROW AC_AttitudeControl_TS(*ahrs_view, *motors);
725 201 : if (!attitude_control) {
726 0 : AP_BoardConfig::allocation_error("attitude_control");
727 : }
728 :
729 201 : AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info);
730 201 : pos_control = NEW_NOTHROW AC_PosControl(*ahrs_view, *motors, *attitude_control);
731 201 : if (!pos_control) {
732 0 : AP_BoardConfig::allocation_error("pos_control");
733 : }
734 201 : AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info);
735 201 : wp_nav = NEW_NOTHROW AC_WPNav(*ahrs_view, *pos_control, *attitude_control);
736 201 : if (!wp_nav) {
737 0 : AP_BoardConfig::allocation_error("wp_nav");
738 : }
739 201 : AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info);
740 :
741 201 : loiter_nav = NEW_NOTHROW AC_Loiter(*ahrs_view, *pos_control, *attitude_control);
742 201 : if (!loiter_nav) {
743 0 : AP_BoardConfig::allocation_error("loiter_nav");
744 : }
745 201 : AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info);
746 :
747 201 : weathervane = NEW_NOTHROW AC_WeatherVane();
748 201 : if (!weathervane) {
749 0 : AP_BoardConfig::allocation_error("weathervane");
750 : }
751 201 : AP_Param::load_object_from_eeprom(weathervane, weathervane->var_info);
752 :
753 201 : motors->init(frame_class, frame_type);
754 201 : motors->update_throttle_range();
755 201 : motors->set_update_rate(rc_speed);
756 201 : attitude_control->parameter_sanity_check();
757 :
758 : // setup the trim of any motors used by AP_Motors so I/O board
759 : // failsafe will disable motors
760 201 : uint32_t mask = plane.quadplane.motors->get_motor_mask();
761 201 : hal.rcout->set_failsafe_pwm(mask, plane.quadplane.motors->get_pwm_output_min());
762 :
763 : // default QAssist state as set with Q_OPTIONS
764 201 : if (option_is_set(QuadPlane::Option::Q_ASSIST_FORCE_ENABLE)) {
765 0 : assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
766 : }
767 :
768 201 : setup_defaults();
769 :
770 : // PARAMETER_CONVERSION - Added: Jan-2024 for ArduPlane-4.5
771 201 : land_final_speed_ms.convert_centi_parameter(AP_PARAM_INT16);
772 201 : pilot_speed_z_max_up_ms.convert_centi_parameter(AP_PARAM_INT16);
773 201 : pilot_speed_z_max_dn_ms.convert_centi_parameter(AP_PARAM_INT16);
774 201 : pilot_accel_z_mss.convert_centi_parameter(AP_PARAM_INT16);
775 :
776 : // upgrade attitude controller parameters
777 201 : attitude_control->convert_parameters();
778 :
779 : // upgrade position controller parameters added Dec 2025
780 201 : pos_control->convert_parameters();
781 :
782 : // upgrade waypoint navigation parameters
783 201 : wp_nav->convert_parameters();
784 :
785 : // upgrade loiter navigation parameters
786 201 : loiter_nav->convert_parameters();
787 :
788 : // Provisionally assign the SLT thrust type.
789 : // It will be overwritten by tailsitter or tiltorotor setups.
790 201 : thrust_type = ThrustType::SLT;
791 :
792 201 : tailsitter.setup();
793 :
794 201 : tiltrotor.setup();
795 :
796 201 : if (!transition) {
797 182 : transition = NEW_NOTHROW SLT_Transition(*this, motors);
798 : }
799 201 : if (!transition) {
800 0 : AP_BoardConfig::allocation_error("transition");
801 : }
802 :
803 : // init wp_nav variables after defaults are setup
804 201 : wp_nav->wp_and_spline_init_m();
805 :
806 201 : transition->force_transition_complete();
807 :
808 : // param count will have changed
809 201 : AP_Param::invalidate_count();
810 :
811 : char frame_and_type_string[30];
812 201 : motors->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
813 201 : gcs().send_text(MAV_SEVERITY_INFO, "QuadPlane initialised, %s", frame_and_type_string);
814 201 : initialised = true;
815 201 : return true;
816 : }
817 :
818 : /*
819 : setup default parameters from defaults_table
820 : */
821 201 : void QuadPlane::setup_defaults(void)
822 : {
823 201 : AP_Param::set_defaults_from_table(defaults_table, ARRAY_SIZE(defaults_table));
824 :
825 : // reset ESC calibration
826 201 : if (esc_calibration != 0) {
827 0 : esc_calibration.set_and_save(0);
828 : }
829 : // Quadplanes need the same level of GPS error checking as Copters do, Plane is more relaxed
830 201 : AP_Param::set_default_by_name("EK2_CHECK_SCALE",100);
831 201 : AP_Param::set_default_by_name("EK3_CHECK_SCALE",100);
832 :
833 201 : }
834 :
835 : // run ESC calibration
836 0 : void QuadPlane::run_esc_calibration(void)
837 : {
838 0 : if (!motors->armed()) {
839 0 : motors->set_throttle_passthrough_for_esc_calibration(0);
840 0 : AP_Notify::flags.esc_calibration = false;
841 0 : return;
842 : }
843 0 : if (!AP_Notify::flags.esc_calibration) {
844 0 : gcs().send_text(MAV_SEVERITY_INFO, "Starting ESC calibration");
845 : }
846 0 : AP_Notify::flags.esc_calibration = true;
847 0 : switch (esc_calibration) {
848 0 : case 1:
849 : // throttle based calibration
850 0 : motors->set_throttle_passthrough_for_esc_calibration(plane.get_throttle_input() * 0.01f);
851 0 : break;
852 0 : case 2:
853 : // full range calibration
854 0 : motors->set_throttle_passthrough_for_esc_calibration(1);
855 0 : break;
856 : }
857 : }
858 :
859 : /*
860 : ask the multicopter attitude control to match the roll and pitch rates being demanded by the
861 : fixed wing controller if not in a pure VTOL mode
862 : */
863 588128 : void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
864 : {
865 588128 : bool use_multicopter_control = in_vtol_mode() && !tailsitter.in_vtol_transition() && !force_fw_control_recovery;
866 588128 : bool use_yaw_target = false;
867 :
868 588128 : float yaw_target_cd = 0.0;
869 611189 : if (!use_multicopter_control && transition->update_yaw_target(yaw_target_cd) &&
870 23061 : !force_fw_control_recovery) {
871 23061 : use_multicopter_control = true;
872 23061 : use_yaw_target = true;
873 : }
874 :
875 : // normal control modes for VTOL and FW flight
876 : // tailsitter in transition to VTOL flight is not really in a VTOL mode yet
877 588128 : if (use_multicopter_control) {
878 :
879 : // Pilot input, use yaw rate time constant
880 174078 : set_pilot_yaw_rate_time_constant();
881 :
882 : // tailsitter-only body-frame roll control options
883 : // Angle mode attitude control for pitch and body-frame roll, rate control for euler yaw.
884 174078 : if (tailsitter.enabled() &&
885 0 : (tailsitter.input_type & Tailsitter::input::TAILSITTER_INPUT_BF_ROLL)) {
886 :
887 0 : if (!(tailsitter.input_type & Tailsitter::input::TAILSITTER_INPUT_PLANE)) {
888 : // In multicopter input mode, the roll and yaw stick axes are independent of pitch
889 0 : attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_cd(false,
890 0 : plane.nav_roll_cd,
891 0 : plane.nav_pitch_cd,
892 : yaw_rate_cds);
893 0 : return;
894 : } else {
895 : // In plane input mode, the roll and yaw sticks are swapped
896 : // and their effective axes rotate from yaw to roll and vice versa
897 : // as pitch goes from zero to 90.
898 : // So it is necessary to also rotate their scaling.
899 :
900 : // Get the roll angle and yaw rate limits
901 0 : int16_t roll_limit = attitude_control->lean_angle_max_cd();
902 : // separate limit for tailsitter roll, if set
903 0 : if (tailsitter.max_roll_angle > 0) {
904 0 : roll_limit = tailsitter.max_roll_angle * 100.0f;
905 : }
906 : // Prevent a divide by zero
907 0 : const float yaw_rate_max = command_model_pilot.get_rate();
908 0 : float yaw_rate_limit = ((yaw_rate_max < 1.0f) ? 1 : yaw_rate_max) * 100.0f;
909 0 : float yaw2roll_scale = roll_limit / yaw_rate_limit;
910 :
911 : // Rotate as a function of Euler pitch and swap roll/yaw
912 0 : float euler_pitch = radians(.01f * plane.nav_pitch_cd);
913 0 : float spitch = fabsf(sinf(euler_pitch));
914 0 : float y2r_scale = linear_interpolate(1, yaw2roll_scale, spitch, 0, 1);
915 :
916 0 : float p_yaw_rate = plane.nav_roll_cd / y2r_scale;
917 0 : float p_roll_angle = -y2r_scale * yaw_rate_cds;
918 :
919 0 : attitude_control->input_euler_rate_yaw_euler_angle_pitch_bf_roll_cd(true,
920 : p_roll_angle,
921 0 : plane.nav_pitch_cd,
922 : p_yaw_rate);
923 0 : return;
924 : }
925 : }
926 :
927 : // note this is actually in deg/s for some SID_AXIS values for yaw
928 174078 : Vector3f offset_deg;
929 :
930 : #if AP_PLANE_SYSTEMID_ENABLED
931 174078 : auto &systemid = plane.g2.systemid;
932 174078 : systemid.vtol_update();
933 174078 : offset_deg = systemid.get_attitude_offset_deg();
934 : #endif
935 :
936 174078 : if (use_yaw_target) {
937 23061 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(plane.nav_roll_cd + offset_deg.x*100,
938 23061 : plane.nav_pitch_cd + offset_deg.y*100,
939 23061 : yaw_target_cd + offset_deg.z*100,
940 : true);
941 : } else {
942 : // use euler angle attitude control
943 151017 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(plane.nav_roll_cd + offset_deg.x*100,
944 151017 : plane.nav_pitch_cd + offset_deg.y*100,
945 151017 : yaw_rate_cds + offset_deg.z*100);
946 : }
947 : } else {
948 : // use the fixed wing desired rates
949 414050 : Vector3f bf_input_cd { plane.rollController.get_pid_info().target * 100.0f,
950 828100 : plane.pitchController.get_pid_info().target * 100.0f,
951 414050 : yaw_rate_cds };
952 :
953 : // rotate into multicopter attitude reference frame
954 414050 : ahrs_view->rotate(bf_input_cd);
955 :
956 : // disable yaw time constant for 1:1 match of desired rates
957 414050 : disable_yaw_rate_time_constant();
958 :
959 414050 : attitude_control->input_rate_bf_roll_pitch_yaw_no_shaping_cds(bf_input_cd.x, bf_input_cd.y, bf_input_cd.z);
960 : }
961 : }
962 :
963 : // hold in stabilize with given throttle
964 220611 : void QuadPlane::hold_stabilize(float throttle_in)
965 : {
966 : // call attitude controller
967 220611 : multicopter_attitude_rate_update(get_desired_yaw_rate_cds(false));
968 :
969 220611 : if ((throttle_in <= 0) && !air_mode_active()) {
970 7157 : set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
971 7157 : attitude_control->set_throttle_out(0, true, 0);
972 7157 : relax_attitude_control();
973 : } else {
974 213454 : set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
975 213454 : bool should_boost = true;
976 213454 : if (tailsitter.enabled() && assisted_flight) {
977 : // tailsitters in forward flight should not use angle boost
978 2256 : should_boost = false;
979 : }
980 : #if AP_PLANE_SYSTEMID_ENABLED
981 213454 : throttle_in += plane.g2.systemid.get_throttle_offset();
982 : #endif
983 213454 : attitude_control->set_throttle_out(throttle_in, should_boost, 0);
984 : }
985 220611 : }
986 :
987 : // run the multicopter Z controller
988 2534010 : void QuadPlane::run_z_controller(void)
989 : {
990 2534010 : if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED ) {
991 241682 : return;
992 : }
993 2292328 : const uint32_t now = AP_HAL::millis();
994 2292328 : if (tailsitter.in_vtol_transition(now)) {
995 : // never run Z controller in tailsitter transition
996 790 : return;
997 : }
998 2291538 : if ((now - last_pidz_active_ms) > 20 || !pos_control->D_is_active()) {
999 : // set vertical speed and acceleration limits
1000 : // All limits must be positive
1001 264 : pos_control->D_set_max_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss);
1002 :
1003 : // initialise the vertical position controller
1004 264 : if (!tailsitter.enabled()) {
1005 258 : pos_control->D_init_controller();
1006 : } else {
1007 : // initialise the vertical position controller with no descent
1008 6 : pos_control->D_init_controller_no_descent();
1009 : }
1010 264 : last_pidz_init_ms = now;
1011 : }
1012 2291538 : last_pidz_active_ms = now;
1013 2291538 : pos_control->D_update_controller();
1014 : }
1015 :
1016 318754 : void QuadPlane::relax_attitude_control()
1017 : {
1018 : // disable roll and yaw control for vectored tailsitters
1019 : // if not a vectored tailsitter completely disable attitude control
1020 318754 : attitude_control->relax_attitude_controllers(!tailsitter.relax_pitch());
1021 318754 : }
1022 :
1023 1123645 : void QuadPlane::set_climb_rate_ms(float target_climb_rate_ms)
1024 : {
1025 1123645 : float vel_d_m = -target_climb_rate_ms;
1026 1123645 : pos_control->input_vel_accel_D_m(vel_d_m, 0, false);
1027 1123645 : }
1028 :
1029 : /*
1030 : hold hover with target climb rate
1031 : */
1032 367517 : void QuadPlane::hold_hover(float target_climb_rate_cms)
1033 : {
1034 : // motors use full range
1035 367517 : set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1036 :
1037 : // set vertical speed and acceleration limits
1038 : // All limits must be positive
1039 367517 : pos_control->D_set_max_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss);
1040 :
1041 : // call attitude controller
1042 367517 : multicopter_attitude_rate_update(get_desired_yaw_rate_cds(false));
1043 :
1044 : // call position controller
1045 367517 : set_climb_rate_ms(target_climb_rate_cms * 0.01);
1046 :
1047 367517 : run_z_controller();
1048 367517 : }
1049 :
1050 26448 : float QuadPlane::get_pilot_throttle()
1051 : {
1052 : // get scaled throttle input
1053 26448 : float throttle_in = plane.channel_throttle->get_control_in();
1054 :
1055 : // normalize to [0,1]
1056 26448 : throttle_in /= plane.channel_throttle->get_range();
1057 :
1058 26448 : if (is_positive(throttle_expo)) {
1059 : // get hover throttle level [0,1]
1060 26448 : float thr_mid = motors->get_throttle_hover();
1061 26448 : float thrust_curve_expo = constrain_float(throttle_expo, 0.0f, 1.0f);
1062 :
1063 : // this puts mid stick at hover throttle
1064 26448 : return throttle_curve(thr_mid, thrust_curve_expo, throttle_in);
1065 : } else {
1066 0 : return throttle_in;
1067 : }
1068 : }
1069 :
1070 : /*
1071 : get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle.
1072 : The angle_max_cd and angle_limit_cd are mode dependent
1073 : */
1074 636619 : void QuadPlane::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const
1075 : {
1076 : // failsafe check
1077 636619 : if (plane.failsafe.rc_failsafe || plane.failsafe.throttle_counter > 0) {
1078 0 : roll_out_cd = 0;
1079 0 : pitch_out_cd = 0;
1080 0 : return;
1081 : }
1082 :
1083 : // fetch roll and pitch inputs
1084 636619 : roll_out_cd = plane.channel_roll->get_control_in();
1085 636619 : pitch_out_cd = plane.channel_pitch->get_control_in();
1086 :
1087 : // limit max lean angle, always allow for 10 degrees
1088 636619 : angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd);
1089 :
1090 : // scale roll and pitch inputs to ANGLE_MAX parameter range
1091 636619 : float scaler = angle_max_cd/4500.0;
1092 636619 : roll_out_cd *= scaler;
1093 636619 : pitch_out_cd *= scaler;
1094 :
1095 : // apply circular limit
1096 636619 : float total_in = norm(pitch_out_cd, roll_out_cd);
1097 636619 : if (total_in > angle_limit_cd) {
1098 217 : float ratio = angle_limit_cd / total_in;
1099 217 : roll_out_cd *= ratio;
1100 217 : pitch_out_cd *= ratio;
1101 : }
1102 :
1103 : // apply lateral tilt to euler roll conversion
1104 636619 : roll_out_cd = 100 * degrees(atanf(cosf(cd_to_rad(pitch_out_cd)) * tanf(cd_to_rad(roll_out_cd))));
1105 : }
1106 :
1107 : /*
1108 : get pilot throttle in for landing code. Return value on scale of 0 to 1
1109 : */
1110 0 : float QuadPlane::get_pilot_land_throttle(void) const
1111 : {
1112 0 : if (plane.rc_failsafe_active()) {
1113 : // assume zero throttle if lost RC
1114 0 : return 0;
1115 : }
1116 : // get scaled throttle input
1117 0 : float throttle_in = plane.channel_throttle->get_control_in();
1118 :
1119 : // normalize to [0,1]
1120 0 : throttle_in /= plane.channel_throttle->get_range();
1121 :
1122 0 : return constrain_float(throttle_in, 0, 1);
1123 : }
1124 :
1125 : // helper for is_flying()
1126 462811 : bool QuadPlane::is_flying(void)
1127 : {
1128 462811 : if (!available()) {
1129 203595 : return false;
1130 : }
1131 259216 : if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
1132 879 : return true;
1133 : }
1134 258337 : if (motors->get_throttle() > 0.01f && !motors->limit.throttle_lower) {
1135 36777 : return true;
1136 : }
1137 221560 : if (tailsitter.in_vtol_transition()) {
1138 0 : return true;
1139 : }
1140 221560 : return false;
1141 : }
1142 :
1143 : // crude landing detector to prevent tipover
1144 3226556 : bool QuadPlane::should_relax(void)
1145 : {
1146 3226556 : const uint32_t tnow = millis();
1147 :
1148 3226556 : bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();
1149 3226556 : if (motors->get_throttle() < 0.01f) {
1150 477046 : motor_at_lower_limit = true;
1151 : }
1152 :
1153 3226556 : if (!motor_at_lower_limit) {
1154 2749510 : landing_detect.lower_limit_start_ms = 0;
1155 2749510 : landing_detect.land_start_ms = 0;
1156 2749510 : return false;
1157 477046 : } else if (landing_detect.lower_limit_start_ms == 0) {
1158 276 : landing_detect.lower_limit_start_ms = tnow;
1159 : }
1160 :
1161 477046 : return (tnow - landing_detect.lower_limit_start_ms) > 1000;
1162 : }
1163 :
1164 : // see if we are flying in vtol
1165 12163210 : bool QuadPlane::is_flying_vtol(void) const
1166 : {
1167 12163210 : if (!available()) {
1168 2481605 : return false;
1169 : }
1170 9681605 : if (motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) {
1171 : // assume that with no motor outputs we're not flying in VTOL mode
1172 3339058 : return false;
1173 : }
1174 6342547 : if (motors->get_throttle() > 0.01f) {
1175 : // if we are demanding more than 1% throttle then don't consider aircraft landed
1176 5628533 : return true;
1177 : }
1178 714014 : if (plane.control_mode->is_vtol_man_throttle() && air_mode_active()) {
1179 : // in manual throttle modes with airmode on, don't consider aircraft landed
1180 22514 : return true;
1181 : }
1182 691500 : if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
1183 1922 : return true;
1184 : }
1185 689578 : if (plane.control_mode->is_vtol_man_mode()) {
1186 : // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero
1187 91024 : return is_positive(get_throttle_input());
1188 : }
1189 598554 : if (in_vtol_mode() && millis() - landing_detect.lower_limit_start_ms > 5000) {
1190 : // use landing detector
1191 105093 : return true;
1192 : }
1193 493461 : return false;
1194 : }
1195 :
1196 : /*
1197 : smooth out descent rate for landing to prevent a jerk as we get to
1198 : land_final_alt_m.
1199 : */
1200 856222 : float QuadPlane::landing_descent_rate_ms(float height_above_ground_m)
1201 : {
1202 856222 : if (poscontrol.last_override_descent_ms != 0) {
1203 15795 : const uint32_t now = AP_HAL::millis();
1204 15795 : if (now - poscontrol.last_override_descent_ms < 1000) {
1205 2296 : return poscontrol.override_descent_rate_ms;
1206 : }
1207 : }
1208 :
1209 853926 : if (poscontrol.get_state() == QPOS_LAND_FINAL) {
1210 : // when in final use descent rate for final even if alt has climbed again
1211 361512 : height_above_ground_m = MIN(height_above_ground_m, land_final_alt_m);
1212 : }
1213 853926 : const float max_climb_speed_ms = wp_nav->get_default_speed_up_ms();
1214 853926 : float ret_ms = linear_interpolate(land_final_speed_ms, wp_nav->get_default_speed_down_ms(),
1215 : height_above_ground_m,
1216 853926 : land_final_alt_m, land_final_alt_m + 6);
1217 :
1218 853926 : if (option_is_set(QuadPlane::Option::THR_LANDING_CONTROL)) {
1219 : // allow throttle control for landing speed
1220 0 : const float thr_in = get_pilot_land_throttle();
1221 0 : if (thr_in > THR_CTRL_LAND_THRESH) {
1222 0 : thr_ctrl_land = true;
1223 : }
1224 0 : if (thr_ctrl_land) {
1225 0 : const float dz = 0.1;
1226 0 : const float thresh1 = 0.5 + dz;
1227 0 : const float thresh2 = 0.5 - dz;
1228 0 : const float scaling = 1.0 / (0.5 - dz);
1229 0 : if (thr_in > thresh1) {
1230 : // start climbing
1231 0 : ret_ms = -(thr_in - thresh1) * scaling * max_climb_speed_ms;
1232 0 : } else if (thr_in > thresh2) {
1233 : // hold height
1234 0 : ret_ms = 0;
1235 : } else {
1236 0 : ret_ms *= (thresh2 - thr_in) * scaling;
1237 : }
1238 : }
1239 : }
1240 :
1241 853926 : if (poscontrol.pilot_correction_active) {
1242 : // stop descent when repositioning
1243 0 : ret_ms = MIN(0, ret_ms);
1244 : }
1245 :
1246 853926 : return ret_ms;
1247 : }
1248 :
1249 : /*
1250 : get pilot input yaw rate in cd/s
1251 : */
1252 2502050 : float QuadPlane::get_pilot_input_yaw_rate_cds(void) const
1253 : {
1254 2502050 : const auto rudder_in = plane.channel_rudder->get_control_in();
1255 2502050 : bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active();
1256 7497414 : if (!manual_air_mode &&
1257 2493314 : !is_positive(get_throttle_input()) &&
1258 1624418 : (!plane.control_mode->does_auto_throttle() || motors->limit.throttle_lower) &&
1259 488518 : plane.arming.get_rudder_arming_type() == AP_Arming::RudderArming::ARMDISARM &&
1260 4995364 : rudder_in < 0 &&
1261 0 : fabsf(inertial_nav.get_velocity_z_up_cms()) < (0.5 * get_pilot_velocity_z_max_dn_m()) * 100) {
1262 : // the user may be trying to disarm, disable pilot yaw control
1263 0 : return 0;
1264 : }
1265 :
1266 2549484 : if ((plane.g.stick_mixing == StickMixing::NONE) &&
1267 78916 : (plane.control_mode == &plane.mode_qrtl ||
1268 62964 : plane.control_mode->is_guided_mode() ||
1269 31482 : in_vtol_auto())) {
1270 15952 : return 0;
1271 : }
1272 :
1273 : // add in rudder input
1274 2486098 : const float yaw_rate_max = command_model_pilot.get_rate();
1275 2486098 : float max_rate = yaw_rate_max;
1276 2486098 : if (!in_vtol_mode() && tailsitter.enabled()) {
1277 : // scale by RUDD_DT_GAIN when not in a VTOL mode for
1278 : // tailsitters. This allows for flat turns in tailsitters for
1279 : // fixed wing modes if you want them, but prevents crazy yaw
1280 : // rate demands in fixed wing based on your preferred yaw rate
1281 : // when hovering
1282 2914 : max_rate *= plane.g2.rudd_dt_gain * 0.01;
1283 : }
1284 2562136 : if (tailsitter.enabled() &&
1285 76038 : tailsitter.input_type & Tailsitter::input::TAILSITTER_INPUT_BF_ROLL) {
1286 : // must have a non-zero max yaw rate for scaling to work
1287 0 : max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max;
1288 : }
1289 2486098 : return input_expo(rudder_in * (1/4500.0), command_model_pilot.get_expo()) * max_rate * 100.0;
1290 : }
1291 :
1292 : /*
1293 : get overall desired yaw rate in cd/s
1294 : */
1295 1354271 : float QuadPlane::get_desired_yaw_rate_cds(bool should_weathervane)
1296 : {
1297 1354271 : float yaw_cds = 0;
1298 1354271 : if (assisted_flight) {
1299 : // use bank angle to get desired yaw rate
1300 420416 : yaw_cds += desired_auto_yaw_rate_cds();
1301 : }
1302 :
1303 : // add in pilot input
1304 1354271 : yaw_cds += get_pilot_input_yaw_rate_cds();
1305 :
1306 1354271 : if (should_weathervane) {
1307 : // add in weathervaning
1308 766143 : yaw_cds += get_weathervane_yaw_rate_cds();
1309 : }
1310 :
1311 1354271 : return yaw_cds;
1312 : }
1313 :
1314 : // get pilot desired climb rate in cm/s
1315 649966 : float QuadPlane::get_pilot_desired_climb_rate_cms(void) const
1316 : {
1317 649966 : if (!rc().has_valid_input()) {
1318 : // no valid input means no sensible pilot desired climb rate.
1319 : // descend at 0.5m/s for now
1320 0 : return -50;
1321 : }
1322 649966 : uint16_t dead_zone = plane.channel_throttle->get_dead_zone();
1323 649966 : uint16_t trim = (plane.channel_throttle->get_radio_max() + plane.channel_throttle->get_radio_min()) / 2;
1324 649966 : const float throttle_request = plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) * 0.01f;
1325 649966 : return throttle_request * (throttle_request > 0.0f ? pilot_speed_z_max_up_ms : get_pilot_velocity_z_max_dn_m()) * 100;
1326 : }
1327 :
1328 :
1329 : /*
1330 : initialise throttle_wait based on throttle and is_flying()
1331 : */
1332 15622 : void QuadPlane::init_throttle_wait(void)
1333 : {
1334 15944 : if (get_throttle_input() >= 10 ||
1335 322 : plane.is_flying()) {
1336 15315 : throttle_wait = false;
1337 : } else {
1338 307 : throttle_wait = true;
1339 : }
1340 15622 : }
1341 :
1342 : // set motor arming
1343 816 : void QuadPlane::set_armed(bool armed)
1344 : {
1345 816 : if (!initialised) {
1346 354 : return;
1347 : }
1348 462 : motors->armed(armed);
1349 :
1350 462 : if (plane.control_mode == &plane.mode_guided) {
1351 12 : guided_wait_takeoff = armed;
1352 : }
1353 :
1354 : // re-init throttle wait on arm and disarm, to prevent rudder
1355 : // arming on 2nd flight causing yaw
1356 462 : if (!air_mode_active()) {
1357 454 : init_throttle_wait();
1358 : }
1359 : }
1360 :
1361 :
1362 : /*
1363 : estimate desired climb rate for assistance (in cm/s)
1364 : */
1365 219920 : float QuadPlane::assist_climb_rate_cms(void) const
1366 : {
1367 : float climb_rate_cms;
1368 219920 : if (plane.control_mode->does_auto_throttle()) {
1369 : // use altitude_error_cm, spread over 10s interval
1370 183980 : climb_rate_cms = plane.calc_altitude_error_cm() * 0.1f;
1371 : } else {
1372 : // otherwise estimate from pilot input
1373 35940 : climb_rate_cms = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd / (plane.aparm.pitch_limit_max * 100));
1374 35940 : climb_rate_cms *= plane.get_throttle_input();
1375 : }
1376 219920 : climb_rate_cms = constrain_float(climb_rate_cms, -wp_nav->get_default_speed_down_ms() * 100.0, wp_nav->get_default_speed_up_ms() * 100.0);
1377 :
1378 : // bring in the demanded climb rate over 2 seconds
1379 219920 : const uint32_t ramp_up_time_ms = 2000;
1380 219920 : const uint32_t dt_since_start = last_pidz_active_ms - last_pidz_init_ms;
1381 219920 : if (dt_since_start < ramp_up_time_ms) {
1382 26777 : climb_rate_cms = linear_interpolate(0, climb_rate_cms, dt_since_start, 0, ramp_up_time_ms);
1383 : }
1384 :
1385 219920 : return climb_rate_cms;
1386 : }
1387 :
1388 : /*
1389 : calculate desired yaw rate for assistance
1390 : */
1391 945068 : float QuadPlane::desired_auto_yaw_rate_cds(bool body_frame) const
1392 : {
1393 : float aspeed;
1394 945068 : if (!ahrs.airspeed_EAS(aspeed) || aspeed < plane.aparm.airspeed_min) {
1395 332062 : aspeed = plane.aparm.airspeed_min;
1396 : }
1397 945068 : if (aspeed < 1) {
1398 0 : aspeed = 1;
1399 : }
1400 945068 : if (body_frame) {
1401 374475 : return degrees(GRAVITY_MSS * sinf(cd_to_rad(plane.nav_roll_cd))/aspeed) * 100;
1402 : }
1403 570593 : return degrees(GRAVITY_MSS * tanf(cd_to_rad(plane.nav_roll_cd))/aspeed) * 100;
1404 : }
1405 :
1406 : /*
1407 : update for transition from quadplane to fixed wing mode
1408 : */
1409 3408218 : void SLT_Transition::update()
1410 : {
1411 3408218 : const uint32_t now = millis();
1412 :
1413 3408218 : if (!plane.arming.is_armed_and_safety_off()) {
1414 : // reset the failure timer if we are disarmed
1415 814855 : transition_start_ms = now;
1416 : }
1417 :
1418 : float aspeed;
1419 3408218 : bool have_airspeed = quadplane.ahrs.airspeed_EAS(aspeed);
1420 :
1421 : /*
1422 : see if we should provide some assistance
1423 : */
1424 3408218 : if (quadplane.assist.should_assist(aspeed, have_airspeed)) {
1425 : // the quad should provide some assistance to the plane
1426 163422 : quadplane.assisted_flight = true;
1427 : // update transition state for vehicles using airspeed wait
1428 163422 : if (!in_forced_transition) {
1429 163422 : const bool show_message = transition_state != State::AIRSPEED_WAIT || transition_start_ms == 0;
1430 163422 : if (show_message) {
1431 148 : gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed);
1432 : }
1433 163422 : transition_state = State::AIRSPEED_WAIT;
1434 163422 : if (transition_start_ms == 0) {
1435 102 : transition_start_ms = now;
1436 : }
1437 : }
1438 : } else {
1439 3244796 : quadplane.assisted_flight = false;
1440 : }
1441 :
1442 :
1443 : // if rotors are fully forward then we are not transitioning,
1444 : // unless we are waiting for airspeed to increase (in which case
1445 : // the tilt will decrease rapidly)
1446 3408218 : if (quadplane.tiltrotor.fully_fwd() && transition_state != State::AIRSPEED_WAIT) {
1447 224256 : if (transition_state == State::TIMER) {
1448 : float throttle;
1449 13 : if (plane.quadplane.tiltrotor.get_forward_throttle(throttle)) {
1450 : // Reset the TECS minimum throttle to match throttle of forward thrust motors
1451 : // and set the throttle channel slew rate limiter to prevent a sudden drop in throttle
1452 7 : if (plane.control_mode->does_auto_throttle()) {
1453 6 : plane.TECS_controller.set_throttle_min(throttle, true);
1454 : }
1455 7 : SRV_Channels::set_slew_last_scaled_output(SRV_Channel::k_throttle, throttle * 100);
1456 7 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle * 100);
1457 : }
1458 13 : gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done");
1459 : }
1460 224256 : transition_state = State::DONE;
1461 224256 : transition_start_ms = 0;
1462 224256 : transition_low_airspeed_ms = 0;
1463 : }
1464 :
1465 3408218 : if (transition_state != State::DONE) {
1466 : // during transition we ask TECS to use a synthetic
1467 : // airspeed. Otherwise the pitch limits will throw off the
1468 : // throttle calculation which is driven by pitch
1469 392449 : plane.TECS_controller.use_synthetic_airspeed();
1470 : }
1471 :
1472 3408218 : switch (transition_state) {
1473 193361 : case State::AIRSPEED_WAIT: {
1474 193361 : quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1475 : // we hold in hover until the required airspeed is reached
1476 193361 : if (transition_start_ms == 0) {
1477 4 : gcs().send_text(MAV_SEVERITY_INFO, "Transition airspeed wait");
1478 4 : transition_start_ms = now;
1479 : }
1480 :
1481 : // check if we have failed to transition while in State::AIRSPEED_WAIT
1482 580083 : if (transition_start_ms != 0 &&
1483 196265 : (quadplane.transition_failure.timeout > 0) &&
1484 2904 : ((now - transition_start_ms) > ((uint32_t)quadplane.transition_failure.timeout * 1000))) {
1485 1 : if (!quadplane.transition_failure.warned) {
1486 1 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit");
1487 1 : quadplane.transition_failure.warned = true;
1488 : }
1489 : // if option is set and ground speed> 1/2 AIRSPEED_MIN for non-tiltrotors, then complete transition, otherwise QLAND.
1490 : // tiltrotors will immediately transition
1491 1 : const bool tiltrotor_with_ground_speed = quadplane.tiltrotor.enabled() && (plane.ahrs.groundspeed() > plane.aparm.airspeed_min * 0.5);
1492 1 : if (quadplane.option_is_set(QuadPlane::Option::TRANS_FAIL_TO_FW) && tiltrotor_with_ground_speed) {
1493 0 : transition_state = State::TIMER;
1494 0 : in_forced_transition = true;
1495 : } else {
1496 1 : switch (QuadPlane::TRANS_FAIL::ACTION(quadplane.transition_failure.action)) {
1497 0 : case QuadPlane::TRANS_FAIL::ACTION::QLAND:
1498 0 : plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TRANSITION);
1499 0 : break;
1500 :
1501 1 : case QuadPlane::TRANS_FAIL::ACTION::QRTL:
1502 1 : plane.set_mode(plane.mode_qrtl, ModeReason::VTOL_FAILED_TRANSITION);
1503 1 : quadplane.poscontrol.set_state(QuadPlane::QPOS_POSITION1);
1504 1 : break;
1505 :
1506 0 : default:
1507 0 : break;
1508 : }
1509 : }
1510 : } else {
1511 193360 : quadplane.transition_failure.warned = false;
1512 : }
1513 :
1514 193361 : transition_low_airspeed_ms = now;
1515 193361 : if (have_airspeed && aspeed > plane.aparm.airspeed_min && !quadplane.assisted_flight) {
1516 172 : transition_state = State::TIMER;
1517 172 : airspeed_reached_tilt = quadplane.tiltrotor.current_tilt;
1518 172 : gcs().send_text(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", (double)aspeed);
1519 : }
1520 193361 : quadplane.assisted_flight = true;
1521 :
1522 : // do not allow a climb on the quad motors during transition a
1523 : // climb would add load to the airframe, and prolongs the
1524 : // transition. We don't limit the climb rate on tilt rotors as
1525 : // otherwise the plane can end up in high-alpha flight with
1526 : // low VTOL thrust and may not complete a transition
1527 193361 : float climb_rate_cms = quadplane.assist_climb_rate_cms();
1528 193361 : if (quadplane.option_is_set(QuadPlane::Option::LEVEL_TRANSITION) && !quadplane.tiltrotor.enabled()) {
1529 0 : climb_rate_cms = MIN(climb_rate_cms, 0.0f);
1530 : }
1531 193361 : quadplane.hold_hover(climb_rate_cms);
1532 :
1533 193361 : if (!quadplane.tiltrotor.is_vectored()) {
1534 : // set desired yaw rate to a coordinated turn
1535 177874 : quadplane.attitude_control->reset_yaw_target_and_rate();
1536 177874 : quadplane.attitude_control->rate_bf_yaw_target(quadplane.desired_auto_yaw_rate_cds(true));
1537 : }
1538 193361 : if (quadplane.tiltrotor.enabled() && !quadplane.tiltrotor.has_fw_motor()) {
1539 : // tilt rotors without dedicated fw motors do not have forward throttle output in this stage
1540 : // prevent throttle I wind up
1541 21278 : plane.TECS_controller.reset_throttle_I();
1542 : }
1543 :
1544 193361 : last_throttle = motors->get_throttle();
1545 :
1546 : // reset integrators while we are below target airspeed as we
1547 : // may build up too much while still primarily under
1548 : // multicopter control
1549 193361 : plane.pitchController.reset_I();
1550 193361 : plane.rollController.reset_I();
1551 :
1552 : // give full authority to attitude control
1553 193361 : quadplane.attitude_control->set_throttle_mix_max(1.0f);
1554 193361 : break;
1555 : }
1556 :
1557 199088 : case State::TIMER: {
1558 199088 : quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1559 : // after airspeed is reached we degrade throttle over the transition time, but continue
1560 : // to stabilize and wait for any required forward tilt to complete and the timer to expire
1561 199088 : const uint32_t transition_timer_ms = now - transition_low_airspeed_ms;
1562 199088 : const float trans_time_ms = constrain_float(quadplane.transition_time_ms, 500, 30000);
1563 199088 : const bool tilt_fwd_complete = !quadplane.tiltrotor.enabled() || quadplane.tiltrotor.tilt_angle_achieved();
1564 199088 : if (transition_timer_ms > unsigned(trans_time_ms) && tilt_fwd_complete) {
1565 112 : transition_state = State::DONE;
1566 112 : in_forced_transition = false;
1567 112 : transition_start_ms = 0;
1568 112 : transition_low_airspeed_ms = 0;
1569 : float throttle;
1570 112 : if (plane.quadplane.tiltrotor.get_forward_throttle(throttle)) {
1571 : // Reset the TECS minimum throttle to match throttle of forward thrust motors
1572 : // and set the throttle channel slew rate limiter to prevent a sudden drop in throttle
1573 0 : if (plane.control_mode->does_auto_throttle()) {
1574 0 : plane.TECS_controller.set_throttle_min(throttle, true);
1575 : }
1576 0 : SRV_Channels::set_slew_last_scaled_output(SRV_Channel::k_throttle, throttle * 100);
1577 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle * 100);
1578 : }
1579 112 : gcs().send_text(MAV_SEVERITY_INFO, "Transition done");
1580 : }
1581 :
1582 199088 : float transition_scale = (trans_time_ms - transition_timer_ms) / trans_time_ms;
1583 199088 : float throttle_scaled = last_throttle * transition_scale;
1584 :
1585 : // set zero throttle mix, to give full authority to
1586 : // throttle. This ensures that the fixed wing controllers get
1587 : // a chance to learn the right integrators during the transition
1588 199088 : quadplane.attitude_control->set_throttle_mix_value(0.5 * transition_scale);
1589 :
1590 199088 : if (throttle_scaled < 0.01) {
1591 : // ensure we don't drop all the way to zero or the motors
1592 : // will stop stabilizing
1593 150137 : throttle_scaled = 0.01;
1594 : }
1595 199088 : if (quadplane.tiltrotor.enabled() && !quadplane.tiltrotor.has_vtol_motor() && !quadplane.tiltrotor.has_fw_motor()) {
1596 : // All motors tilting, Use a combination of vertical and forward throttle based on current tilt angle
1597 : // scale from all VTOL throttle at airspeed_reached_tilt to all forward throttle at fully forward tilt
1598 : // this removes a step change in throttle once assistance is stopped
1599 0 : const float ratio = (constrain_float(quadplane.tiltrotor.current_tilt, airspeed_reached_tilt, quadplane.tiltrotor.get_fully_forward_tilt()) - airspeed_reached_tilt) / (quadplane.tiltrotor.get_fully_forward_tilt() - airspeed_reached_tilt);
1600 0 : const float fw_throttle = MAX(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),0) * 0.01;
1601 0 : throttle_scaled = constrain_float(throttle_scaled * (1.0-ratio) + fw_throttle * ratio, 0.0, 1.0);
1602 : }
1603 199088 : quadplane.assisted_flight = true;
1604 199088 : quadplane.hold_stabilize(throttle_scaled);
1605 :
1606 199088 : if (!quadplane.tiltrotor.is_vectored()) {
1607 : // set desired yaw rate to a coordinated turn
1608 196601 : quadplane.attitude_control->reset_yaw_target_and_rate();
1609 196601 : quadplane.attitude_control->rate_bf_yaw_target(quadplane.desired_auto_yaw_rate_cds(true));
1610 : }
1611 199088 : break;
1612 : }
1613 :
1614 3015769 : case State::DONE:
1615 3015769 : quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
1616 3015769 : motors->output();
1617 3015769 : set_last_fw_pitch();
1618 3015769 : in_forced_transition = false;
1619 3015769 : return;
1620 : }
1621 :
1622 392449 : quadplane.motors_output();
1623 :
1624 392449 : set_last_fw_pitch();
1625 : }
1626 :
1627 2671882 : void SLT_Transition::VTOL_update()
1628 : {
1629 : /*
1630 : setup the transition state appropriately for next time we go into a non-VTOL mode
1631 : */
1632 2671882 : transition_start_ms = 0;
1633 2671882 : transition_low_airspeed_ms = 0;
1634 2671882 : if (quadplane.throttle_wait && !plane.is_flying()) {
1635 316080 : in_forced_transition = false;
1636 316080 : transition_state = State::DONE;
1637 : } else {
1638 : /*
1639 : setup for airspeed wait for later
1640 : */
1641 2355802 : transition_state = State::AIRSPEED_WAIT;
1642 : }
1643 2671882 : last_throttle = motors->get_throttle();
1644 :
1645 : // Keep assistance reset while not checking
1646 2671882 : quadplane.assist.reset();
1647 2671882 : }
1648 :
1649 : /*
1650 : update motor output for quadplane
1651 : */
1652 8377244 : void QuadPlane::update(void)
1653 : {
1654 8377244 : if (!setup()) {
1655 1873190 : return;
1656 : }
1657 :
1658 : // keep motors interlock state upto date with E-stop
1659 6504054 : motors->set_interlock(!SRV_Channels::get_emergency_stop());
1660 :
1661 6504054 : if ((ahrs_view != NULL) && !is_equal(_last_ahrs_trim_pitch, ahrs_trim_pitch.get())) {
1662 0 : _last_ahrs_trim_pitch = ahrs_trim_pitch.get();
1663 0 : ahrs_view->set_pitch_trim(_last_ahrs_trim_pitch);
1664 : }
1665 :
1666 : #if AP_ADVANCEDFAILSAFE_ENABLED
1667 6504054 : if (plane.afs.should_crash_vehicle() && !plane.afs.terminating_vehicle_via_landing()) {
1668 0 : set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
1669 0 : motors->output();
1670 0 : return;
1671 : }
1672 : #endif
1673 :
1674 6504054 : if (motor_test.running) {
1675 6000 : motor_test_output();
1676 6000 : return;
1677 : }
1678 :
1679 6498054 : if (SRV_Channels::get_emergency_stop()) {
1680 0 : attitude_control->reset_rate_controller_I_terms();
1681 : }
1682 :
1683 6498054 : if (!plane.arming.is_armed_and_safety_off()) {
1684 : /*
1685 : make sure we don't have any residual control from previous flight stages
1686 : */
1687 1487549 : if (tailsitter.enabled()) {
1688 : // tailsitters only relax I terms, to make ground testing easier
1689 39313 : attitude_control->reset_rate_controller_I_terms();
1690 : } else {
1691 : // otherwise full relax
1692 1448236 : attitude_control->relax_attitude_controllers();
1693 : }
1694 : // todo: do you want to set the throttle at this point?
1695 1487549 : pos_control->D_relax_controller(0);
1696 : }
1697 :
1698 6498054 : const uint32_t now = AP_HAL::millis();
1699 6498054 : if (!in_vtol_mode() && !in_vtol_airbrake()) {
1700 : // we're in a fixed wing mode, cope with transitions and check
1701 : // for assistance needed
1702 3731271 : if (plane.control_mode == &plane.mode_manual ||
1703 3473152 : plane.control_mode == &plane.mode_acro ||
1704 3471953 : plane.control_mode == &plane.mode_training) {
1705 : // in manual modes quad motors are always off
1706 260698 : if (!tailsitter.enabled()) {
1707 260194 : set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
1708 260194 : motors->output();
1709 : }
1710 260698 : transition->force_transition_complete();
1711 260698 : assisted_flight = false;
1712 : } else {
1713 3470573 : transition->update();
1714 : }
1715 :
1716 : } else {
1717 :
1718 2766783 : assisted_flight = in_vtol_airbrake();
1719 :
1720 : // output to motors
1721 2766783 : motors_output();
1722 :
1723 2766783 : transition->VTOL_update();
1724 :
1725 : }
1726 :
1727 : // disable throttle_wait when throttle rises above 10%
1728 9720073 : if (throttle_wait &&
1729 1611035 : (plane.get_throttle_input() > 10 ||
1730 1610984 : !rc().has_valid_input())) {
1731 51 : throttle_wait = false;
1732 : }
1733 :
1734 6498054 : tiltrotor.update();
1735 :
1736 6498054 : if (in_vtol_mode()) {
1737 : // if enabled output forward throttle else 0
1738 2741745 : float fwd_thr = 0;
1739 2741745 : if (allow_forward_throttle_in_vtol_mode()) {
1740 2292091 : fwd_thr = forward_throttle_pct();
1741 : }
1742 2741745 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
1743 : }
1744 :
1745 : #if HAL_LOGGING_ENABLED
1746 : // motors logging
1747 6498054 : if (motors->armed()) {
1748 5010505 : const bool motors_active = in_vtol_mode() || assisted_flight;
1749 5010505 : if (motors_active && (motors->get_spool_state() != AP_Motors::SpoolState::SHUT_DOWN)) {
1750 : // log ANG at main loop rate
1751 2663143 : bool sysid_running = false;
1752 : #if AP_PLANE_SYSTEMID_ENABLED
1753 2663143 : sysid_running = plane.g2.systemid.is_running();
1754 : #endif
1755 2663143 : if (!sysid_running) {
1756 2663143 : if (show_vtol_view()) {
1757 2301001 : attitude_control->Write_ANG();
1758 : }
1759 : // log RATE at main loop rate
1760 2663143 : attitude_control->Write_Rate(*pos_control);
1761 : }
1762 :
1763 : // log MOTB at 10 Hz
1764 2663143 : if (now - last_motb_log_ms > 100) {
1765 86115 : last_motb_log_ms = now;
1766 86115 : motors->Log_Write();
1767 : }
1768 : }
1769 : // log QTUN at 25 Hz if motors are active, or have been active in the last quarter second
1770 5010505 : if ((motors_active || (now - last_motors_active_ms < 250)) && (now - last_qtun_log_ms > 40)) {
1771 205625 : last_qtun_log_ms = now;
1772 205625 : Log_Write_QControl_Tuning();
1773 : }
1774 : }
1775 : #else
1776 : (void)now;
1777 : #endif // HAL_LOGGING_ENABLED
1778 : }
1779 :
1780 : /*
1781 : see if motors should be shutdown. If they should be then change AP_Motors state to
1782 : AP_Motors::DesiredSpoolState::SHUT_DOWN
1783 :
1784 : This is a safety check to prevent accidental motor runs on the
1785 : ground, such as if RC fails and QRTL is started
1786 : */
1787 2737526 : void QuadPlane::update_throttle_suppression(void)
1788 : {
1789 : // if the motors have been running in the last 2 seconds then
1790 : // allow them to run now
1791 2737526 : if (AP_HAL::millis() - last_motors_active_ms < 2000) {
1792 2401343 : return;
1793 : }
1794 :
1795 : // see if motors are already disabled
1796 336183 : if (motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
1797 18235 : return;
1798 : }
1799 :
1800 317948 : if (guided_wait_takeoff) {
1801 412 : goto idle_state;
1802 : }
1803 :
1804 : /* if the users throttle is above zero then allow motors to run
1805 :
1806 : if the user has unset the "check throttle zero when arming"
1807 : then the RC controller has a sprung throttle and we should not
1808 : consider non-zero throttle to mean that pilot is commanding
1809 : takeoff unless in a manual throttle mode
1810 : */
1811 458219 : if (!is_zero(get_throttle_input()) &&
1812 140683 : (rc().arming_check_throttle() ||
1813 0 : plane.control_mode->is_vtol_man_throttle() ||
1814 0 : plane.channel_throttle->norm_input_dz() > 0)) {
1815 140683 : return;
1816 : }
1817 :
1818 : // if in a VTOL manual throttle mode and air_mode is on then allow motors to run
1819 176853 : if (plane.control_mode->is_vtol_man_throttle() && air_mode_active()) {
1820 15847 : return;
1821 : }
1822 :
1823 : // if we are in a fixed wing auto throttle mode and we have
1824 : // unsuppressed the throttle then allow motors to run
1825 161006 : if (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed) {
1826 142845 : return;
1827 : }
1828 :
1829 : // if our vertical velocity is greater than 1m/s then allow motors to run
1830 18161 : if (fabsf(inertial_nav.get_velocity_z_up_cms()) > 100) {
1831 2299 : return;
1832 : }
1833 :
1834 : // if we are more than 5m from home altitude then allow motors to run
1835 15862 : if (plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING) > 5) {
1836 202 : return;
1837 : }
1838 :
1839 : // allow for takeoff
1840 15660 : if (plane.control_mode == &plane.mode_auto && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
1841 4207 : return;
1842 : }
1843 :
1844 11453 : idle_state:
1845 : // motors should be in the spin when armed state to warn user they could become active
1846 11865 : set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
1847 11865 : motors->set_throttle(0);
1848 11865 : last_motors_active_ms = 0;
1849 : }
1850 :
1851 : // update estimated throttle required to hover (if necessary)
1852 : // called at 100hz
1853 3894351 : void QuadPlane::update_throttle_hover()
1854 : {
1855 3894351 : if (!available()) {
1856 3810735 : return;
1857 : }
1858 :
1859 : // if not armed or landed exit
1860 2167938 : if (!motors->armed() || !is_flying_vtol()) {
1861 1376366 : return;
1862 : }
1863 :
1864 : // do not update while climbing or descending
1865 791572 : if (!is_zero(pos_control->get_vel_desired_U_ms())) {
1866 656406 : return;
1867 : }
1868 :
1869 : // do not update if quadplane forward motor is running (wing may be generating lift)
1870 : // we use the THR_MIN value to account for petrol motors idling at THR_MIN
1871 135166 : if (!tailsitter.enabled() && (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > MAX(0, plane.aparm.throttle_min + 10))) {
1872 5400 : return;
1873 : }
1874 :
1875 : // don't update if Z controller not running
1876 129766 : const uint32_t now = AP_HAL::millis();
1877 129766 : if (now - last_pidz_active_ms > 20) {
1878 46150 : return;
1879 : }
1880 :
1881 : // get throttle output
1882 83616 : float throttle = motors->get_throttle();
1883 :
1884 : float aspeed;
1885 : // calc average throttle if we are in a level hover and low airspeed
1886 78911 : if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z_up_cms()) < 60 &&
1887 141351 : labs(ahrs_view->roll_sensor) < 500 && labs(ahrs_view->pitch_sensor) < 500 &&
1888 229457 : ahrs.airspeed_EAS(aspeed) && aspeed < plane.aparm.airspeed_min * 0.3) {
1889 : // Can we set the time constant automatically
1890 55011 : motors->update_throttle_hover(0.01f);
1891 : #if HAL_GYROFFT_ENABLED
1892 55011 : plane.gyro_fft.update_freq_hover(0.01f, motors->get_throttle_out());
1893 : #endif
1894 : }
1895 : }
1896 : /*
1897 : output motors and do any copter needed
1898 : */
1899 3259693 : void QuadPlane::motors_output(bool run_rate_controller)
1900 : {
1901 : // QuadPlane has no pre-takeoff RPM gate that asserts the spool-up block,
1902 : // so clear it every cycle before motors->output() runs output_logic().
1903 : // Without this the block set during ground-idle ramp would never clear
1904 : // and the spool would stay in GROUND_IDLE.
1905 3259693 : motors->set_spoolup_block(false);
1906 :
1907 : /* Delay for ARMING_DELAY_MS after arming before allowing props to spin:
1908 : 1) for safety (OPTION_DELAY_ARMING)
1909 : 2) to allow motors to return to vertical (OPTION_DISARMED_TILT)
1910 : */
1911 3259693 : if (option_is_set(QuadPlane::Option::DISARMED_TILT) || option_is_set(QuadPlane::Option::DELAY_ARMING)) {
1912 0 : if (plane.arming.get_delay_arming()) {
1913 : // delay motor start after arming
1914 0 : set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
1915 0 : motors->output();
1916 0 : return;
1917 : }
1918 : }
1919 :
1920 : #if AP_ADVANCEDFAILSAFE_ENABLED
1921 5998438 : if (!plane.arming.is_armed_and_safety_off() ||
1922 5998438 : (plane.afs.should_crash_vehicle() && !plane.afs.terminating_vehicle_via_landing()) ||
1923 2738745 : SRV_Channels::get_emergency_stop()) {
1924 : #else
1925 : if (!plane.arming.is_armed_and_safety_off() || SRV_Channels::get_emergency_stop()) {
1926 : #endif
1927 520948 : set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
1928 520948 : motors->output();
1929 520948 : return;
1930 : }
1931 2738745 : if (esc_calibration && AP_Notify::flags.esc_calibration && plane.control_mode == &plane.mode_qstabilize) {
1932 : // output is direct from run_esc_calibration()
1933 0 : return;
1934 : }
1935 :
1936 2738745 : const uint32_t now = AP_HAL::millis();
1937 2738745 : if (tailsitter.in_vtol_transition(now) && !assisted_flight) {
1938 : /*
1939 : don't run the motor outputs while in tailsitter->vtol
1940 : transition. That is taken care of by the fixed wing
1941 : stabilisation code
1942 : */
1943 1219 : return;
1944 : }
1945 :
1946 2737526 : if (run_rate_controller) {
1947 2661945 : if (now - last_att_control_ms > 100) {
1948 : // relax if have been inactive
1949 281 : relax_attitude_control();
1950 : }
1951 :
1952 : // see if we need to be in VTOL recovery
1953 2661945 : assist.check_VTOL_recovery();
1954 :
1955 : // run low level rate controllers that only require IMU data and set loop time
1956 2661945 : const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
1957 2661945 : motors->set_dt_s(last_loop_time_s);
1958 2661945 : attitude_control->set_dt_s(last_loop_time_s);
1959 2661945 : pos_control->set_dt_s(last_loop_time_s);
1960 2661945 : attitude_control->rate_controller_run();
1961 : // reset sysid and other temporary inputs
1962 2661945 : attitude_control->rate_controller_target_reset();
1963 2661945 : last_att_control_ms = now;
1964 : }
1965 :
1966 : // see if motors should be shut down
1967 2737526 : update_throttle_suppression();
1968 :
1969 2737526 : motors->output();
1970 :
1971 : // Run thrust loss check, reset if motors have not been active
1972 2737526 : const bool motors_inactive = (now - last_motors_active_ms) > 100;
1973 2737526 : thrust_loss_check(motors_inactive);
1974 :
1975 : // remember when motors were last active for throttle suppression
1976 2737526 : if (motors->get_throttle() > 0.01f || tiltrotor.motors_active()) {
1977 2290934 : last_motors_active_ms = now;
1978 : }
1979 :
1980 : }
1981 :
1982 : /*
1983 : handle a MAVLink DO_VTOL_TRANSITION
1984 : */
1985 6 : bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) const
1986 : {
1987 6 : if (!available()) {
1988 0 : gcs().send_text(MAV_SEVERITY_NOTICE, "VTOL not available");
1989 0 : return false;
1990 : }
1991 6 : if (plane.control_mode != &plane.mode_auto) {
1992 0 : gcs().send_text(MAV_SEVERITY_NOTICE, "VTOL transition only in AUTO");
1993 0 : return false;
1994 : }
1995 6 : switch (state) {
1996 3 : case MAV_VTOL_STATE_MC:
1997 3 : if (!plane.auto_state.vtol_mode) {
1998 3 : gcs().send_text(MAV_SEVERITY_NOTICE, "Entered VTOL mode");
1999 : }
2000 3 : plane.auto_state.vtol_mode = true;
2001 : // This is a precaution. It should be looked after by the call to QuadPlane::mode_enter(void) on mode entry.
2002 3 : plane.quadplane.q_fwd_throttle = 0.0f;
2003 3 : plane.quadplane.q_fwd_pitch_lim_cd = 100.0f * plane.quadplane.q_fwd_pitch_lim;
2004 3 : return true;
2005 :
2006 3 : case MAV_VTOL_STATE_FW:
2007 3 : if (plane.auto_state.vtol_mode) {
2008 3 : gcs().send_text(MAV_SEVERITY_NOTICE, "Exited VTOL mode");
2009 : }
2010 3 : plane.auto_state.vtol_mode = false;
2011 :
2012 3 : return true;
2013 :
2014 0 : default:
2015 0 : break;
2016 : }
2017 :
2018 0 : gcs().send_text(MAV_SEVERITY_NOTICE, "Invalid VTOL mode");
2019 0 : return false;
2020 : }
2021 :
2022 : /*
2023 : are we in a VTOL auto state?
2024 : */
2025 247595824 : bool QuadPlane::in_vtol_auto(void) const
2026 : {
2027 247595824 : if (!available()) {
2028 6993797 : return false;
2029 : }
2030 240602030 : if (plane.control_mode != &plane.mode_auto) {
2031 138330909 : return false;
2032 : }
2033 102271121 : if (plane.auto_state.vtol_mode) {
2034 1773827 : return true;
2035 : }
2036 100497294 : uint16_t id = plane.mission.get_current_nav_cmd().id;
2037 100497282 : switch (id) {
2038 11783724 : case MAV_CMD_NAV_VTOL_TAKEOFF:
2039 11783724 : return true;
2040 2233760 : case MAV_CMD_NAV_LOITER_UNLIM:
2041 : case MAV_CMD_NAV_LOITER_TIME:
2042 : case MAV_CMD_NAV_LOITER_TURNS:
2043 : case MAV_CMD_NAV_LOITER_TO_ALT:
2044 2233760 : return plane.auto_state.vtol_loiter;
2045 6232762 : case MAV_CMD_NAV_TAKEOFF:
2046 6232762 : return is_vtol_takeoff(id);
2047 22744941 : case MAV_CMD_NAV_VTOL_LAND:
2048 : case MAV_CMD_NAV_LAND:
2049 : case MAV_CMD_NAV_PAYLOAD_PLACE:
2050 22744941 : return is_vtol_land(id);
2051 57502095 : default:
2052 57502095 : return false;
2053 : }
2054 : }
2055 :
2056 : /*
2057 : are we in a VTOL mode? This is used to decide if we run the
2058 : transition handling code or not
2059 :
2060 : note that AIRBRAKE is not considered in_vtol_mode even though the
2061 : VTOL motors are running
2062 : */
2063 76641966 : bool QuadPlane::in_vtol_mode(void) const
2064 : {
2065 76641966 : if (!available()) {
2066 6786345 : return false;
2067 : }
2068 69855621 : if (in_vtol_land_sequence()) {
2069 14161694 : return poscontrol.get_state() != QPOS_APPROACH && poscontrol.get_state() != QPOS_AIRBRAKE;
2070 : }
2071 55693927 : if (plane.control_mode->is_vtol_mode()) {
2072 18077066 : return true;
2073 : }
2074 37616861 : if (plane.control_mode->is_guided_mode()
2075 42138960 : && plane.auto_state.vtol_loiter &&
2076 4522099 : poscontrol.get_state() > QPOS_APPROACH) {
2077 4522099 : return true;
2078 : }
2079 33094762 : if (plane.control_mode == &plane.mode_guided &&
2080 2784106 : guided_takeoff) {
2081 0 : return true;
2082 : }
2083 33094762 : if (in_vtol_auto()) {
2084 4427492 : if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_AIRBRAKE) {
2085 4427492 : return true;
2086 : }
2087 : }
2088 28667270 : return false;
2089 : }
2090 :
2091 : /*
2092 : are we in a VTOL mode that needs position and velocity estimates?
2093 : */
2094 488395 : bool QuadPlane::in_vtol_posvel_mode(void) const
2095 : {
2096 488395 : if (!available()) {
2097 290897 : return false;
2098 : }
2099 379903 : return (plane.control_mode == &plane.mode_qloiter ||
2100 182405 : plane.control_mode == &plane.mode_qland ||
2101 173290 : plane.control_mode == &plane.mode_qrtl ||
2102 : #if QAUTOTUNE_ENABLED
2103 317364 : plane.control_mode == &plane.mode_qautotune ||
2104 : #endif
2105 156513 : (plane.control_mode->is_guided_mode() &&
2106 21013 : plane.auto_state.vtol_loiter &&
2107 379903 : poscontrol.get_state() > QPOS_APPROACH) ||
2108 343743 : in_vtol_auto());
2109 : }
2110 :
2111 : /*
2112 : update landing positioning offset
2113 : */
2114 862928 : void QuadPlane::update_land_positioning(void)
2115 : {
2116 862928 : if (!option_is_set(QuadPlane::Option::REPOSITION_LANDING)) {
2117 : // not enabled
2118 862928 : poscontrol.pilot_correction_active = false;
2119 862928 : poscontrol.target_vel_ms.zero();
2120 862928 : return;
2121 : }
2122 0 : const float scale = 1.0 / 4500;
2123 0 : float roll_in = plane.channel_roll->get_control_in() * scale;
2124 0 : float pitch_in = plane.channel_pitch->get_control_in() * scale;
2125 :
2126 : // limit correction speed to accel with stopping time constant of 0.5s
2127 0 : const float speed_max_ms = wp_nav->get_wp_acceleration_mss() * 0.5;
2128 0 : const float dt = plane.scheduler.get_loop_period_s();
2129 :
2130 0 : poscontrol.target_vel_ms = Vector3f(-pitch_in, roll_in, 0) * speed_max_ms;
2131 0 : poscontrol.target_vel_ms.rotate_xy(ahrs_view->yaw);
2132 :
2133 : // integrate our corrected position
2134 0 : poscontrol.correction_ne_m += poscontrol.target_vel_ms.xy() * dt;
2135 :
2136 0 : poscontrol.pilot_correction_active = (!is_zero(roll_in) || !is_zero(pitch_in));
2137 0 : if (poscontrol.pilot_correction_active) {
2138 0 : poscontrol.pilot_correction_done = true;
2139 : }
2140 : }
2141 :
2142 : /*
2143 : run (and possibly init) xy controller
2144 : */
2145 1341938 : void QuadPlane::run_xy_controller(float accel_limit_mss)
2146 : {
2147 1341938 : float accel_mss = wp_nav->get_wp_acceleration_mss();
2148 1341938 : if (is_positive(accel_limit_mss)) {
2149 : // allow for accel limit override
2150 802669 : accel_mss = MAX(accel_mss, accel_limit_mss);
2151 : }
2152 1341938 : const float speed_ms = wp_nav->get_default_speed_NE_ms();
2153 1341938 : pos_control->NE_set_max_speed_accel_m(speed_ms, accel_mss);
2154 1341938 : pos_control->NE_set_correction_speed_accel_m(speed_ms, accel_mss);
2155 1341938 : if (!pos_control->NE_is_active()) {
2156 110 : pos_control->NE_init_controller();
2157 : }
2158 1341938 : pos_control->set_lean_angle_max_cd(MIN(4500, MAX(accel_mss_to_angle_deg(accel_limit_mss) * 100, attitude_control->lean_angle_max_cd())));
2159 1341938 : if (q_fwd_throttle > 0.95f) {
2160 : // prevent wind up of the velocity controller I term due to a saturated forward throttle
2161 0 : pos_control->NE_set_externally_limited();
2162 : }
2163 1341938 : pos_control->NE_update_controller();
2164 1341938 : }
2165 :
2166 : /*
2167 : initialise QPOS_APPROACH
2168 : */
2169 131240 : void QuadPlane::poscontrol_init_approach(void)
2170 : {
2171 131240 : const float dist = plane.current_loc.get_distance(plane.next_WP_loc);
2172 262480 : if (option_is_set(QuadPlane::Option::DISABLE_APPROACH) ||
2173 131240 : (is_positive(approach_distance_m) && dist < approach_distance_m)) {
2174 : // go straight to QPOS_POSITION1
2175 0 : poscontrol.set_state(QPOS_POSITION1);
2176 0 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist);
2177 131240 : } else if (poscontrol.get_state() != QPOS_APPROACH) {
2178 : // check if we are close to the destination. We don't want to
2179 : // do a full approach when very close
2180 146 : if (dist < transition_threshold_m()) {
2181 126 : if (tailsitter.enabled() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
2182 99 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist);
2183 99 : poscontrol.set_state(QPOS_POSITION1);
2184 99 : transition->set_last_fw_pitch();
2185 : } else {
2186 54 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL airbrake v=%.1f d=%.0f sd=%.0f h=%.1f",
2187 27 : plane.ahrs.groundspeed(),
2188 : dist,
2189 27 : stopping_distance_m(),
2190 27 : plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING));
2191 27 : poscontrol.set_state(QPOS_AIRBRAKE);
2192 : }
2193 : } else {
2194 20 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL approach d=%.1f", dist);
2195 20 : poscontrol.set_state(QPOS_APPROACH);
2196 : }
2197 146 : poscontrol.thrust_loss_start_ms = 0;
2198 : }
2199 131240 : poscontrol.pilot_correction_done = false;
2200 131240 : poscontrol.correction_ne_m.zero();
2201 131240 : poscontrol.slow_descent = false;
2202 131240 : }
2203 :
2204 : #if HAL_LOGGING_ENABLED
2205 : /*
2206 : log the QPOS message
2207 : */
2208 106130 : void QuadPlane::log_QPOS(void)
2209 : {
2210 : // @LoggerMessage: QPOS
2211 : // @Description: Quadplane position data
2212 : // @Field: TimeUS: Time since system startup
2213 : // @Field: State: Position control state
2214 : // @FieldValueEnum: State: QuadPlane::position_control_state
2215 : // @Field: Dist: Distance to next waypoint
2216 : // @Field: TSpd: Target speed
2217 : // @Field: TAcc: Target acceleration
2218 : // @Field: OShoot: True if landing point is overshot or heading off by more than 60 degrees
2219 :
2220 212260 : AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist,TSpd,TAcc,OShoot", "QBfffB",
2221 : AP_HAL::micros64(),
2222 106130 : poscontrol.get_state(),
2223 106130 : plane.auto_state.wp_distance,
2224 106130 : poscontrol.target_speed_ms,
2225 106130 : poscontrol.target_accel_mss,
2226 106130 : poscontrol.overshoot);
2227 106130 : }
2228 : #endif
2229 :
2230 : /*
2231 : change position control state
2232 : */
2233 2430 : void QuadPlane::PosControlState::set_state(enum position_control_state s)
2234 : {
2235 2430 : const uint32_t now = AP_HAL::millis();
2236 2430 : if (state != s) {
2237 472 : auto &qp = plane.quadplane;
2238 472 : pilot_correction_done = false;
2239 : // handle resets needed for when the state changes
2240 472 : if (s == QPOS_POSITION1) {
2241 87 : reached_wp_speed = false;
2242 : // never do a rate reset, if attitude control is not active it will be automatically reset before running, see: last_att_control_ms
2243 : // if it is active then the rate control should not be reset at all
2244 87 : qp.attitude_control->reset_yaw_target_and_rate(false);
2245 87 : pos1_speed_limit_ms = plane.ahrs.groundspeed_vector().length();
2246 87 : done_accel_init = false;
2247 385 : } else if (s == QPOS_AIRBRAKE) {
2248 : // start with zero integrator on vertical throttle
2249 40 : qp.pos_control->D_get_accel_pid().set_integrator(0);
2250 345 : } else if (s == QPOS_LAND_DESCEND) {
2251 : // reset throttle descent control
2252 67 : qp.thr_ctrl_land = false;
2253 67 : qp.land_descend_start_alt_m = plane.current_loc.alt*0.01;
2254 67 : last_override_descent_ms = 0;
2255 278 : } else if (s == QPOS_LAND_ABORT) {
2256 : // reset throttle descent control
2257 0 : qp.thr_ctrl_land = false;
2258 278 : } else if (s == QPOS_LAND_FINAL) {
2259 : // remember last pos reset to handle GPS glitch in LAND_FINAL
2260 67 : ahrs_position_NE_reset_count = plane.ahrs.get_position_NE_reset_count();
2261 67 : qp.landing_detect.land_start_ms = 0;
2262 67 : qp.landing_detect.lower_limit_start_ms = 0;
2263 : }
2264 : // double log to capture the state change
2265 : #if HAL_LOGGING_ENABLED
2266 472 : qp.log_QPOS();
2267 : #endif
2268 472 : state = s;
2269 : #if HAL_LOGGING_ENABLED
2270 472 : qp.log_QPOS();
2271 : #endif
2272 472 : last_log_ms = now;
2273 472 : overshoot = false;
2274 : }
2275 2430 : last_state_change_ms = now;
2276 :
2277 : // we consider setting the state to be equivalent to running to
2278 : // prevent code from overriding the state as stale
2279 2430 : last_run_ms = now;
2280 2430 : }
2281 :
2282 : /*
2283 : main landing controller. Used for landing and RTL.
2284 : */
2285 1267474 : void QuadPlane::vtol_position_controller(void)
2286 : {
2287 1267474 : if (!setup()) {
2288 0 : return;
2289 : }
2290 :
2291 1267474 : const Location &loc = plane.next_WP_loc;
2292 1267474 : uint32_t now_ms = AP_HAL::millis();
2293 :
2294 : // distance that we switch to QPOS_POSITION2
2295 1267474 : const float position2_dist_threshold_m = 10.0;
2296 :
2297 : // target speed when we reach position2 threshold
2298 1267474 : const float position2_target_speed_ms = 3.0;
2299 :
2300 1267474 : if (plane.arming.is_armed_and_safety_off()) {
2301 1225361 : poscontrol.last_run_ms = now_ms;
2302 : }
2303 :
2304 : // avoid running the z controller in approach and airbrake if we're not already running it
2305 : // and tilt is more than tilt max
2306 1267474 : bool suppress_z_controller = false;
2307 :
2308 1267474 : Vector2f landing_velocity_ne_ms;
2309 1267474 : if (now_ms - poscontrol.last_velocity_match_ms < 1000) {
2310 13946 : landing_velocity_ne_ms = poscontrol.velocity_match_ms;
2311 : }
2312 :
2313 : // horizontal position control
2314 1267474 : switch (poscontrol.get_state()) {
2315 :
2316 0 : case QPOS_NONE:
2317 0 : poscontrol.set_state(QPOS_POSITION1);
2318 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2319 0 : break;
2320 :
2321 131114 : case QPOS_APPROACH:
2322 131114 : if (in_vtol_mode()) {
2323 : // this means we're not running transition update code and
2324 : // thus not doing qassist checking, force POSITION1 mode
2325 : // now. We don't expect this to trigger, it is a failsafe
2326 : // for a logic error
2327 0 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 nvtol");
2328 0 : poscontrol.set_state(QPOS_POSITION1);
2329 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2330 : }
2331 : FALLTHROUGH;
2332 :
2333 : case QPOS_AIRBRAKE: {
2334 : float aspeed_ms;
2335 157074 : const Vector2f closing_vel_ne_ms = landing_closing_velocity_NE_ms();
2336 157074 : const Vector2f desired_closing_vel_ne_ms = landing_desired_closing_velocity_NE_ms();
2337 157074 : const float groundspeed_ms = plane.ahrs.groundspeed();
2338 157074 : const float distance_m = plane.auto_state.wp_distance;
2339 157074 : const float closing_speed_ms = closing_vel_ne_ms.length();
2340 157074 : const float desired_closing_speed_ms = desired_closing_vel_ne_ms.length();
2341 157074 : if (!plane.ahrs.airspeed_EAS(aspeed_ms)) {
2342 0 : aspeed_ms = groundspeed_ms;
2343 : }
2344 :
2345 157074 : if (tiltrotor.enabled() && poscontrol.get_state() == QPOS_AIRBRAKE) {
2346 11648 : if ((now_ms - last_pidz_active_ms > 2000 && tiltrotor.tilt_over_max_angle()) ||
2347 4137 : tiltrotor.current_tilt >= tiltrotor.get_fully_forward_tilt()) {
2348 : // use low throttle stabilization when airbraking on a
2349 : // tiltrotor. We don't want quite zero throttle as we
2350 : // want some drag, but don't want to run the Z
2351 : // controller which can result in high throttle on
2352 : // motors that are tilted forward, thus increasing
2353 : // speed
2354 3374 : suppress_z_controller = true;
2355 3374 : hold_stabilize(0.01);
2356 : }
2357 : }
2358 :
2359 : // speed for crossover to POSITION1 controller
2360 157074 : const float aspeed_threshold_ms = MAX(plane.aparm.airspeed_min - 2, assist.speed);
2361 :
2362 : // run fixed wing navigation
2363 157074 : plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, loc);
2364 :
2365 : // use TECS for throttle
2366 157074 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.TECS_controller.get_throttle_demand());
2367 :
2368 : // use TECS for pitch
2369 157074 : int32_t commanded_pitch = plane.TECS_controller.get_pitch_demand();
2370 157074 : plane.nav_pitch_cd = constrain_int32(commanded_pitch, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100);
2371 157074 : if (poscontrol.get_state() == QPOS_AIRBRAKE) {
2372 : // don't allow down pitch in airbrake
2373 25960 : plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, 0);
2374 : }
2375 :
2376 : // use nav controller roll
2377 157074 : plane.calc_nav_roll();
2378 :
2379 : // work out the point to enter airbrake mode. We want enough
2380 : // distance to stop, plus some margin for the time it takes to
2381 : // change the accel (jerk limit) plus the min time in airbrake
2382 : // mode. For simplicity we assume 2 seconds margin
2383 157074 : const float stop_distance = stopping_distance_m() + 2*closing_speed_ms;
2384 :
2385 157074 : if (!suppress_z_controller && poscontrol.get_state() == QPOS_AIRBRAKE) {
2386 22586 : hold_hover(0);
2387 : // don't run Z controller again in this loop
2388 22586 : suppress_z_controller = true;
2389 : }
2390 :
2391 : /*
2392 : see if we should start airbraking stage. For non-tailsitters
2393 : we can use the VTOL motors as airbrakes by firing them up
2394 : before we transition. This gives a smoother transition and
2395 : gives us a nice lot of deceleration
2396 : */
2397 157074 : if (poscontrol.get_state() == QPOS_APPROACH && distance_m < stop_distance) {
2398 20 : if (tailsitter.enabled() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
2399 : // tailsitters don't use airbrake stage for landing
2400 1 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 v=%.1f d=%.0f sd=%.0f h=%.1f",
2401 : groundspeed_ms,
2402 1 : plane.auto_state.wp_distance,
2403 : stop_distance,
2404 1 : plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING));
2405 1 : poscontrol.set_state(QPOS_POSITION1);
2406 1 : transition->set_last_fw_pitch();
2407 : } else {
2408 38 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL airbrake v=%.1f d=%.0f sd=%.0f h=%.1f",
2409 : groundspeed_ms,
2410 : distance_m,
2411 : stop_distance,
2412 19 : plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING));
2413 19 : poscontrol.set_state(QPOS_AIRBRAKE);
2414 : }
2415 : }
2416 :
2417 : /*
2418 : we must switch to POSITION1 if our airspeed drops below the
2419 : assist speed. We additionally switch to POSITION1 if we are
2420 : too far above our desired velocity profile, or our attitude
2421 : has deviated too much
2422 : */
2423 157074 : const int32_t attitude_error_threshold_cd = 1000;
2424 :
2425 : // use at least 1s of airbrake time to ensure motors have a chance to
2426 : // properly spin up
2427 157074 : const uint32_t min_airbrake_ms = 1000;
2428 183053 : if (poscontrol.get_state() == QPOS_AIRBRAKE &&
2429 183053 : poscontrol.time_since_state_start_ms() > min_airbrake_ms &&
2430 13963 : (aspeed_ms < aspeed_threshold_ms || // too low airspeed
2431 13949 : fabsf(degrees(closing_vel_ne_ms.angle(desired_closing_vel_ne_ms))) > 60 || // wrong direction
2432 171019 : closing_speed_ms > MAX(desired_closing_speed_ms*1.2, desired_closing_speed_ms+2) || // too fast
2433 13925 : closing_speed_ms < desired_closing_speed_ms*0.5 || // too slow ground speed
2434 13925 : labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || // bad attitude
2435 13923 : labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) {
2436 40 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 v=%.1f d=%.1f h=%.1f dc=%.1f",
2437 : (double)groundspeed_ms,
2438 40 : (double)plane.auto_state.wp_distance,
2439 40 : plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING),
2440 : desired_closing_speed_ms);
2441 40 : poscontrol.set_state(QPOS_POSITION1);
2442 40 : transition->set_last_fw_pitch();
2443 :
2444 : // switch to vfwd for throttle control
2445 40 : vel_forward.integrator = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
2446 :
2447 : // adjust the initial forward throttle based on our desired and actual closing speed
2448 : // this allows for significant initial forward throttle
2449 : // when we have a strong headwind, but low throttle in the usual case where
2450 : // we want to slow down ready for POSITION2
2451 40 : vel_forward.integrator = linear_interpolate(0, vel_forward.integrator,
2452 : closing_speed_ms,
2453 : 1.2*desired_closing_speed_ms, 0.5*desired_closing_speed_ms);
2454 :
2455 : // limit our initial forward throttle in POSITION1 to be 0.5 of cruise throttle
2456 40 : vel_forward.integrator = constrain_float(vel_forward.integrator, 0, plane.aparm.throttle_cruise*0.5);
2457 :
2458 40 : vel_forward.last_ms = now_ms;
2459 : }
2460 :
2461 157074 : if (!tiltrotor.enabled() && !tailsitter.enabled()) {
2462 : /*
2463 : cope with fwd motor thrust loss during approach. We detect
2464 : this by looking for the fwd throttle saturating. This only
2465 : applies to separate lift-thrust vehicles
2466 : */
2467 130948 : bool throttle_saturated = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) >= plane.aparm.throttle_max;
2468 0 : if (throttle_saturated &&
2469 0 : motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED &&
2470 130948 : plane.auto_state.sink_rate > 0.2 && aspeed_ms < aspeed_threshold_ms + 4) {
2471 0 : if (poscontrol.thrust_loss_start_ms == 0) {
2472 0 : poscontrol.thrust_loss_start_ms = now_ms;
2473 : }
2474 0 : if (now_ms - poscontrol.thrust_loss_start_ms > 5000) {
2475 0 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 thrust loss as=%.1f at=%.1f",
2476 : aspeed_ms, aspeed_threshold_ms);
2477 0 : poscontrol.set_state(QPOS_POSITION1);
2478 0 : transition->set_last_fw_pitch();
2479 : }
2480 : } else {
2481 130948 : poscontrol.thrust_loss_start_ms = 0;
2482 : }
2483 :
2484 : // handle loss of forward thrust in approach based on low airspeed detection
2485 130948 : if (poscontrol.get_state() == QPOS_APPROACH && aspeed_ms < aspeed_threshold_ms &&
2486 0 : motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
2487 0 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 low speed as=%.1f at=%.1f",
2488 : aspeed_ms, aspeed_threshold_ms);
2489 0 : poscontrol.set_state(QPOS_POSITION1);
2490 0 : transition->set_last_fw_pitch();
2491 : }
2492 : }
2493 :
2494 157074 : if (poscontrol.get_state() == QPOS_APPROACH) {
2495 131094 : poscontrol_init_approach();
2496 : }
2497 157074 : break;
2498 : }
2499 :
2500 217588 : case QPOS_POSITION1: {
2501 217588 : setup_target_position();
2502 :
2503 217588 : if (tailsitter.enabled() && tailsitter.in_vtol_transition(now_ms)) {
2504 379 : break;
2505 : }
2506 :
2507 217209 : const Vector2f wp_distance_ne_m = plane.current_loc.get_distance_NE(loc);
2508 217209 : const float wp_distance_m = wp_distance_ne_m.length();
2509 217209 : const Vector2f rel_groundspeed_vector_ne_ms = landing_closing_velocity_NE_ms();
2510 217209 : const float rel_groundspeed_sq = rel_groundspeed_vector_ne_ms.length_squared();
2511 217209 : float closing_groundspeed_ms = 0;
2512 :
2513 217209 : if (wp_distance_m > 0.1) {
2514 217198 : closing_groundspeed_ms = rel_groundspeed_vector_ne_ms * wp_distance_ne_m.normalized();
2515 : }
2516 :
2517 : // calculate speed we should be at to reach the position2
2518 : // target speed at the position2 distance threshold, assuming
2519 : // Q_TRANS_DECEL is correct
2520 217209 : const float stopping_speed_ms = safe_sqrt(MAX(0, wp_distance_m - position2_dist_threshold_m) * 2 * transition_decel_mss + sq(position2_target_speed_ms));
2521 :
2522 217209 : float approach_speed_ms = stopping_speed_ms;
2523 :
2524 : // maximum configured VTOL speed
2525 217209 : const float wp_speed_ms = MAX(1.0, wp_nav->get_default_speed_NE_ms());
2526 217209 : const float scaled_wp_speed_ms = get_scaled_wp_speed(degrees(wp_distance_ne_m.angle()));
2527 :
2528 : // limit target speed to a the pos1 speed limit, which starts out at the initial speed
2529 : // but is adjusted if we start putting our nose down. We always allow at least twice
2530 : // the WP speed
2531 217209 : approach_speed_ms = MIN(MAX(poscontrol.pos1_speed_limit_ms, 2 * wp_speed_ms), approach_speed_ms);
2532 :
2533 139462 : if (poscontrol.reached_wp_speed ||
2534 356671 : rel_groundspeed_sq < sq(wp_speed_ms) ||
2535 139439 : wp_speed_ms > 1.35*scaled_wp_speed_ms) {
2536 : // once we get below the Q_WP_SPD then we don't want to
2537 : // speed up again. At that point we should fly within the
2538 : // limits of the configured VTOL controller we also apply
2539 : // this limit when we are more than 45 degrees off the
2540 : // target in yaw, which is when we start to become
2541 : // unstable
2542 77776 : approach_speed_ms = MIN(approach_speed_ms, scaled_wp_speed_ms);
2543 77776 : poscontrol.reached_wp_speed = true;
2544 : }
2545 :
2546 : // run fixed wing navigation
2547 217209 : plane.nav_controller->update_waypoint(plane.current_loc, loc);
2548 :
2549 217209 : Vector2f target_speed_ne_ms;
2550 217209 : Vector2f target_accel_ne_mss;
2551 217209 : bool have_target_yaw = false;
2552 : float target_yaw_deg;
2553 217209 : const float approach_accel_mss = MIN(accel_needed(wp_distance_m, sq(closing_groundspeed_ms)), transition_decel_mss * 2);
2554 217209 : if (wp_distance_m > 0.1) {
2555 217198 : Vector2f diff_wp_norm = wp_distance_ne_m.normalized();
2556 217198 : target_speed_ne_ms = diff_wp_norm * approach_speed_ms;
2557 217198 : target_accel_ne_mss = diff_wp_norm * (-approach_accel_mss);
2558 217198 : target_yaw_deg = degrees(diff_wp_norm.angle());
2559 217198 : const float yaw_err_deg = wrap_180(target_yaw_deg - plane.ahrs.get_yaw_deg());
2560 217198 : bool overshoot = (closing_groundspeed_ms < 0 || fabsf(yaw_err_deg) > 60);
2561 217198 : if (overshoot && !poscontrol.overshoot) {
2562 14 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL Overshoot d=%.1f cs=%.1f yerr=%.1f",
2563 : wp_distance_m, closing_groundspeed_ms, yaw_err_deg);
2564 14 : poscontrol.overshoot = true;
2565 14 : pos_control->set_accel_desired_NE_mss(Vector2f());
2566 : }
2567 217198 : if (poscontrol.overshoot) {
2568 : /* we have overshot the landing point or our nose is
2569 : off by more than 60 degrees. Zero target accel and
2570 : point nose at the landing point. Set target speed
2571 : to our position2 threshold speed
2572 : */
2573 70130 : target_accel_ne_mss.zero();
2574 :
2575 : // allow up to the WP speed when we are further away, slowing to the pos2 target speed
2576 : // when we are close
2577 70130 : approach_speed_ms = linear_interpolate(position2_target_speed_ms, wp_speed_ms,
2578 : wp_distance_m,
2579 : position2_dist_threshold_m*1.5,
2580 70130 : 2*position2_dist_threshold_m + stopping_distance_m(rel_groundspeed_sq));
2581 :
2582 70130 : target_speed_ne_ms = diff_wp_norm * approach_speed_ms;
2583 :
2584 : // adjust target yaw angle into the apparent wind
2585 70130 : Vector3f wind_ned_ms;
2586 : // use the estimate even if it is not marked valid, to
2587 : // preserve existing behaviour
2588 70130 : IGNORE_RETURN(plane.ahrs.get_wind(wind_ned_ms));
2589 70130 : const Vector2f wind_ms = wind_ned_ms.xy();
2590 70130 : const Vector2f airspeed_ne_ms = plane.ahrs.groundspeed_vector() - wind_ms;
2591 70130 : if (airspeed_ne_ms.length_squared() < 1) {
2592 : // Don't cause unpredictable yaw swings if the airspeed vector
2593 : // is very small; let the weathervane logic handle it instead.
2594 3098 : have_target_yaw = false;
2595 : } else {
2596 67032 : have_target_yaw = true;
2597 67032 : target_yaw_deg = degrees(airspeed_ne_ms.angle());
2598 : }
2599 : }
2600 : }
2601 217209 : const float target_speed_ms = target_speed_ne_ms.length();
2602 :
2603 217209 : target_speed_ne_ms += landing_velocity_ne_ms;
2604 217209 : poscontrol.target_speed_ms = target_speed_ms;
2605 217209 : poscontrol.target_accel_mss = approach_accel_mss;
2606 :
2607 139433 : if (!poscontrol.reached_wp_speed &&
2608 190556 : rel_groundspeed_sq < sq(target_speed_ms) &&
2609 407765 : rel_groundspeed_sq > sq(2*wp_speed_ms) &&
2610 48686 : plane.nav_pitch_cd < 0) {
2611 : // we have slowed down more than expected, likely due to
2612 : // drag from the props and we're starting to put our nose
2613 : // down as a result. We want to accept the slowdown and
2614 : // re-calculate the target speed profile
2615 12366 : poscontrol.pos1_speed_limit_ms = sqrtf(rel_groundspeed_sq);
2616 : }
2617 :
2618 : // use input shaping and abide by accel and jerk limits
2619 217209 : pos_control->input_vel_accel_NE_m(target_speed_ne_ms, target_accel_ne_mss);
2620 : // During POS1, we only want to control velocity and acceleration
2621 217209 : pos_control->NE_stop_pos_stabilisation();
2622 :
2623 : // run horizontal velocity controller
2624 217209 : run_xy_controller(MAX(approach_accel_mss, transition_decel_mss)*1.5);
2625 :
2626 217209 : if (!poscontrol.done_accel_init) {
2627 : /*
2628 : the pos controller init assumes zero accel, we need to
2629 : override that so that we can start decelerating more
2630 : quickly at the start of POSITION1
2631 : */
2632 78 : poscontrol.done_accel_init = true;
2633 78 : pos_control->set_accel_desired_NE_mss(target_accel_ne_mss);
2634 : }
2635 :
2636 : // nav roll and pitch are controller by position controller
2637 217209 : plane.nav_roll_cd = pos_control->get_roll_cd();
2638 217209 : plane.nav_pitch_cd = pos_control->get_pitch_cd();
2639 :
2640 217209 : assign_tilt_to_fwd_thr();
2641 :
2642 217209 : if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
2643 20545 : pos_control->NE_set_externally_limited();
2644 : }
2645 :
2646 : // call attitude controller
2647 217209 : disable_yaw_rate_time_constant();
2648 :
2649 : // setup scaling of roll and pitch angle P gains to match fixed wing gains
2650 217209 : setup_rp_fw_angle_gains();
2651 :
2652 217209 : if (have_target_yaw) {
2653 67032 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(plane.nav_roll_cd,
2654 67032 : plane.nav_pitch_cd,
2655 : target_yaw_deg * 100, true);
2656 : } else {
2657 300354 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(plane.nav_roll_cd,
2658 150177 : plane.nav_pitch_cd,
2659 150177 : desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
2660 : }
2661 217673 : if ((plane.auto_state.wp_distance < position2_dist_threshold_m) && tiltrotor.tilt_angle_achieved() &&
2662 464 : fabsf(rel_groundspeed_sq) < sq(3 * position2_target_speed_ms)) {
2663 : // if continuous tiltrotor only advance to position 2 once tilts have finished moving
2664 78 : poscontrol.set_state(QPOS_POSITION2);
2665 78 : poscontrol.pilot_correction_done = false;
2666 156 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f h=%.1f",
2667 78 : (double)ahrs.groundspeed(), (double)plane.auto_state.wp_distance,
2668 78 : plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING));
2669 : }
2670 217209 : break;
2671 : }
2672 :
2673 585460 : case QPOS_POSITION2:
2674 : case QPOS_LAND_ABORT:
2675 : case QPOS_LAND_DESCEND: {
2676 585460 : setup_target_position();
2677 : /*
2678 : for final land repositioning and descent we run the position controller
2679 : */
2680 585460 : Vector2f zero;
2681 585460 : Vector2f vel_ne_ms = poscontrol.target_vel_ms.xy() + landing_velocity_ne_ms;
2682 585460 : pos_control->input_pos_vel_accel_NE_m(poscontrol.target_ned_m.xy(), vel_ne_ms, zero);
2683 :
2684 : // also run fixed wing navigation
2685 585460 : plane.nav_controller->update_waypoint(plane.current_loc, loc);
2686 :
2687 585460 : update_land_positioning();
2688 :
2689 585460 : run_xy_controller(transition_decel_mss*1.5);
2690 :
2691 : // nav roll and pitch are controlled by position controller
2692 585460 : plane.nav_roll_cd = pos_control->get_roll_cd();
2693 585460 : plane.nav_pitch_cd = pos_control->get_pitch_cd();
2694 :
2695 585460 : assign_tilt_to_fwd_thr();
2696 :
2697 585460 : if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
2698 589 : pos_control->NE_set_externally_limited();
2699 : }
2700 :
2701 : // call attitude controller
2702 585460 : set_pilot_yaw_rate_time_constant();
2703 1170920 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(plane.nav_roll_cd,
2704 585460 : plane.nav_pitch_cd,
2705 585460 : get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
2706 585460 : break;
2707 : }
2708 :
2709 277468 : case QPOS_LAND_FINAL:
2710 277468 : setup_target_position();
2711 277468 : update_land_positioning();
2712 :
2713 : // relax when close to the ground
2714 277468 : if (should_relax()) {
2715 60956 : pos_control->NE_relax_velocity_controller();
2716 : } else {
2717 216512 : Vector2f zero;
2718 216512 : Vector2f vel_ne_ms = poscontrol.target_vel_ms.xy() + landing_velocity_ne_ms;
2719 216512 : const uint16_t last_reset_count = plane.ahrs.get_position_NE_reset_count();
2720 : /* we use velocity control when we may be touching the
2721 : ground or if we've had a position reset from AHRS. This
2722 : helps us handle a GPS glitch in the final land phase,
2723 : and also prevents trying to reposition after touchdown
2724 : */
2725 634756 : if (motors->limit.throttle_lower ||
2726 411334 : motors->get_throttle() < 0.5*motors->get_throttle_hover() ||
2727 194822 : last_reset_count != poscontrol.ahrs_position_NE_reset_count) {
2728 21690 : pos_control->input_vel_accel_NE_m(vel_ne_ms, zero);
2729 : } else {
2730 : // otherwise use full pos control
2731 194822 : pos_control->input_pos_vel_accel_NE_m(poscontrol.target_ned_m.xy(), vel_ne_ms, zero);
2732 : }
2733 : }
2734 :
2735 277468 : run_xy_controller();
2736 :
2737 : // nav roll and pitch are controller by position controller
2738 277468 : plane.nav_roll_cd = pos_control->get_roll_cd();
2739 277468 : plane.nav_pitch_cd = pos_control->get_pitch_cd();
2740 :
2741 277468 : assign_tilt_to_fwd_thr();
2742 :
2743 : // call attitude controller
2744 277468 : set_pilot_yaw_rate_time_constant();
2745 554936 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(plane.nav_roll_cd,
2746 277468 : plane.nav_pitch_cd,
2747 277468 : get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
2748 277468 : break;
2749 :
2750 29884 : case QPOS_LAND_COMPLETE:
2751 : // nothing to do
2752 29884 : break;
2753 : }
2754 :
2755 : // now height control
2756 1267474 : switch (poscontrol.get_state()) {
2757 0 : case QPOS_NONE:
2758 0 : poscontrol.set_state(QPOS_POSITION1);
2759 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2760 0 : break;
2761 :
2762 157033 : case QPOS_APPROACH:
2763 : case QPOS_AIRBRAKE:
2764 : // we just want stability from the VTOL controller in these
2765 : // phases of landing, so relax the Z controller, unless we are
2766 : // providing assistance
2767 157033 : if (transition->complete()) {
2768 131134 : pos_control->D_relax_controller(0);
2769 : }
2770 157033 : break;
2771 217551 : case QPOS_POSITION1:
2772 217551 : if (tailsitter.in_vtol_transition(now_ms)) {
2773 380 : pos_control->D_relax_controller(0);
2774 380 : break;
2775 : }
2776 : FALLTHROUGH;
2777 : case QPOS_POSITION2: {
2778 491702 : bool vtol_loiter_auto = false;
2779 491702 : if (plane.control_mode == &plane.mode_auto) {
2780 151112 : switch (plane.mission.get_current_nav_cmd().id) {
2781 0 : case MAV_CMD_NAV_LOITER_UNLIM:
2782 : case MAV_CMD_NAV_LOITER_TIME:
2783 : case MAV_CMD_NAV_LOITER_TURNS:
2784 : case MAV_CMD_NAV_LOITER_TO_ALT:
2785 0 : vtol_loiter_auto = true;
2786 0 : break;
2787 : }
2788 : }
2789 491702 : if (plane.control_mode == &plane.mode_guided || vtol_loiter_auto) {
2790 : // FIXME: we have updated current_loc without updating the
2791 : // health flag. And why are we doing this here?!
2792 257498 : UNUSED_RESULT(plane.ahrs.get_location(plane.current_loc));
2793 : int32_t target_altitude_cm;
2794 257498 : if (!plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN,target_altitude_cm)) {
2795 2767 : break;
2796 : }
2797 383621 : if (poscontrol.slow_descent &&
2798 128890 : plane.prev_WP_loc.get_distance(plane.next_WP_loc) > 50) {
2799 : // gradually descend as we approach target
2800 115731 : plane.auto_state.wp_proportion = plane.current_loc.line_path_proportion(plane.prev_WP_loc, plane.next_WP_loc);
2801 : int32_t prev_alt_cm;
2802 115731 : if (plane.prev_WP_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN,prev_alt_cm)) {
2803 115731 : target_altitude_cm = linear_interpolate(prev_alt_cm,
2804 : target_altitude_cm,
2805 : plane.auto_state.wp_proportion,
2806 : 0, 1);
2807 : }
2808 : }
2809 254731 : float zero = 0;
2810 254731 : float target_d_m = -target_altitude_cm * 0.01;
2811 254731 : pos_control->input_pos_vel_accel_D_m(target_d_m, zero, 0);
2812 488935 : } else if (plane.control_mode == &plane.mode_qrtl) {
2813 83092 : Location loc2 = loc;
2814 83092 : loc2.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN);
2815 83092 : float target_d_m = -(loc2.alt * 0.01);
2816 83092 : float zero = 0;
2817 83092 : pos_control->input_pos_vel_accel_D_m(target_d_m, zero, 0);
2818 : } else {
2819 151112 : set_climb_rate_ms(0);
2820 : }
2821 488935 : break;
2822 : }
2823 :
2824 588475 : case QPOS_LAND_DESCEND:
2825 : case QPOS_LAND_ABORT:
2826 : case QPOS_LAND_FINAL: {
2827 588475 : float height_above_ground_m = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
2828 588475 : if (poscontrol.get_state() == QPOS_LAND_FINAL) {
2829 277468 : if (!option_is_set(QuadPlane::Option::DISABLE_GROUND_EFFECT_COMP)) {
2830 277468 : ahrs.set_touchdown_expected(true);
2831 : }
2832 : }
2833 588475 : if (poscontrol.get_state() == QPOS_LAND_ABORT) {
2834 0 : set_climb_rate_ms(wp_nav->get_default_speed_up_ms());
2835 0 : break;
2836 : }
2837 588475 : const float descent_rate_ms = landing_descent_rate_ms(height_above_ground_m);
2838 588475 : pos_control->D_set_pos_target_from_climb_rate_ms(-descent_rate_ms, descent_rate_ms > 0);
2839 588475 : break;
2840 : }
2841 :
2842 29884 : case QPOS_LAND_COMPLETE:
2843 29884 : break;
2844 : }
2845 :
2846 : /*
2847 : run the z controller unless something has already run it or set a target throttle
2848 : */
2849 1267474 : if (!suppress_z_controller) {
2850 : // otherwise run z controller
2851 1241514 : run_z_controller();
2852 : }
2853 :
2854 : #if HAL_LOGGING_ENABLED
2855 1267474 : if (now_ms - poscontrol.last_log_ms >= 40) {
2856 : // log poscontrol at 25Hz
2857 105186 : poscontrol.last_log_ms = now_ms;
2858 105186 : log_QPOS();
2859 : }
2860 : #endif
2861 : }
2862 :
2863 : /*
2864 : determine which fwd throttle handling method is active
2865 : */
2866 6846771 : QuadPlane::ActiveFwdThr QuadPlane::get_vfwd_method(void) const
2867 : {
2868 : // May not be enabled by type spesfic transition logic
2869 6846771 : if (!transition->allow_vfwd()) {
2870 17499 : return ActiveFwdThr::NONE;
2871 : }
2872 :
2873 6829272 : const bool have_fwd_thr_gain = is_positive(q_fwd_thr_gain);
2874 6829272 : const bool have_vfwd_gain = is_positive(vel_forward.gain);
2875 :
2876 : #if AP_ICENGINE_ENABLED
2877 6829272 : const auto ice_state = plane.g2.ice_control.get_state();
2878 6829272 : if (ice_state != AP_ICEngine::ICE_DISABLED && ice_state != AP_ICEngine::ICE_RUNNING) {
2879 : // we need the engine running for fwd throttle
2880 100181 : return ActiveFwdThr::NONE;
2881 : }
2882 : #endif
2883 :
2884 : #if QAUTOTUNE_ENABLED
2885 6729091 : if (plane.control_mode == &plane.mode_qautotune) {
2886 259048 : return ActiveFwdThr::NONE;
2887 : }
2888 : #endif
2889 :
2890 6470043 : if (have_fwd_thr_gain) {
2891 6470043 : if (vfwd_enable_active) {
2892 : // user has used AUX function to activate new method
2893 0 : return ActiveFwdThr::NEW;
2894 : }
2895 6470043 : if (q_fwd_thr_use == FwdThrUse::ALL) {
2896 41896 : return ActiveFwdThr::NEW;
2897 : }
2898 6428147 : if (q_fwd_thr_use == FwdThrUse::POSCTRL && pos_control->NE_is_active()) {
2899 0 : return ActiveFwdThr::NEW;
2900 : }
2901 : }
2902 6428147 : if (have_vfwd_gain && pos_control->NE_is_active()) {
2903 265499 : return ActiveFwdThr::OLD;
2904 : }
2905 6162648 : return ActiveFwdThr::NONE;
2906 : }
2907 :
2908 : /*
2909 : map from pitch tilt to fwd throttle when enabled
2910 : */
2911 2171921 : void QuadPlane::assign_tilt_to_fwd_thr(void)
2912 : {
2913 :
2914 2171921 : const auto fwd_thr_active = get_vfwd_method();
2915 2171921 : if (fwd_thr_active != ActiveFwdThr::NEW) {
2916 2150855 : q_fwd_throttle = 0.0f;
2917 2150855 : q_fwd_pitch_lim_cd = 100.0f * q_fwd_pitch_lim;
2918 2150855 : return;
2919 : }
2920 : // Handle the case where we are limiting the forward pitch angle to prevent negative wing lift
2921 : // and are using the forward thrust motor or tilting rotors to provide the forward acceleration
2922 21066 : float fwd_tilt_rad = radians(constrain_float(-0.01f * (float)plane.nav_pitch_cd, 0.0f, 45.0f));
2923 21066 : q_fwd_throttle = MIN(q_fwd_thr_gain * tanf(fwd_tilt_rad), 1.0f);
2924 :
2925 : // Relax forward tilt limit if the position controller is saturating in the forward direction because
2926 : // the forward thrust motor could be failed. Do not do this with tilt rotors because they do not rely on
2927 : // forward throttle during VTOL flight
2928 21066 : const float angle_max_cd = attitude_control->lean_angle_max_cd();
2929 21066 : if (!tiltrotor.enabled()) {
2930 21066 : const float fwd_tilt_range_cd = angle_max_cd - 100.0f * q_fwd_pitch_lim;
2931 21066 : if (is_positive(fwd_tilt_range_cd)) {
2932 : // rate limit the forward tilt change to slew between the motor good and motor failed
2933 : // value over 10 seconds
2934 21066 : const bool fwd_limited = plane.quadplane.pos_control->NE_is_active() and plane.quadplane.pos_control->get_fwd_pitch_is_limited();
2935 21066 : const float fwd_pitch_lim_cd_tgt = fwd_limited ? angle_max_cd : 100.0f * q_fwd_pitch_lim;
2936 21066 : const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt;
2937 21066 : q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max);
2938 : // Don't let the forward pitch limit be more than the forward pitch demand before limiting to
2939 : // avoid opening up the limit more than necessary
2940 21066 : q_fwd_pitch_lim_cd = MIN(q_fwd_pitch_lim_cd, MAX(-(float)plane.nav_pitch_cd, 100.0f * q_fwd_pitch_lim));
2941 : } else {
2942 : // take the lesser of the two limits
2943 0 : q_fwd_pitch_lim_cd = angle_max_cd;
2944 : }
2945 : }
2946 :
2947 : // Prevent the wing from being overloaded when braking from high speed in a VTOL mode
2948 21066 : float nav_pitch_upper_limit_cd = 100.0f * q_bck_pitch_lim;
2949 : float aspeed;
2950 21066 : if (is_positive(q_bck_pitch_lim) && ahrs.airspeed_EAS(aspeed)) {
2951 21066 : const float reference_speed = MAX(plane.aparm.airspeed_min, MIN_AIRSPEED_MIN);
2952 21066 : float speed_scaler = sq(reference_speed / MAX(aspeed, 0.1f));
2953 21066 : nav_pitch_upper_limit_cd *= speed_scaler;
2954 21066 : nav_pitch_upper_limit_cd = MIN(nav_pitch_upper_limit_cd, angle_max_cd);
2955 :
2956 21066 : const float tconst = 0.5f;
2957 21066 : const float dt = AP_HAL::millis() - q_pitch_limit_update_ms;
2958 21066 : q_pitch_limit_update_ms = AP_HAL::millis();
2959 21066 : if (is_positive(dt)) {
2960 21066 : const float coef = dt / (dt + tconst);
2961 21066 : q_bck_pitch_lim_cd = (1.0f - coef) * q_bck_pitch_lim_cd + coef * nav_pitch_upper_limit_cd;
2962 : }
2963 :
2964 21066 : plane.nav_pitch_cd = MIN(plane.nav_pitch_cd, (int32_t)q_bck_pitch_lim_cd);
2965 :
2966 : #if HAL_LOGGING_ENABLED
2967 : // @LoggerMessage: QBRK
2968 : // @Description: Quadplane Braking
2969 : // @Field: TimeUS: Time since system startup
2970 : // @Field: SpdScaler: braking speed scaler
2971 : // @Field: NPULCD: upper limit for navigation pitch
2972 : // @Field: QBPLCD: upper limit for back transition pitch
2973 : // @Field: NPCD: demanded navigation pitch
2974 42132 : AP::logger().WriteStreaming("QBRK",
2975 : "TimeUS,SpdScaler,NPULCD,QBPLCD,NPCD", // labels
2976 : "Qffii", // fmt
2977 : AP_HAL::micros64(),
2978 : (double)speed_scaler,
2979 : (double)nav_pitch_upper_limit_cd,
2980 21066 : (int32_t)q_bck_pitch_lim_cd,
2981 21066 : (int32_t)plane.nav_pitch_cd);
2982 : #endif
2983 : }
2984 :
2985 : float fwd_thr_scaler;
2986 21066 : if (!in_vtol_land_approach()) {
2987 : // To prevent forward motor prop strike, reduce throttle to zero when close to ground.
2988 21066 : float alt_cutoff_m = MAX(0, vel_forward_alt_cutoff_m);
2989 21066 : float height_above_ground_m = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
2990 21066 : fwd_thr_scaler = linear_interpolate(0.0f, 1.0f, height_above_ground_m, alt_cutoff_m, alt_cutoff_m + 2);
2991 : } else {
2992 : // When we are doing horizontal positioning in a VTOL land we always allow the fwd motor
2993 : // to run. Otherwise a bad height above landing point estimate could cause the aircraft
2994 : // not to be able to approach the landing point
2995 0 : fwd_thr_scaler = 1.0f;
2996 : }
2997 21066 : q_fwd_throttle *= fwd_thr_scaler;
2998 :
2999 : // When reducing forward throttle use, relax lower pitch limit to maintain forward
3000 : // acceleration capability.
3001 21066 : const float nav_pitch_lower_limit_cd = - (int32_t)(angle_max_cd * (1.0f - fwd_thr_scaler) + q_fwd_pitch_lim_cd * fwd_thr_scaler);
3002 :
3003 : #if HAL_LOGGING_ENABLED
3004 : // Diagnostics logging - remove when feature is fully flight tested.
3005 : // @LoggerMessage: FWDT
3006 : // @Description: Forward Throttle calculations
3007 : // @Field: TimeUS: Time since system startup
3008 : // @Field: fts: forward throttle scaler
3009 : // @Field: qfplcd: quadplane forward pitch limit
3010 : // @Field: npllcd: navigation pitch lower limit
3011 : // @Field: npcd: demanded navigation pitch
3012 : // @Field: qft: quadplane forward throttle
3013 : // @Field: npulcd: upper limit for navigation pitch
3014 42132 : AP::logger().WriteStreaming("FWDT",
3015 : "TimeUS,fts,qfplcd,npllcd,npcd,qft,npulcd", // labels
3016 : "Qffffff", // fmt
3017 : AP_HAL::micros64(),
3018 : (double)fwd_thr_scaler,
3019 21066 : (double)q_fwd_pitch_lim_cd,
3020 : (double)nav_pitch_lower_limit_cd,
3021 21066 : (double)plane.nav_pitch_cd,
3022 21066 : (double)q_fwd_throttle,
3023 : (float)nav_pitch_upper_limit_cd);
3024 : #endif
3025 :
3026 21066 : plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, (int32_t)nav_pitch_lower_limit_cd);
3027 : }
3028 :
3029 : /*
3030 : we want to limit WP speed to a lower speed when more than 20 degrees
3031 : off pointing at the destination. quadplanes are often
3032 : unstable when flying sideways or backwards
3033 : */
3034 217209 : float QuadPlane::get_scaled_wp_speed(float target_bearing_deg) const
3035 : {
3036 217209 : const float yaw_difference = fabsf(wrap_180(plane.ahrs.get_yaw_deg() - target_bearing_deg));
3037 217209 : const float wp_speed_ms = wp_nav->get_default_speed_NE_ms();
3038 217209 : if (yaw_difference > 20) {
3039 : // this gives a factor of 2x reduction in max speed when
3040 : // off by 90 degrees, and 3x when off by 180 degrees
3041 33162 : const float speed_reduction = linear_interpolate(1, 3,
3042 : yaw_difference,
3043 : 20, 160);
3044 33162 : return wp_speed_ms / speed_reduction;
3045 : }
3046 184047 : return wp_speed_ms;
3047 : }
3048 :
3049 : /*
3050 : setup the target position based on plane.next_WP_loc
3051 : */
3052 1369006 : void QuadPlane::setup_target_position(void)
3053 : {
3054 1369006 : const Location &loc = plane.next_WP_loc;
3055 1369006 : Location origin;
3056 1369006 : if (!ahrs.get_origin(origin)) {
3057 2769 : origin.zero();
3058 : }
3059 2738012 : if ((!in_vtol_land_approach() || poscontrol.get_state() > QPOS_APPROACH) &&
3060 1369006 : poscontrol.get_state() != QPOS_LAND_FINAL) {
3061 1091521 : set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
3062 : }
3063 :
3064 1369006 : Vector2f diff2d = origin.get_distance_NE(loc);
3065 1369006 : diff2d += poscontrol.correction_ne_m;
3066 1369006 : poscontrol.target_ned_m.x = diff2d.x;
3067 1369006 : poscontrol.target_ned_m.y = diff2d.y;
3068 1369006 : poscontrol.target_ned_m.z = -(plane.next_WP_loc.alt - origin.alt) * 0.01;
3069 :
3070 : // set vertical speed and acceleration limits
3071 : // All limits must be positive
3072 1369006 : pos_control->D_set_max_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss);
3073 1369006 : pos_control->D_set_correction_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss);
3074 1369006 : }
3075 :
3076 : /*
3077 : run takeoff controller to climb vertically
3078 : */
3079 348529 : void QuadPlane::takeoff_controller(void)
3080 : {
3081 : // reset fixed wing controller to neutral as base output
3082 348529 : plane.nav_roll_cd = 0;
3083 348529 : plane.nav_pitch_cd = 0;
3084 :
3085 : #if HAL_WITH_ESC_TELEM
3086 : // calling this early so it can clear its warning timer when disarmed
3087 348529 : const bool motor_check_passed = plane.motors_takeoff_check(takeoff_rpm_min, takeoff_rpm_max);
3088 : #endif
3089 :
3090 348529 : if (!plane.arming.is_armed_and_safety_off()) {
3091 86728 : return;
3092 : }
3093 :
3094 266344 : uint32_t now = AP_HAL::millis();
3095 266344 : const auto spool_state = motors->get_desired_spool_state();
3096 : // Don't allow takeoff until it is safe to do so
3097 266344 : if (spool_state != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
3098 : // Waiting for motors to tilt up
3099 0 : if (plane.control_mode == &plane.mode_guided && guided_takeoff
3100 4586 : && tiltrotor.enabled() && !tiltrotor.fully_up()) {
3101 0 : takeoff_start_time_ms = now;
3102 0 : return;
3103 : }
3104 : #if HAL_WITH_ESC_TELEM
3105 : // Waiting for motors to reach the expected RPM
3106 4586 : if (!motor_check_passed) {
3107 : // start motors spinning if not spinning already so RPM check can work
3108 1381 : set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
3109 1381 : takeoff_start_time_ms = now;
3110 1381 : return;
3111 : }
3112 : #endif
3113 : // Waiting for stick to be re-centered after rudder arming
3114 3205 : if (plane.arming.last_arm_method() == AP_Arming::Method::RUDDER &&
3115 3163 : (takeoff_last_run_ms == 0 ||
3116 6368 : now - takeoff_last_run_ms > 1000) &&
3117 3163 : !rc().seen_neutral_rudder()) {
3118 : // start motor spinning if not spinning already so user sees it is armed
3119 3162 : set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
3120 3162 : takeoff_start_time_ms = now;
3121 3162 : if (now - plane.takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) {
3122 3 : gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release");
3123 3 : plane.takeoff_state.rudder_takeoff_warn_ms = now;
3124 : }
3125 3162 : return;
3126 : }
3127 : }
3128 :
3129 :
3130 : /*
3131 : for takeoff we use the position controller
3132 : */
3133 261801 : setup_target_position();
3134 :
3135 : // set position control target and update
3136 :
3137 261801 : Vector2f vel_ne_ms, zero;
3138 261801 : if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) {
3139 12447 : vel_ne_ms = poscontrol.velocity_match_ms;
3140 : }
3141 :
3142 : /*
3143 : support zeroing roll/pitch during early part of takeoff. This
3144 : can help particularly with poor GPS velocity data
3145 : */
3146 261801 : bool no_navigation = false;
3147 261801 : if (takeoff_navalt_min_m > 0) {
3148 0 : const float alt_m = plane.current_loc.alt * 0.01;
3149 0 : if (takeoff_last_run_ms == 0 ||
3150 0 : now - takeoff_last_run_ms > 1000) {
3151 0 : takeoff_start_alt_m = alt_m;
3152 : }
3153 0 : if (alt_m - takeoff_start_alt_m < takeoff_navalt_min_m) {
3154 0 : no_navigation = true;
3155 : }
3156 : }
3157 261801 : takeoff_last_run_ms = now;
3158 :
3159 261801 : if (no_navigation) {
3160 0 : pos_control->NE_relax_velocity_controller();
3161 : } else {
3162 261801 : pos_control->input_vel_accel_NE_m(vel_ne_ms, zero);
3163 :
3164 : // nav roll and pitch are controller by position controller
3165 261801 : plane.nav_roll_cd = pos_control->get_roll_cd();
3166 261801 : plane.nav_pitch_cd = pos_control->get_pitch_cd();
3167 :
3168 261801 : assign_tilt_to_fwd_thr();
3169 : }
3170 :
3171 261801 : run_xy_controller();
3172 :
3173 261801 : set_pilot_yaw_rate_time_constant();
3174 523602 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(plane.nav_roll_cd,
3175 261801 : plane.nav_pitch_cd,
3176 261801 : get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
3177 :
3178 261801 : float vel_u_ms = wp_nav->get_default_speed_up_ms();
3179 261801 : if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
3180 : // for guided takeoff we aim for a specific height with zero
3181 : // velocity at that height
3182 52242 : Location origin;
3183 52242 : if (ahrs.get_origin(origin)) {
3184 : // a small margin to ensure we do move to the next takeoff
3185 : // stage
3186 52242 : const int32_t margin_cm = 5;
3187 52242 : float pos_d_m = -(margin_cm + plane.next_WP_loc.alt - origin.alt) * 0.01;
3188 52242 : float vel_d_ms = 0.0;
3189 52242 : pos_control->input_pos_vel_accel_D_m(pos_d_m, vel_d_ms, 0);
3190 : } else {
3191 0 : set_climb_rate_ms(vel_u_ms);
3192 : }
3193 52242 : } else {
3194 209559 : set_climb_rate_ms(vel_u_ms);
3195 : }
3196 :
3197 261801 : run_z_controller();
3198 : }
3199 :
3200 : /*
3201 : run waypoint controller between prev_WP_loc and next_WP_loc
3202 : */
3203 26559 : void QuadPlane::waypoint_controller(void)
3204 : {
3205 26559 : setup_target_position();
3206 :
3207 26559 : const Location &loc = plane.next_WP_loc;
3208 26559 : const uint32_t now = AP_HAL::millis();
3209 53115 : if (!loc.same_loc_as(last_auto_target) ||
3210 26556 : now - last_loiter_ms > 500) {
3211 4 : wp_nav->set_wp_destination_NED_m(poscontrol.target_ned_m);
3212 4 : last_auto_target = loc;
3213 : }
3214 26559 : last_loiter_ms = now;
3215 :
3216 : /*
3217 : this is full copter control of auto flight
3218 : */
3219 : // run wpnav controller
3220 26559 : wp_nav->update_wpnav();
3221 :
3222 : // nav roll and pitch are controller by waypoint controller
3223 26559 : plane.nav_roll_cd = wp_nav->get_roll();
3224 26559 : plane.nav_pitch_cd = wp_nav->get_pitch();
3225 :
3226 26559 : assign_tilt_to_fwd_thr();
3227 :
3228 26559 : if (transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
3229 1712 : pos_control->NE_set_externally_limited();
3230 : }
3231 :
3232 : // call attitude controller
3233 26559 : disable_yaw_rate_time_constant();
3234 53118 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(plane.nav_roll_cd,
3235 26559 : plane.nav_pitch_cd,
3236 26559 : wp_nav->get_yaw(),
3237 : true);
3238 :
3239 : // climb based on altitude error
3240 26559 : set_climb_rate_ms(assist_climb_rate_cms() * 0.01);
3241 26559 : run_z_controller();
3242 26559 : }
3243 :
3244 :
3245 : /*
3246 : handle auto-mode when auto_state.vtol_mode is true
3247 : */
3248 958749 : void QuadPlane::control_auto(void)
3249 : {
3250 958749 : if (!setup()) {
3251 0 : return;
3252 : }
3253 :
3254 958749 : if (poscontrol.get_state() > QPOS_APPROACH) {
3255 623139 : bool should_run_motors = false;
3256 :
3257 : // don't run the motors if in an arming delay
3258 623139 : if (plane.arming.get_delay_arming()) {
3259 2398 : should_run_motors = false;
3260 : }
3261 :
3262 : // don't run motors if we are in the wait state for payload place
3263 638797 : if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::SHUT_DOWN &&
3264 638797 : plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
3265 0 : poscontrol.get_state() == QPOS_LAND_COMPLETE) {
3266 0 : should_run_motors = false;
3267 : }
3268 :
3269 623139 : if (should_run_motors) {
3270 0 : set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
3271 : }
3272 : }
3273 :
3274 958749 : uint16_t id = plane.mission.get_current_nav_cmd().id;
3275 958749 : switch (id) {
3276 296287 : case MAV_CMD_NAV_VTOL_TAKEOFF:
3277 : case MAV_CMD_NAV_TAKEOFF:
3278 296287 : if (is_vtol_takeoff(id)) {
3279 296287 : takeoff_controller();
3280 : }
3281 296287 : break;
3282 635903 : case MAV_CMD_NAV_VTOL_LAND:
3283 : case MAV_CMD_NAV_PAYLOAD_PLACE:
3284 : case MAV_CMD_NAV_LAND:
3285 635903 : if (is_vtol_land(id)) {
3286 635903 : vtol_position_controller();
3287 : }
3288 635903 : break;
3289 0 : case MAV_CMD_NAV_LOITER_UNLIM:
3290 : case MAV_CMD_NAV_LOITER_TIME:
3291 : case MAV_CMD_NAV_LOITER_TURNS:
3292 : case MAV_CMD_NAV_LOITER_TO_ALT: {
3293 0 : const uint32_t now = AP_HAL::millis();
3294 0 : if (now - poscontrol.last_run_ms > 100) {
3295 : // ensure that poscontrol is reset
3296 0 : poscontrol.set_state(QPOS_POSITION1);
3297 : }
3298 0 : vtol_position_controller();
3299 : }
3300 0 : break;
3301 26559 : default:
3302 26559 : waypoint_controller();
3303 26559 : break;
3304 : }
3305 : }
3306 :
3307 : /*
3308 : start a VTOL takeoff
3309 : */
3310 2624 : bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
3311 : {
3312 2624 : if (!setup()) {
3313 0 : return false;
3314 : }
3315 :
3316 : // we always use the current location in XY for takeoff. The altitude defaults
3317 : // to relative to current height, but if Q_OPTIONS is set to respect takeoff frame
3318 : // then it will use normal frame handling for height
3319 2624 : Location loc = cmd.content.location;
3320 2624 : loc.lat = 0;
3321 2624 : loc.lng = 0;
3322 2624 : plane.set_next_WP(loc);
3323 2624 : if (option_is_set(QuadPlane::Option::RESPECT_TAKEOFF_FRAME)) {
3324 : // convert to absolute frame for takeoff
3325 0 : if (!plane.next_WP_loc.change_alt_frame(Location::AltFrame::ABSOLUTE) ||
3326 0 : plane.current_loc.alt >= plane.next_WP_loc.alt) {
3327 : // we are above the takeoff already, no need to do anything
3328 0 : return false;
3329 : }
3330 : } else {
3331 2624 : plane.next_WP_loc.set_alt_cm(plane.current_loc.alt + cmd.content.location.alt,
3332 : Location::AltFrame::ABSOLUTE);
3333 : }
3334 2624 : throttle_wait = false;
3335 :
3336 : // set vertical speed and acceleration limits
3337 : // All limits must be positive
3338 2624 : pos_control->D_set_max_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss);
3339 2624 : pos_control->D_set_correction_speed_accel_m(get_pilot_velocity_z_max_dn_m(), pilot_speed_z_max_up_ms, pilot_accel_z_mss);
3340 :
3341 : // initialise the vertical position controller
3342 2624 : pos_control->D_init_controller();
3343 :
3344 : // also update nav_controller for status output
3345 2624 : plane.nav_controller->update_waypoint(plane.current_loc, plane.next_WP_loc);
3346 :
3347 : // calculate the time required to complete a takeoff
3348 : // this may be conservative and accept extra time due to clamping
3349 : // derived from the following latex equations if you want a nicely formatted view
3350 : // t_{accel} = \frac{V_max - V_z}{a}
3351 : // d_{accel} = V_z*t_{accel} + \frac{1}{2}*a*t_{accel}^2
3352 : // d_{remaining} = d_{total} - d_{accel}
3353 : // t_{constant} = \frac{d_{remaining}}{V_z}
3354 : // t = max(t_{accel}, 0) + max(t_{constant}, 0)
3355 2624 : const float d_total_m = (plane.next_WP_loc.alt - plane.current_loc.alt) * 0.01f;
3356 2624 : const float accel_m_s_s = MAX(0.1, pilot_accel_z_mss);
3357 2624 : const float vel_max_ms = MAX(0.1, pilot_speed_z_max_up_ms);
3358 2624 : const float vel_u_ms = inertial_nav.get_velocity_z_up_cms() * 0.01f;
3359 2624 : const float t_accel_s = (vel_max_ms - vel_u_ms) / accel_m_s_s;
3360 2624 : const float d_accel_m = vel_u_ms * t_accel_s + 0.5f * accel_m_s_s * sq(t_accel_s);
3361 2624 : const float d_remaining_m = d_total_m - d_accel_m;
3362 2624 : const float t_constant = d_remaining_m / vel_max_ms;
3363 2624 : const float travel_time_s = MAX(t_accel_s, 0) + MAX(t_constant, 0);
3364 :
3365 : // setup the takeoff failure handling code
3366 2624 : takeoff_start_time_ms = millis();
3367 2624 : takeoff_time_limit_ms = MAX(travel_time_s * takeoff_failure_scalar * 1000, 5000); // minimum time 5 seconds
3368 :
3369 2624 : return true;
3370 : }
3371 :
3372 :
3373 : /*
3374 : start a VTOL landing
3375 : */
3376 31 : bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
3377 : {
3378 31 : if (!setup()) {
3379 0 : return false;
3380 : }
3381 :
3382 31 : plane.set_next_WP(cmd.content.location);
3383 : // initially aim for current altitude
3384 31 : plane.next_WP_loc.copy_alt_from(plane.current_loc);
3385 :
3386 : // initialise the position controller
3387 31 : pos_control->NE_init_controller();
3388 31 : pos_control->D_init_controller();
3389 :
3390 31 : throttle_wait = false;
3391 31 : landing_detect.lower_limit_start_ms = 0;
3392 31 : landing_detect.land_start_ms = 0;
3393 :
3394 31 : plane.crash_state.is_crashed = false;
3395 :
3396 : // also update nav_controller for status output
3397 31 : plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc,
3398 : plane.next_WP_loc);
3399 :
3400 31 : poscontrol_init_approach();
3401 31 : return true;
3402 : }
3403 :
3404 : /*
3405 : check if a VTOL takeoff has completed
3406 : */
3407 9726 : bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
3408 : {
3409 9726 : if (!available()) {
3410 0 : return true;
3411 : }
3412 :
3413 9726 : const uint32_t now = millis();
3414 :
3415 : // reset takeoff if we aren't armed
3416 9726 : if (!plane.arming.is_armed_and_safety_off()) {
3417 2575 : do_vtol_takeoff(cmd);
3418 2575 : return false;
3419 : }
3420 :
3421 8621 : if (now - takeoff_start_time_ms < 3000 &&
3422 1470 : !option_is_set(QuadPlane::Option::DISABLE_GROUND_EFFECT_COMP)) {
3423 1470 : ahrs.set_takeoff_expected(true);
3424 : }
3425 :
3426 : // check for failure conditions
3427 7151 : if (is_positive(takeoff_failure_scalar) && ((now - takeoff_start_time_ms) > takeoff_time_limit_ms)) {
3428 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff within time limit");
3429 0 : plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF);
3430 0 : return false;
3431 : }
3432 :
3433 : #if AP_AIRSPEED_ENABLED
3434 7151 : if (is_positive(maximum_takeoff_airspeed_ms) && (plane.airspeed.get_airspeed() > maximum_takeoff_airspeed_ms)) {
3435 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff, excessive wind");
3436 0 : plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF);
3437 0 : return false;
3438 : }
3439 : #endif
3440 :
3441 7151 : if (plane.current_loc.alt < plane.next_WP_loc.alt) {
3442 7107 : return false;
3443 : }
3444 44 : transition->restart();
3445 44 : plane.TECS_controller.set_pitch_max(transition_pitch_max);
3446 44 : plane.TECS_controller.set_pitch_min(-transition_pitch_max);
3447 :
3448 : // todo: why are you doing this, I want to delete it.
3449 44 : set_alt_target_current();
3450 :
3451 : #if AP_FENCE_ENABLED
3452 44 : plane.fence.auto_enable_fence_after_takeoff();
3453 : #endif
3454 :
3455 44 : if (plane.control_mode == &plane.mode_auto) {
3456 : // we reset TECS so that the target height filter is not
3457 : // constrained by the climb and sink rates from the initial
3458 : // takeoff height.
3459 44 : plane.TECS_controller.reset();
3460 : }
3461 :
3462 : // don't crosstrack on next WP
3463 44 : plane.auto_state.next_wp_crosstrack = false;
3464 :
3465 44 : return true;
3466 : }
3467 :
3468 : /*
3469 : a landing detector based on change in altitude over a timeout
3470 : */
3471 438311 : bool QuadPlane::land_detector(uint32_t timeout_ms)
3472 : {
3473 438311 : bool might_be_landed = should_relax() && !poscontrol.pilot_correction_active;
3474 438311 : if (!might_be_landed) {
3475 384378 : landing_detect.land_start_ms = 0;
3476 384378 : return false;
3477 : }
3478 53933 : const uint32_t now = AP_HAL::millis();
3479 53933 : float height_m = inertial_nav.get_position_z_up_cm() * 0.01;
3480 53933 : if (landing_detect.land_start_ms == 0) {
3481 406 : landing_detect.land_start_ms = now;
3482 406 : landing_detect.vpos_start_m = height_m;
3483 : }
3484 :
3485 : // we only consider the vehicle landed when the motors have been
3486 : // at minimum for timeout_ms+1000 and the vertical position estimate has not
3487 : // changed by more than 20cm for timeout_ms
3488 53933 : if (fabsf(height_m - landing_detect.vpos_start_m) > landing_detect.detect_alt_change_m) {
3489 : // height has changed, call off landing detection
3490 331 : landing_detect.land_start_ms = 0;
3491 331 : return false;
3492 : }
3493 :
3494 53602 : if ((now - landing_detect.land_start_ms) < timeout_ms ||
3495 69 : (now - landing_detect.lower_limit_start_ms) < (timeout_ms+1000)) {
3496 : // not landed yet
3497 53533 : return false;
3498 : }
3499 :
3500 69 : return true;
3501 : }
3502 :
3503 : /*
3504 : check if a landing is complete
3505 : */
3506 487507 : bool QuadPlane::check_land_complete(void)
3507 : {
3508 487507 : if (poscontrol.get_state() != QPOS_LAND_FINAL) {
3509 : // only apply to final landing phase
3510 295827 : return false;
3511 : }
3512 191680 : if (land_detector(4000)) {
3513 67 : poscontrol.set_state(QPOS_LAND_COMPLETE);
3514 67 : gcs().send_text(MAV_SEVERITY_INFO,"Land complete");
3515 :
3516 67 : if (plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE)) {
3517 : // for payload place with full landing we shutdown motors
3518 : // and wait for the lua script to trigger a climb (using
3519 : // landing abort) or disarm
3520 0 : set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
3521 0 : return false;
3522 : }
3523 :
3524 98 : if (plane.control_mode != &plane.mode_auto ||
3525 31 : !plane.mission.continue_after_land()) {
3526 : // disarm on land unless we have MIS_OPTIONS setup to
3527 : // continue after land in AUTO
3528 67 : plane.arming.disarm(AP_Arming::Method::LANDED);
3529 : }
3530 67 : return true;
3531 : }
3532 191613 : return false;
3533 : }
3534 :
3535 :
3536 : /*
3537 : check if we should switch from QPOS_LAND_DESCEND to QPOS_LAND_FINAL
3538 : */
3539 246696 : bool QuadPlane::check_land_final(void)
3540 : {
3541 246696 : float height_above_ground_m = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
3542 : // we require 2 readings at 10Hz to be within 5m of each other to
3543 : // trigger the switch to land final. This prevents a short term
3544 : // glitch at high altitude from triggering land final
3545 246696 : const float max_change_m = 5;
3546 246761 : if (height_above_ground_m < land_final_alt_m &&
3547 65 : fabsf(height_above_ground_m - last_land_final_agl_m) < max_change_m) {
3548 65 : return true;
3549 : }
3550 246631 : last_land_final_agl_m = height_above_ground_m;
3551 :
3552 : /*
3553 : also apply landing detector, in case we have landed in descent
3554 : phase. Use a longer threshold
3555 : */
3556 246631 : return land_detector(6000);
3557 : }
3558 :
3559 : /*
3560 : check if a VTOL landing has completed
3561 : */
3562 219760 : bool QuadPlane::verify_vtol_land(void)
3563 : {
3564 219760 : if (!available()) {
3565 0 : return true;
3566 : }
3567 :
3568 219760 : if (poscontrol.get_state() == QPOS_POSITION2) {
3569 : // see if we should move onto the descend stage of landing
3570 19634 : const float descend_dist_threshold_m = 2.0;
3571 19634 : const float descend_speed_threshold_ms = 3.0;
3572 19634 : bool reached_position = false;
3573 19634 : if (poscontrol.pilot_correction_done) {
3574 0 : reached_position = !poscontrol.pilot_correction_active;
3575 : } else {
3576 19634 : const float dist_m = (inertial_nav.get_position_neu_cm().topostype() * 0.01 - poscontrol.target_ned_m).xy().length();
3577 19634 : reached_position = dist_m < descend_dist_threshold_m;
3578 : }
3579 19634 : Vector2f approach_vel_ne_ms;
3580 19634 : if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) {
3581 732 : approach_vel_ne_ms = poscontrol.velocity_match_ms;
3582 : }
3583 19634 : Vector3f vel_ned_ms;
3584 19634 : UNUSED_RESULT(plane.ahrs.get_velocity_NED(vel_ned_ms));
3585 :
3586 20080 : if (reached_position &&
3587 20080 : (vel_ned_ms.xy() - approach_vel_ne_ms).length() < descend_speed_threshold_ms) {
3588 50 : poscontrol.set_state(QPOS_LAND_DESCEND);
3589 50 : poscontrol.pilot_correction_done = false;
3590 50 : pos_control->set_lean_angle_max_cd(0);
3591 50 : poscontrol.correction_ne_m.zero();
3592 : #if AP_LANDINGGEAR_ENABLED
3593 50 : plane.g2.landing_gear.deploy_for_landing();
3594 : #endif
3595 50 : last_land_final_agl_m = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
3596 50 : gcs().send_text(MAV_SEVERITY_INFO,"Land descend started");
3597 50 : if (plane.control_mode == &plane.mode_auto) {
3598 : // set height to mission height, so we can use the mission
3599 : // WP height for triggering land final if no rangefinder
3600 : // available
3601 31 : plane.set_next_WP(plane.mission.get_current_nav_cmd().content.location);
3602 : } else {
3603 19 : plane.set_next_WP(plane.next_WP_loc);
3604 19 : plane.next_WP_loc.copy_alt_from(ahrs.get_home());
3605 : }
3606 : }
3607 : }
3608 :
3609 : // at land_final_alt_m begin final landing
3610 219760 : if (poscontrol.get_state() == QPOS_LAND_DESCEND && check_land_final()) {
3611 50 : poscontrol.set_state(QPOS_LAND_FINAL);
3612 :
3613 : #if AP_ICENGINE_ENABLED
3614 : // cut IC engine if enabled
3615 50 : if (land_icengine_cut != 0) {
3616 50 : plane.g2.ice_control.engine_control(0, 0, 0, false);
3617 : }
3618 : #endif // AP_ICENGINE_ENABLED
3619 50 : gcs().send_text(MAV_SEVERITY_INFO,"Land final started");
3620 : }
3621 :
3622 : // at land_final_alt_m begin final landing
3623 219760 : if (poscontrol.get_state() == QPOS_LAND_ABORT &&
3624 0 : plane.current_loc.alt * 0.01 >= land_descend_start_alt_m) {
3625 : // continue to next WP, if there is one
3626 0 : return true;
3627 : }
3628 :
3629 219760 : if (plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
3630 0 : (poscontrol.get_state() == QPOS_LAND_DESCEND ||
3631 0 : poscontrol.get_state() == QPOS_LAND_FINAL)) {
3632 0 : const auto &cmd = plane.mission.get_current_nav_cmd();
3633 0 : if (cmd.p1 > 0 && plane.current_loc.alt * 0.01 < land_descend_start_alt_m - cmd.p1 * 0.01) {
3634 0 : gcs().send_text(MAV_SEVERITY_INFO,"Payload place aborted");
3635 0 : poscontrol.set_state(QPOS_LAND_ABORT);
3636 : }
3637 : }
3638 :
3639 219760 : if (check_land_complete() && plane.mission.continue_after_land()) {
3640 0 : gcs().send_text(MAV_SEVERITY_INFO,"Mission continue");
3641 0 : return true;
3642 : }
3643 219760 : return false;
3644 : }
3645 :
3646 : #if HAL_LOGGING_ENABLED
3647 : // Write a control tuning packet
3648 205625 : void QuadPlane::Log_Write_QControl_Tuning()
3649 : {
3650 205625 : float des_alt_m = 0.0f;
3651 205625 : float target_climb_rate_ms = 0;
3652 205625 : if (plane.control_mode != &plane.mode_qstabilize) {
3653 204916 : des_alt_m = pos_control->get_pos_desired_U_m();
3654 204916 : target_climb_rate_ms = pos_control->get_vel_target_U_ms();
3655 : }
3656 :
3657 : // Assemble assistance bitmask, definition here is used to generate log documentation
3658 : enum class log_assistance_flags {
3659 : in_assisted_flight = 1U<<0, // true if VTOL assist is active
3660 : forced = 1U<<1, // true if assistance is forced
3661 : speed = 1U<<2, // true if assistance due to low airspeed
3662 : alt = 1U<<3, // true if assistance due to low altitude
3663 : angle = 1U<<4, // true if assistance due to attitude error
3664 : fw_force = 1U<<5, // true if forcing use of fixed wing controllers
3665 : spin_recovery = 1U<<6, // true if recovering from a spin
3666 : };
3667 :
3668 205625 : uint8_t assist_flags = 0;
3669 205625 : if (assisted_flight) {
3670 28631 : assist_flags |= (uint8_t)log_assistance_flags::in_assisted_flight;
3671 : }
3672 205625 : if (assist.in_force_assist()) {
3673 0 : assist_flags |= (uint8_t)log_assistance_flags::forced;
3674 : }
3675 205625 : if (assist.in_speed_assist()) {
3676 11942 : assist_flags |= (uint8_t)log_assistance_flags::speed;
3677 : }
3678 205625 : if (assist.in_alt_assist()) {
3679 232 : assist_flags |= (uint8_t)log_assistance_flags::alt;
3680 : }
3681 205625 : if (assist.in_angle_assist()) {
3682 1582 : assist_flags |= (uint8_t)log_assistance_flags::angle;
3683 : }
3684 205625 : if (force_fw_control_recovery) {
3685 2451 : assist_flags |= (uint8_t)log_assistance_flags::fw_force;
3686 : }
3687 205625 : if (in_spin_recovery) {
3688 227 : assist_flags |= (uint8_t)log_assistance_flags::spin_recovery;
3689 : }
3690 :
3691 205625 : struct log_QControl_Tuning pkt = {
3692 : LOG_PACKET_HEADER_INIT(LOG_QTUN_MSG),
3693 205625 : time_us : AP_HAL::micros64(),
3694 205625 : throttle_in : attitude_control->get_throttle_in(),
3695 205625 : angle_boost : attitude_control->angle_boost(),
3696 205625 : throttle_out : motors->get_throttle(),
3697 205625 : throttle_hover : motors->get_throttle_hover(),
3698 : desired_alt : des_alt_m,
3699 411250 : inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f,
3700 411250 : baro_alt : int32_t(plane.barometer.get_altitude() * 100),
3701 205625 : target_climb_rate : int16_t(target_climb_rate_ms * 100.0),
3702 411250 : climb_rate : int16_t(inertial_nav.get_velocity_z_up_cms()),
3703 205625 : throttle_mix : attitude_control->get_throttle_mix(),
3704 205625 : transition_state : transition->get_log_transition_state(),
3705 : assist : assist_flags,
3706 205625 : };
3707 205625 : plane.logger.WriteBlock(&pkt, sizeof(pkt));
3708 :
3709 : // write multicopter position control message
3710 205625 : pos_control->write_log();
3711 :
3712 : // Write tiltrotor tilt angle log
3713 205625 : tiltrotor.write_log();
3714 205625 : }
3715 : #endif
3716 :
3717 :
3718 : /*
3719 : calculate the forward throttle percentage. The forward throttle can
3720 : be used to assist with position hold and with landing approach. It
3721 : reduces the need for down pitch which reduces load on the vertical
3722 : lift motors.
3723 : */
3724 2292091 : float QuadPlane::forward_throttle_pct()
3725 : {
3726 : // handle special case where forward thrust motor is used instead of forward pitch.
3727 2292091 : if (get_vfwd_method() == ActiveFwdThr::NEW) {
3728 20830 : return 100.0f * q_fwd_throttle;
3729 : }
3730 :
3731 : /*
3732 : Unless an RC channel is assigned for manual forward throttle control,
3733 : we don't use forward throttle in QHOVER or QSTABILIZE as they are the primary
3734 : recovery modes for a quadplane and need to be as simple as
3735 : possible. They will drift with the wind.
3736 : */
3737 2271261 : if (plane.control_mode == &plane.mode_qacro ||
3738 2263040 : plane.control_mode == &plane.mode_qstabilize ||
3739 2253860 : plane.control_mode == &plane.mode_qhover) {
3740 :
3741 157063 : if (rc_fwd_thr_ch == nullptr) {
3742 157063 : return 0;
3743 : } else {
3744 : // calculate fwd throttle demand from manual input
3745 0 : float fwd_thr = rc_fwd_thr_ch->percent_input();
3746 :
3747 : // set forward throttle to fwd_thr_max * (manual input + mix): range [0,100]
3748 0 : fwd_thr *= 0.01f * constrain_float(fwd_thr_max, 0, 100);
3749 0 : return fwd_thr;
3750 : }
3751 : }
3752 :
3753 : /*
3754 : see if the controller should be active
3755 : */
3756 2114198 : if (get_vfwd_method() != ActiveFwdThr::OLD) {
3757 2047824 : return 0;
3758 : }
3759 :
3760 : /*
3761 : in modes with a velocity controller
3762 : */
3763 66374 : float deltat = (AP_HAL::millis() - vel_forward.last_ms) * 0.001f;
3764 66374 : if (deltat > 1 || deltat < 0) {
3765 4 : vel_forward.integrator = 0;
3766 4 : deltat = 0.1;
3767 : }
3768 66374 : if (deltat < 0.1) {
3769 : // run at 10Hz
3770 64167 : return vel_forward.last_pct;
3771 : }
3772 2207 : vel_forward.last_ms = AP_HAL::millis();
3773 :
3774 : // work out the desired speed in forward direction
3775 2207 : Vector3f desired_velocity_ned_ms = pos_control->get_vel_desired_NED_ms();
3776 :
3777 2207 : Vector3f vel_ned_ms;
3778 2207 : if (!plane.ahrs.get_velocity_NED(vel_ned_ms)) {
3779 : // we don't know our velocity? EKF must be pretty sick
3780 0 : vel_forward.last_pct = 0;
3781 0 : vel_forward.integrator = 0;
3782 0 : return 0;
3783 : }
3784 : // get component of velocity error in fwd body frame direction
3785 2207 : Vector3f vel_error_body_ms = ahrs.get_rotation_body_to_ned().transposed() * (desired_velocity_ned_ms - vel_ned_ms);
3786 :
3787 2207 : float fwd_vel_error_ms = vel_error_body_ms.x;
3788 :
3789 : // scale forward velocity error by maximum airspeed
3790 2207 : fwd_vel_error_ms /= MAX(plane.aparm.airspeed_max, 5);
3791 :
3792 : // add in a component from our current pitch demand. This tends to
3793 : // move us to zero pitch. Assume that LIM_PITCH would give us the
3794 : // WP nav speed.
3795 2207 : fwd_vel_error_ms -= wp_nav->get_default_speed_NE_ms() * plane.nav_pitch_cd / (plane.aparm.pitch_limit_max * 100);
3796 :
3797 2207 : if (should_relax() && vel_ned_ms.length() < 1) {
3798 : // we may be landed
3799 123 : fwd_vel_error_ms = 0;
3800 123 : vel_forward.integrator *= 0.95f;
3801 : }
3802 :
3803 : // integrator as throttle percentage (-100 to 100)
3804 2207 : vel_forward.integrator += fwd_vel_error_ms * deltat * vel_forward.gain * 100;
3805 :
3806 : // inhibit reverse throttle and allow petrol engines with min > 0
3807 2207 : int8_t fwd_throttle_min = plane.have_reverse_thrust() ? 0 : plane.aparm.throttle_min;
3808 2207 : vel_forward.integrator = constrain_float(vel_forward.integrator, fwd_throttle_min, plane.aparm.throttle_cruise);
3809 :
3810 2207 : if (in_vtol_land_approach()) {
3811 : // when we are doing horizontal positioning in a VTOL land
3812 : // we always allow the fwd motor to run. Otherwise a bad
3813 : // lidar could cause the aircraft not to be able to
3814 : // approach the landing point when landing below the takeoff point
3815 353 : vel_forward.last_pct = vel_forward.integrator;
3816 3556 : } else if ((in_vtol_land_final() && motors->limit.throttle_lower) ||
3817 : #if AP_RANGEFINDER_ENABLED
3818 1702 : (plane.rangefinder_use(RangeFinderUse::TAKEOFF_LANDING) &&
3819 0 : (plane.rangefinder.status_orient(plane.rangefinder_orientation()) == RangeFinder::Status::OutOfRangeLow))) {
3820 : #else
3821 : false) {
3822 : #endif
3823 : // we're in the settling phase of landing or using a rangefinder that is out of range low, disable fwd motor
3824 152 : vel_forward.last_pct = 0;
3825 152 : vel_forward.integrator = 0;
3826 : } else {
3827 : // If we are below alt_cutoff_m then scale down the effect until
3828 : // it turns off at alt_cutoff_m and decay the integrator
3829 1702 : float alt_cutoff_m = MAX(0, vel_forward_alt_cutoff_m);
3830 1702 : float height_above_ground_m = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
3831 :
3832 1702 : vel_forward.last_pct = linear_interpolate(0, vel_forward.integrator,
3833 : height_above_ground_m, alt_cutoff_m, alt_cutoff_m + 2);
3834 : }
3835 2207 : if (is_zero(vel_forward.last_pct)) {
3836 : // if the percent is 0 then decay the integrator
3837 1838 : vel_forward.integrator *= 0.95f;
3838 : }
3839 :
3840 2207 : return vel_forward.last_pct;
3841 : }
3842 :
3843 : /*
3844 : get weathervaning yaw rate in cd/s
3845 : */
3846 2041049 : float QuadPlane::get_weathervane_yaw_rate_cds(void)
3847 : {
3848 : /*
3849 : we only do weathervaning in modes where we are doing VTOL
3850 : position control.
3851 : */
3852 2041049 : if (!in_vtol_mode() ||
3853 2041049 : !transition->allow_weathervane() ||
3854 2027850 : !motors->armed() || (motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) ||
3855 2000505 : plane.control_mode == &plane.mode_qstabilize ||
3856 : #if QAUTOTUNE_ENABLED
3857 2000505 : plane.control_mode == &plane.mode_qautotune ||
3858 : #endif
3859 5954049 : plane.control_mode == &plane.mode_qhover ||
3860 1871951 : should_relax()
3861 : ) {
3862 : // Ensure the weathervane controller is reset to prevent weathervaning from happening outside of the timer
3863 367907 : weathervane->reset();
3864 367907 : return 0.0;
3865 : }
3866 :
3867 1673142 : const bool is_takeoff = in_vtol_auto() && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id);
3868 : float wv_output;
3869 3346284 : if (weathervane->get_yaw_out(wv_output,
3870 1673142 : plane.channel_rudder->get_control_in(),
3871 : plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING),
3872 1673142 : pos_control->get_roll_cd(),
3873 1673142 : pos_control->get_pitch_cd(),
3874 : is_takeoff,
3875 1673142 : in_vtol_land_sequence())) {
3876 51130 : return constrain_float(wv_output * (1/45.0), -100.0, 100.0) * command_model_pilot.get_rate() * 0.5;
3877 : }
3878 :
3879 1622012 : return 0.0;
3880 : }
3881 :
3882 : /*
3883 : start guided mode control
3884 : */
3885 96 : void QuadPlane::guided_start(void)
3886 : {
3887 96 : guided_takeoff = false;
3888 96 : setup_target_position();
3889 : int32_t from_alt;
3890 : int32_t to_alt;
3891 96 : poscontrol_init_approach();
3892 96 : if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, to_alt)) {
3893 96 : poscontrol.slow_descent = from_alt > to_alt;
3894 : } else {
3895 : // default back to old method
3896 0 : poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
3897 : }
3898 96 : }
3899 :
3900 : /*
3901 : update guided mode control
3902 : */
3903 310640 : void QuadPlane::guided_update(void)
3904 : {
3905 310640 : if (plane.control_mode == &plane.mode_guided && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) {
3906 52242 : throttle_wait = false;
3907 52242 : set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
3908 52242 : takeoff_controller();
3909 : } else {
3910 258398 : if (guided_takeoff) {
3911 5 : poscontrol.set_state(QPOS_POSITION2);
3912 : }
3913 258398 : guided_takeoff = false;
3914 : // run VTOL position controller
3915 258398 : vtol_position_controller();
3916 : }
3917 310640 : }
3918 :
3919 303 : void QuadPlane::afs_terminate(void)
3920 : {
3921 303 : if (available()) {
3922 0 : set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
3923 0 : motors->output();
3924 : }
3925 303 : }
3926 :
3927 : /*
3928 : return true if we should do guided mode loitering using VTOL motors
3929 : */
3930 78515 : bool QuadPlane::guided_mode_enabled(void)
3931 : {
3932 78515 : if (!available()) {
3933 58349 : return false;
3934 : }
3935 : // only use quadplane guided when in AUTO or GUIDED mode
3936 20166 : if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) {
3937 6359 : return false;
3938 : }
3939 14585 : if (plane.control_mode == &plane.mode_auto &&
3940 778 : plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LOITER_TURNS) {
3941 : // loiter turns is a fixed wing only operation
3942 0 : return false;
3943 : }
3944 13807 : return guided_mode != 0;
3945 : }
3946 :
3947 : /*
3948 : set altitude target to current altitude
3949 : */
3950 44 : void QuadPlane::set_alt_target_current(void)
3951 : {
3952 44 : pos_control->set_pos_desired_U_m(inertial_nav.get_position_z_up_cm() * 0.01);
3953 44 : }
3954 :
3955 : // user initiated takeoff for guided mode
3956 12 : bool QuadPlane::do_user_takeoff(float takeoff_altitude)
3957 : {
3958 12 : if (plane.control_mode != &plane.mode_guided) {
3959 0 : gcs().send_text(MAV_SEVERITY_INFO, "User Takeoff only in GUIDED mode");
3960 0 : return false;
3961 : }
3962 12 : if (!plane.arming.is_armed_and_safety_off()) {
3963 0 : gcs().send_text(MAV_SEVERITY_INFO, "Must be armed for takeoff");
3964 0 : return false;
3965 : }
3966 12 : if (is_flying()) {
3967 0 : gcs().send_text(MAV_SEVERITY_INFO, "Already flying - no takeoff");
3968 0 : return false;
3969 : }
3970 12 : plane.auto_state.vtol_loiter = true;
3971 12 : plane.prev_WP_loc = plane.current_loc;
3972 12 : plane.next_WP_loc = plane.current_loc;
3973 12 : plane.next_WP_loc.offset_up_m(takeoff_altitude);
3974 12 : set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
3975 12 : guided_start();
3976 12 : guided_takeoff = true;
3977 12 : guided_wait_takeoff = false;
3978 12 : if (!option_is_set(QuadPlane::Option::DISABLE_GROUND_EFFECT_COMP)) {
3979 12 : ahrs.set_takeoff_expected(true);
3980 : }
3981 12 : return true;
3982 : }
3983 :
3984 : // return true if the wp_nav controller is being updated
3985 121945 : bool QuadPlane::using_wp_nav(void) const
3986 : {
3987 121945 : if (plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qland) {
3988 33602 : return true;
3989 : }
3990 88343 : return false;
3991 : }
3992 :
3993 : /*
3994 : return mav_type for heartbeat
3995 : */
3996 60885 : MAV_TYPE QuadPlane::get_mav_type(void) const
3997 : {
3998 60885 : if (!available()) {
3999 : // Not enabled, must be a normal plane
4000 37544 : return MAV_TYPE_FIXED_WING;
4001 : }
4002 23341 : if (mav_type.get() != 0) {
4003 : // Override parameter set by user
4004 0 : return MAV_TYPE(mav_type.get());
4005 : }
4006 23341 : if (tiltrotor.enabled()) {
4007 : // Tiltrotor specific type
4008 2050 : return MAV_TYPE_VTOL_TILTROTOR;
4009 : }
4010 21291 : if (tailsitter.enabled()) {
4011 : // Tailsitter specific types
4012 557 : switch (motors->get_frame_mav_type()) {
4013 88 : case MAV_TYPE_VTOL_DUOROTOR:
4014 88 : return MAV_TYPE_VTOL_DUOROTOR;
4015 :
4016 469 : case MAV_TYPE_QUADROTOR:
4017 469 : return MAV_TYPE_VTOL_QUADROTOR;
4018 :
4019 0 : default:
4020 0 : break;
4021 : }
4022 : }
4023 : // Default to normal plane
4024 20734 : return MAV_TYPE_FIXED_WING;
4025 : }
4026 :
4027 : /*
4028 : return true if current mission item is a vtol takeoff
4029 : */
4030 7304823 : bool QuadPlane::is_vtol_takeoff(uint16_t id) const
4031 : {
4032 7304823 : if (id == MAV_CMD_NAV_VTOL_TAKEOFF) {
4033 324251 : return true;
4034 : }
4035 6980572 : if (id == MAV_CMD_NAV_TAKEOFF && available() && !option_is_set(QuadPlane::Option::ALLOW_FW_TAKEOFF)) {
4036 : // treat fixed wing takeoff as VTOL takeoff
4037 6415629 : return true;
4038 : }
4039 564943 : return false;
4040 : }
4041 :
4042 : /*
4043 : return true if current mission item is a vtol land
4044 : */
4045 63445650 : bool QuadPlane::is_vtol_land(uint16_t id) const
4046 : {
4047 63445650 : if (id == MAV_CMD_NAV_VTOL_LAND || id == MAV_CMD_NAV_PAYLOAD_PLACE) {
4048 42015701 : if (landing_with_fixed_wing_spiral_approach()) {
4049 3843074 : return plane.vtol_approach_s.approach_stage == Plane::VTOLApproach::Stage::VTOL_LANDING;
4050 : } else {
4051 38172619 : return true;
4052 : }
4053 : }
4054 21429949 : if (id == MAV_CMD_NAV_LAND && available() && !option_is_set(QuadPlane::Option::ALLOW_FW_LAND)) {
4055 : // treat fixed wing land as VTOL land
4056 2625752 : return true;
4057 : }
4058 18804197 : return false;
4059 : }
4060 :
4061 : /*
4062 : return true if we are in a transition to fwd flight from hover
4063 : */
4064 21122850 : bool QuadPlane::in_frwd_transition(void) const
4065 : {
4066 21122850 : return available() && transition->active_frwd();
4067 : }
4068 :
4069 : /*
4070 : calculate current stopping distance for a quadplane in fixed wing flight
4071 : */
4072 233022 : float QuadPlane::stopping_distance_m(float ground_speed_squared_m) const
4073 : {
4074 : // use v^2/(2*accel). This is only quite approximate as the drag
4075 : // varies with pitch, but it gives something for the user to
4076 : // control the transition distance in a reasonable way
4077 233022 : return ground_speed_squared_m / (2 * transition_decel_mss);
4078 : }
4079 :
4080 : /*
4081 : calculate acceleration needed to stop in the given distance given current speed
4082 : */
4083 217209 : float QuadPlane::accel_needed(float stop_distance, float ground_speed_squared) const
4084 : {
4085 217209 : return ground_speed_squared / (2 * MAX(1,stop_distance));
4086 : }
4087 :
4088 : /*
4089 : calculate current stopping distance for a quadplane in fixed wing flight
4090 : */
4091 157944 : float QuadPlane::stopping_distance_m(void)
4092 : {
4093 157944 : return stopping_distance_m(plane.ahrs.groundspeed_vector().length_squared());
4094 : }
4095 :
4096 : /*
4097 : distance below which we don't do approach, based on stopping
4098 : distance for cruise speed
4099 : */
4100 146 : float QuadPlane::transition_threshold_m(void)
4101 : {
4102 : // 1.5 times stopping distance for cruise speed
4103 146 : return 1.5 * stopping_distance_m(sq(plane.aparm.airspeed_cruise));
4104 : }
4105 :
4106 : #define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing
4107 : #define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing
4108 : #define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity
4109 :
4110 527359 : void QuadPlane::update_throttle_mix(void)
4111 : {
4112 : // update filtered acceleration
4113 527359 : Vector3f accel_ef_mss = ahrs.get_accel_ef();
4114 527359 : accel_ef_mss.z += GRAVITY_MSS;
4115 527359 : throttle_mix_accel_ef_filter.apply(accel_ef_mss, plane.scheduler.get_loop_period_s());
4116 :
4117 : // transition will directly manage the mix
4118 527359 : if (!transition->allow_update_throttle_mix()) {
4119 144500 : return;
4120 : }
4121 :
4122 : // if disarmed or landed prioritise throttle
4123 457792 : if (!motors->armed()) {
4124 74933 : attitude_control->set_throttle_mix_min();
4125 74933 : return;
4126 : }
4127 :
4128 382859 : if (plane.control_mode->is_vtol_man_throttle()) {
4129 : // manual throttle
4130 2960 : if (!is_positive(get_throttle_input()) && !air_mode_active()) {
4131 319 : attitude_control->set_throttle_mix_min();
4132 : } else {
4133 2641 : attitude_control->set_throttle_mix_man();
4134 : }
4135 : } else {
4136 : // autopilot controlled throttle
4137 :
4138 : // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
4139 379899 : const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
4140 379899 : bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD;
4141 :
4142 : // check for large external disturbance - angle error over 30 degrees
4143 379899 : const float angle_error = attitude_control->get_att_error_angle_deg();
4144 379899 : bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
4145 :
4146 : // check for large acceleration - falling or high turbulence
4147 379899 : bool accel_moving = (throttle_mix_accel_ef_filter.get().length() > LAND_CHECK_ACCEL_MOVING);
4148 :
4149 : // check for requested descent
4150 379899 : bool descent_not_demanded = pos_control->get_vel_desired_U_ms() >= 0.0f;
4151 :
4152 379899 : bool use_mix_max = large_angle_request || large_angle_error || accel_moving || descent_not_demanded;
4153 :
4154 : /*
4155 : special case for auto landing, we want a high degree of
4156 : attitude control until LAND_FINAL
4157 : */
4158 379899 : if (in_vtol_land_sequence()) {
4159 : // during the descent phase we don't want to priortise
4160 : // attitude too much. Rapid descent on quadplanes often
4161 : // leads to very poor attitude control and putting more
4162 : // power into attitude doesn't tend to fix it, what we
4163 : // need to do is slow the descent.
4164 137172 : use_mix_max = !in_vtol_land_descent() || poscontrol.get_state() == QPOS_LAND_ABORT;
4165 : }
4166 :
4167 379899 : if (use_mix_max) {
4168 209846 : attitude_control->set_throttle_mix_max(1.0);
4169 : } else {
4170 170053 : attitude_control->set_throttle_mix_min();
4171 : }
4172 : }
4173 : }
4174 :
4175 : /*
4176 : see if we are in the approach phase of a VTOL landing
4177 : */
4178 68617234 : bool QuadPlane::in_vtol_land_approach(void) const
4179 : {
4180 68876897 : if (plane.control_mode == &plane.mode_qrtl &&
4181 259663 : poscontrol.get_state() <= QPOS_POSITION2) {
4182 90607 : return true;
4183 : }
4184 68526627 : if (in_vtol_auto()) {
4185 36761773 : if (is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
4186 21200885 : (poscontrol.get_state() == QPOS_APPROACH ||
4187 20658652 : poscontrol.get_state() == QPOS_AIRBRAKE ||
4188 18899779 : poscontrol.get_state() == QPOS_POSITION1 ||
4189 8701775 : poscontrol.get_state() == QPOS_POSITION2)) {
4190 3095225 : return true;
4191 : }
4192 : }
4193 65431402 : return false;
4194 : }
4195 :
4196 : /*
4197 : see if we are in the descent phase of a VTOL landing
4198 : */
4199 127141845 : bool QuadPlane::in_vtol_land_descent(void) const
4200 : {
4201 127141845 : const auto state = poscontrol.get_state();
4202 127141853 : if (plane.control_mode == &plane.mode_qrtl &&
4203 304484 : (state == QPOS_LAND_DESCEND || state == QPOS_LAND_FINAL || state == QPOS_LAND_ABORT)) {
4204 390762 : return true;
4205 : }
4206 130366390 : if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
4207 3615299 : (state == QPOS_LAND_DESCEND || state == QPOS_LAND_FINAL || state == QPOS_LAND_ABORT)) {
4208 8047766 : return true;
4209 : }
4210 118703318 : return false;
4211 : }
4212 :
4213 : /*
4214 : see if we are in the final phase of a VTOL landing
4215 : */
4216 57040244 : bool QuadPlane::in_vtol_land_final(void) const
4217 : {
4218 57040244 : return in_vtol_land_descent() && poscontrol.get_state() == QPOS_LAND_FINAL;
4219 : }
4220 :
4221 : /*
4222 : see if we are in any of the phases of a VTOL landing
4223 : */
4224 71908684 : bool QuadPlane::in_vtol_land_sequence(void) const
4225 : {
4226 71908684 : return plane.control_mode == &plane.mode_qrtl || in_vtol_land_approach() || in_vtol_land_descent() || in_vtol_land_final();
4227 : }
4228 :
4229 : /*
4230 : see if we are in the VTOL position control phase of a landing
4231 : */
4232 12422497 : bool QuadPlane::in_vtol_land_poscontrol(void) const
4233 : {
4234 13694303 : if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
4235 1271806 : poscontrol.get_state() >= QPOS_POSITION1) {
4236 1178010 : return true;
4237 : }
4238 11244487 : return false;
4239 : }
4240 :
4241 : /*
4242 : see if we are in the airbrake phase of a VTOL landing
4243 : */
4244 11456632 : bool QuadPlane::in_vtol_airbrake(void) const
4245 : {
4246 11955914 : if (plane.control_mode == &plane.mode_qrtl &&
4247 499282 : poscontrol.get_state() == QPOS_AIRBRAKE) {
4248 25302 : return true;
4249 : }
4250 4672393 : if (plane.control_mode == &plane.mode_auto &&
4251 16103723 : is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
4252 763087 : poscontrol.get_state() == QPOS_AIRBRAKE) {
4253 124898 : return true;
4254 : }
4255 11306432 : return false;
4256 : }
4257 :
4258 : // return true if we should show VTOL view
4259 6011411 : bool QuadPlane::show_vtol_view() const
4260 : {
4261 6011411 : return available() && transition->show_vtol_view() && !force_fw_control_recovery;
4262 : }
4263 :
4264 : // return true if we should show VTOL view
4265 3313786 : bool SLT_Transition::show_vtol_view() const
4266 : {
4267 :
4268 3313786 : return quadplane.in_vtol_mode();
4269 : }
4270 :
4271 : /*
4272 : return the PILOT_SPD_DN value if non zero, otherwise returns the PILOT_SPD_UP value.
4273 : return is in cm/s
4274 : */
4275 4375702 : uint16_t QuadPlane::get_pilot_velocity_z_max_dn_m() const
4276 : {
4277 4375702 : if (is_zero(pilot_speed_z_max_dn_ms)) {
4278 4375702 : return abs(pilot_speed_z_max_up_ms);
4279 : }
4280 0 : return abs(pilot_speed_z_max_dn_ms);
4281 : }
4282 :
4283 : /*
4284 : should we use the fixed wing attitude controllers for roll/pitch control
4285 : */
4286 15082558 : bool QuadPlane::use_fw_attitude_controllers(void) const
4287 : {
4288 27534948 : if (available() &&
4289 22344318 : motors->armed() &&
4290 9891928 : motors->get_desired_spool_state() >= AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED &&
4291 10463362 : !tailsitter.enabled() &&
4292 32649188 : poscontrol.get_state() != QPOS_AIRBRAKE &&
4293 5064200 : !force_fw_control_recovery) {
4294 :
4295 5001152 : if (in_vtol_mode()) {
4296 : // in VTOL modes always slave fixed wing to VTOL rate control
4297 4354554 : return false;
4298 : }
4299 :
4300 646598 : if (transition->use_multirotor_control_in_fwd_transition()) {
4301 : /*
4302 : special case for vectored yaw tiltrotors in forward
4303 : transition, keep multicopter control until we reach
4304 : target transition airspeed. This can result in loss of
4305 : yaw control on some tilt-vectored airframes without
4306 : strong VTOL yaw control
4307 : */
4308 35962 : return false;
4309 : }
4310 : }
4311 :
4312 10692042 : if (force_fw_control_recovery) {
4313 : // tell the roll controller not to apply roll rate limits
4314 270914 : plane.rollController.set_in_recovery();
4315 : }
4316 :
4317 10692042 : return true;
4318 : }
4319 :
4320 : /*
4321 : calculate our closing velocity vector on the landing point, taking
4322 : into account target velocity
4323 : */
4324 374283 : Vector2f QuadPlane::landing_closing_velocity_NE_ms()
4325 : {
4326 374283 : Vector2f landing_velocity_ne_m;
4327 374283 : if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) {
4328 4670 : landing_velocity_ne_m = poscontrol.velocity_match_ms;
4329 : }
4330 374283 : return ahrs.groundspeed_vector() - landing_velocity_ne_m;
4331 : }
4332 :
4333 : /*
4334 : calculate our desired closing velocity vector on the landing point.
4335 : */
4336 164172 : Vector2f QuadPlane::landing_desired_closing_velocity_NE_ms()
4337 : {
4338 164172 : if (poscontrol.get_state() >= QPOS_LAND_DESCEND) {
4339 0 : return Vector2f(0,0);
4340 : }
4341 164172 : const Vector2f diff_wp_NE_m = plane.current_loc.get_distance_NE(plane.next_WP_loc);
4342 164172 : float dist_m = diff_wp_NE_m.length();
4343 164172 : if (dist_m < 1) {
4344 915 : return Vector2f(0,0);
4345 : }
4346 :
4347 : // base target speed based on sqrt of distance
4348 163257 : float target_speed_ms = safe_sqrt(2 * transition_decel_mss * dist_m);
4349 :
4350 : // don't let the target speed go above landing approach speed
4351 163257 : const float eas2tas = plane.ahrs.get_EAS2TAS();
4352 163257 : float land_speed_ms = plane.aparm.airspeed_cruise;
4353 163257 : float tecs_land_airspeed_ms = plane.TECS_controller.get_land_airspeed();
4354 163257 : if (is_positive(tecs_land_airspeed_ms)) {
4355 0 : land_speed_ms = tecs_land_airspeed_ms;
4356 : } else {
4357 : // use half way between min airspeed and cruise if
4358 : // TECS_LAND_AIRSPEED not set
4359 163257 : land_speed_ms = 0.5 * (land_speed_ms + plane.aparm.airspeed_min);
4360 : }
4361 163257 : target_speed_ms = MIN(target_speed_ms, eas2tas * land_speed_ms);
4362 :
4363 163257 : Vector2f target_speed_xy = diff_wp_NE_m.normalized() * target_speed_ms;
4364 :
4365 163257 : return target_speed_xy;
4366 : }
4367 :
4368 : /*
4369 : get target airspeed for landing, for use by TECS
4370 : */
4371 12306 : float QuadPlane::get_land_airspeed_ms(void)
4372 : {
4373 12306 : const auto qstate = poscontrol.get_state();
4374 12306 : if (qstate == QPOS_APPROACH ||
4375 7921 : plane.control_mode == &plane.mode_rtl) {
4376 4385 : const float cruise_speed_ms = plane.aparm.airspeed_cruise;
4377 : // assume cruise speed, but try to do better:
4378 4385 : float approach_speed_ms = cruise_speed_ms;
4379 4385 : float tecs_land_airspeed_ms = plane.TECS_controller.get_land_airspeed();
4380 4385 : if (is_positive(tecs_land_airspeed_ms)) {
4381 0 : approach_speed_ms = tecs_land_airspeed_ms;
4382 4385 : } else if (qstate == QPOS_APPROACH) {
4383 : // default to half way between min airspeed and cruise
4384 : // airspeed when on the approach
4385 4385 : approach_speed_ms = 0.5*(cruise_speed_ms + plane.aparm.airspeed_min);
4386 : }
4387 4385 : const float time_to_pos1 = (plane.auto_state.wp_distance - stopping_distance_m(sq(approach_speed_ms))) / MAX(approach_speed_ms, 5);
4388 : /*
4389 : slow down to landing approach speed as we get closer to landing
4390 : */
4391 4385 : approach_speed_ms = linear_interpolate(approach_speed_ms, cruise_speed_ms,
4392 : time_to_pos1,
4393 : 20, 60);
4394 4385 : return approach_speed_ms;
4395 : }
4396 :
4397 7921 : if (qstate == QPOS_AIRBRAKE) {
4398 : // during airbraking ask TECS to slow us to stall speed
4399 823 : return plane.aparm.airspeed_min;
4400 : }
4401 :
4402 : // calculate speed based on landing desired velocity
4403 7098 : Vector2f vel_ne_ms = landing_desired_closing_velocity_NE_ms();
4404 7098 : Vector3f wind_ned_ms;
4405 : // use the estimate even if it is not marked valid, to preserve
4406 : // existing behaviour
4407 7098 : IGNORE_RETURN(plane.ahrs.get_wind(wind_ned_ms));
4408 7098 : const float eas2tas = plane.ahrs.get_EAS2TAS();
4409 7098 : vel_ne_ms -= wind_ned_ms.xy();
4410 7098 : vel_ne_ms /= eas2tas;
4411 7098 : return vel_ne_ms.length();
4412 : }
4413 :
4414 6897704 : void QuadPlane::set_desired_spool_state(AP_Motors::DesiredSpoolState state)
4415 : {
4416 6897704 : if (motors->get_desired_spool_state() != state) {
4417 463098 : if (state == AP_Motors::DesiredSpoolState::SHUT_DOWN) {
4418 : // also request zero throttle, so we avoid the slow ramp down
4419 280 : motors->set_roll(0);
4420 280 : motors->set_pitch(0);
4421 280 : motors->set_yaw(0);
4422 280 : motors->set_throttle(0);
4423 : }
4424 463098 : motors->set_desired_spool_state(state);
4425 : }
4426 6897704 : }
4427 :
4428 77386 : bool QuadPlane::air_mode_active() const
4429 : {
4430 77386 : if ((air_mode == AirMode::ON) || ((air_mode == AirMode::ASSISTED_FLIGHT_ONLY) && assisted_flight)) {
4431 58482 : return true;
4432 : }
4433 18904 : return false;
4434 : }
4435 :
4436 : /*
4437 : return scaling factor for tilting rotors in forward flight throttle
4438 : we want to scale back tilt angle for roll/pitch by throttle in forward flight
4439 : */
4440 122243 : float QuadPlane::FW_vector_throttle_scaling()
4441 : {
4442 122243 : const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01;
4443 : // scale relative to a fixed 0.5 mid throttle so that changes in TRIM_THROTTLE in missions don't change
4444 : // the scaling of tilt
4445 122243 : const float mid_throttle = 0.5;
4446 122243 : return mid_throttle / constrain_float(throttle, 0.1, 1.0);
4447 : }
4448 :
4449 : QuadPlane *QuadPlane::_singleton = nullptr;
4450 :
4451 3430086 : bool SLT_Transition::set_FW_roll_limit(int32_t& roll_limit_cd)
4452 : {
4453 3846588 : if (quadplane.assisted_flight && (transition_state == State::AIRSPEED_WAIT || transition_state == State::TIMER) &&
4454 416502 : quadplane.option_is_set(QuadPlane::Option::LEVEL_TRANSITION)) {
4455 : // the user wants transitions to be kept level to within LEVEL_ROLL_LIMIT
4456 0 : roll_limit_cd = MIN(roll_limit_cd, plane.g.level_roll_limit*100);
4457 0 : return true;
4458 : }
4459 3430086 : return false;
4460 : }
4461 :
4462 510705 : bool SLT_Transition::allow_update_throttle_mix() const
4463 : {
4464 : // transition is directly managing throttle mix in these cases
4465 510705 : return !(quadplane.assisted_flight && (transition_state == State::AIRSPEED_WAIT || transition_state == State::TIMER));
4466 : }
4467 :
4468 15919047 : bool SLT_Transition::active_frwd() const
4469 : {
4470 : // We need to be in assisted flight...
4471 15919047 : if (!quadplane.assisted_flight) {
4472 14684631 : return false;
4473 : }
4474 : // ... and a transition must be active...
4475 1234416 : if (!((transition_state == State::AIRSPEED_WAIT) || (transition_state == State::TIMER))) {
4476 337 : return false;
4477 : }
4478 : // ... but not executing a QPOS_AIRBRAKE maneuver during an automated landing.
4479 1234079 : if (quadplane.in_vtol_airbrake()) {
4480 75080 : return false;
4481 : }
4482 :
4483 1158999 : return true;
4484 : }
4485 :
4486 : /*
4487 : limit VTOL roll/pitch in POSITION1, POSITION2 and waypoint controller. This serves three roles:
4488 : 1) an expanding envelope limit on pitch to prevent sudden pitch at the start of a back transition
4489 :
4490 : 2) limiting roll and pitch down to the Q_A_ANGLE_MAX, as the accel limits may push us beyond that for pitch up.
4491 : This is needed as the position controller doesn't have separate limits for pitch and roll
4492 :
4493 : 3) preventing us pitching up a lot when our airspeed may be low
4494 : enough that the real airspeed may be negative, which would result
4495 : in reversed control surfaces
4496 : */
4497 1409001 : bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_cd)
4498 : {
4499 1409001 : bool ret = false;
4500 1409001 : const float angle_max_cd = quadplane.attitude_control->lean_angle_max_cd();
4501 :
4502 : /*
4503 : we always limit roll to Q_A_ANGLE_MAX
4504 : */
4505 1409001 : int32_t new_roll_cd = constrain_int32(roll_cd, -angle_max_cd, angle_max_cd);
4506 1409001 : if (new_roll_cd != roll_cd) {
4507 0 : roll_cd = new_roll_cd;
4508 0 : ret = true;
4509 : }
4510 :
4511 : /*
4512 : always limit pitch down to Q_A_ANGLE_MAX. We need to do this as
4513 : the position controller accel limits may exceed this limit
4514 : */
4515 1409001 : if (pitch_cd < -angle_max_cd) {
4516 0 : pitch_cd = -angle_max_cd;
4517 0 : ret = true;
4518 : }
4519 :
4520 : /*
4521 : prevent trying to fly backwards (negative airspeed) at high
4522 : pitch angles, which can result in a high degree of instability
4523 : in SLT aircraft. This can happen with a tailwind in a back
4524 : transition, where the position controller (which is unaware of
4525 : airspeed) demands high pitch to hit the desired landing point
4526 : */
4527 : float airspeed;
4528 568 : if (pitch_cd > angle_max_cd &&
4529 1409569 : plane.ahrs.airspeed_EAS(airspeed) && airspeed < 0.5 * plane.aparm.airspeed_min) {
4530 0 : const float max_limit_cd = linear_interpolate(angle_max_cd, 4500,
4531 : airspeed,
4532 0 : 0, 0.5 * plane.aparm.airspeed_min);
4533 0 : if (pitch_cd > max_limit_cd) {
4534 0 : pitch_cd = max_limit_cd;
4535 0 : ret = true;
4536 : }
4537 : }
4538 :
4539 1409001 : if (quadplane.back_trans_pitch_limit_ms <= 0) {
4540 : // time based pitch envelope disabled
4541 0 : return ret;
4542 : }
4543 :
4544 1409001 : const uint32_t limit_time_ms = quadplane.back_trans_pitch_limit_ms;
4545 :
4546 1409001 : const uint32_t dt = AP_HAL::millis() - last_fw_mode_ms;
4547 1409001 : if (last_fw_mode_ms == 0 || dt > limit_time_ms) {
4548 : // we are beyond the time limit, don't apply envelope
4549 1306275 : last_fw_mode_ms = 0;
4550 1306275 : return ret;
4551 : }
4552 :
4553 : // we limit pitch during initial transition
4554 102726 : const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(angle_max_cd,plane.aparm.pitch_limit_max*100),
4555 : dt,
4556 : 0, limit_time_ms);
4557 :
4558 102726 : if (pitch_cd > max_limit_cd) {
4559 35475 : pitch_cd = max_limit_cd;
4560 35475 : return true;
4561 : }
4562 :
4563 : /*
4564 : limit the pitch down with an expanding envelope. This
4565 : prevents the velocity controller demanding nose down during
4566 : the initial slowdown if the target velocity curve is higher
4567 : than the actual velocity curve (for a high drag
4568 : aircraft). Nose down will cause a lot of downforce on the
4569 : wings which will draw a lot of current and also cause the
4570 : aircraft to lose altitude rapidly.pitch limit varies also with speed
4571 : to prevent inability to progress to position if moving from a loiter
4572 : to landing
4573 : */
4574 67251 : const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-angle_max_cd,plane.aparm.pitch_limit_min*100),
4575 : dt,
4576 : 0, limit_time_ms);
4577 :
4578 67251 : if (plane.nav_pitch_cd < min_limit_cd) {
4579 1044 : plane.nav_pitch_cd = min_limit_cd;
4580 1044 : return true;
4581 : }
4582 :
4583 66207 : return ret;
4584 : }
4585 :
4586 : /*
4587 : remember last fixed wing pitch for pitch envelope in back transition
4588 : */
4589 3668748 : void SLT_Transition::set_last_fw_pitch()
4590 : {
4591 3668748 : last_fw_mode_ms = AP_HAL::millis();
4592 3668748 : last_fw_nav_pitch_cd = plane.nav_pitch_cd;
4593 3668748 : }
4594 :
4595 260392 : void SLT_Transition::force_transition_complete()
4596 : {
4597 260392 : transition_state = State::DONE;
4598 260392 : in_forced_transition = false;
4599 260392 : transition_start_ms = 0;
4600 260392 : transition_low_airspeed_ms = 0;
4601 260392 : set_last_fw_pitch();
4602 :
4603 : // Keep assistance reset while not checking
4604 260392 : quadplane.assist.reset();
4605 260392 : }
4606 :
4607 3428 : MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const
4608 : {
4609 3428 : if (quadplane.in_vtol_land_approach()) {
4610 1052 : QuadPlane::position_control_state state = quadplane.poscontrol.get_state();
4611 1052 : if ((state == QuadPlane::position_control_state::QPOS_AIRBRAKE) || (state == QuadPlane::position_control_state::QPOS_POSITION1)) {
4612 384 : return MAV_VTOL_STATE_TRANSITION_TO_MC;
4613 : }
4614 : }
4615 3044 : if (quadplane.in_vtol_mode()) {
4616 1814 : return MAV_VTOL_STATE_MC;
4617 : }
4618 :
4619 1230 : switch (transition_state) {
4620 188 : case State::AIRSPEED_WAIT:
4621 : case State::TIMER:
4622 : // we enter this state during assisted flight, not just
4623 : // during a forward transition.
4624 188 : return MAV_VTOL_STATE_TRANSITION_TO_FW;
4625 :
4626 1042 : case State::DONE:
4627 1042 : return MAV_VTOL_STATE_FW;
4628 : }
4629 :
4630 0 : return MAV_VTOL_STATE_UNDEFINED;
4631 : }
4632 :
4633 : // Set FW roll and pitch limits and keep TECS informed
4634 6346294 : void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd)
4635 : {
4636 6346294 : if (quadplane.in_vtol_mode() || quadplane.in_vtol_airbrake()) {
4637 : // not in FW flight
4638 2671876 : return;
4639 : }
4640 :
4641 3674418 : if (transition_state == State::DONE) {
4642 : // transition complete, nothing to do
4643 3281995 : return;
4644 : }
4645 :
4646 392423 : if (!plane.control_mode->does_auto_throttle()) {
4647 : // don't limit pitch when in manually controlled modes like FBWA, ACRO
4648 71175 : return;
4649 : }
4650 :
4651 : float max_pitch;
4652 321248 : if (transition_state < State::TIMER) {
4653 157392 : if (plane.ahrs.groundspeed() < 3.0) {
4654 : // until we have some ground speed limit to zero pitch
4655 36239 : max_pitch = 0.0;
4656 : } else {
4657 121153 : max_pitch = quadplane.transition_pitch_max;
4658 : }
4659 : } else {
4660 163856 : max_pitch = (quadplane.transition_pitch_max+1.0)*2.0;
4661 : }
4662 :
4663 : // set a single loop pitch limit in TECS
4664 321248 : plane.TECS_controller.set_pitch_max(max_pitch);
4665 321248 : plane.TECS_controller.set_pitch_min(-max_pitch);
4666 :
4667 : // ensure pitch is constrained to limit
4668 321248 : nav_pitch_cd = constrain_int32(nav_pitch_cd, -max_pitch*100.0, max_pitch*100.0);
4669 : }
4670 :
4671 : /*
4672 : see if we are in a VTOL takeoff
4673 : */
4674 7686 : bool QuadPlane::in_vtol_takeoff(void) const
4675 : {
4676 7686 : if (in_vtol_auto() && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
4677 949 : return true;
4678 : }
4679 6737 : return false;
4680 : }
4681 :
4682 : // called when we change mode (for any mode, not just Q modes)
4683 1930 : void QuadPlane::mode_enter(void)
4684 : {
4685 1930 : if (available()) {
4686 929 : pos_control->set_lean_angle_max_cd(0);
4687 : }
4688 1930 : poscontrol.correction_ne_m.zero();
4689 1930 : poscontrol.velocity_match_ms.zero();
4690 1930 : poscontrol.last_velocity_match_ms = 0;
4691 1930 : poscontrol.set_state(QuadPlane::QPOS_NONE);
4692 :
4693 : // Clear any pilot corrections
4694 1930 : poscontrol.pilot_correction_done = false;
4695 1930 : poscontrol.pilot_correction_active = false;
4696 1930 : poscontrol.target_vel_ms.zero();
4697 :
4698 : // clear guided takeoff wait on any mode change, but remember the
4699 : // state for special behaviour
4700 1930 : guided_wait_takeoff_on_mode_enter = guided_wait_takeoff;
4701 1930 : guided_wait_takeoff = false;
4702 :
4703 1930 : q_fwd_throttle = 0.0f;
4704 1930 : q_fwd_pitch_lim_cd = 100.0f * q_fwd_pitch_lim;
4705 :
4706 1930 : force_fw_control_recovery = false;
4707 1930 : in_spin_recovery = false;
4708 1930 : }
4709 :
4710 : // Set attitude control yaw rate time constant to pilot input command model value
4711 1935426 : void QuadPlane::set_pilot_yaw_rate_time_constant()
4712 : {
4713 1935426 : attitude_control->set_yaw_rate_tc(command_model_pilot.get_rate_tc());
4714 1935426 : }
4715 :
4716 : // Disable attitude control yaw rate time constant
4717 659925 : void QuadPlane::disable_yaw_rate_time_constant()
4718 : {
4719 659925 : attitude_control->set_yaw_rate_tc(0.0);
4720 659925 : }
4721 :
4722 : // Check if servo auto trim is allowed, only if countrol surfaces are fully in use
4723 0 : bool QuadPlane::allow_servo_auto_trim()
4724 : {
4725 0 : if (!available()) {
4726 : // Quadplane disabled, auto trim always allowed
4727 0 : return true;
4728 : }
4729 0 : if (in_vtol_mode()) {
4730 : // VTOL motors active in VTOL modes
4731 0 : return false;
4732 : }
4733 0 : if (!in_assisted_flight()) {
4734 : // In forward flight and VTOL motors not active
4735 0 : return true;
4736 : }
4737 0 : if (tailsitter.enabled() && option_is_set(QuadPlane::Option::TAILSIT_Q_ASSIST_MOTORS_ONLY)) {
4738 : // Tailsitter in forward flight, motors providing active stabalisation with motors only option
4739 : // Control surfaces are running as normal with I term active, motor I term is zeroed
4740 0 : return true;
4741 : }
4742 : // In forward flight with active VTOL motors
4743 0 : return false;
4744 : }
4745 :
4746 43322124 : bool QuadPlane::landing_with_fixed_wing_spiral_approach(void) const
4747 : {
4748 43322124 : const AP_Mission::Mission_Command cmd = plane.mission.get_current_nav_cmd();
4749 :
4750 43322123 : if (cmd.id == MAV_CMD_NAV_PAYLOAD_PLACE &&
4751 0 : option_is_set(QuadPlane::Option::MISSION_LAND_FW_APPROACH)) {
4752 0 : return true;
4753 : }
4754 :
4755 128916406 : return ((cmd.id == MAV_CMD_NAV_VTOL_LAND) &&
4756 42797147 : (option_is_set(QuadPlane::Option::MISSION_LAND_FW_APPROACH) ||
4757 86119259 : cmd.p1 == NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH));
4758 : }
4759 :
4760 : /*
4761 : setup scaling of roll and pitch angle P gains to match fixed wing gains
4762 :
4763 : we setup the angle P gain to match fixed wing at high speed (above
4764 : AIRSPEED_MIN) where fixed wing surfaces are presumed to
4765 : dominate. At lower speeds we use the multicopter angle P gains.
4766 : */
4767 217209 : void QuadPlane::setup_rp_fw_angle_gains(void)
4768 : {
4769 217209 : const float mc_angR = attitude_control->get_angle_roll_p().kP();
4770 217209 : const float mc_angP = attitude_control->get_angle_pitch_p().kP();
4771 217209 : const float fw_angR = plane.rollController.get_angle_p();
4772 217209 : const float fw_angP = plane.pitchController.get_angle_p();
4773 :
4774 217209 : if (!is_positive(mc_angR) || !is_positive(mc_angP)) {
4775 : // bad configuration, don't scale
4776 15764 : return;
4777 : }
4778 :
4779 : float aspeed;
4780 217209 : if (!ahrs.airspeed_EAS(aspeed)) {
4781 : // can't get airspeed, no scaling of VTOL angle gains
4782 0 : return;
4783 : }
4784 :
4785 217209 : const float low_airspeed = 3.0;
4786 217209 : if (aspeed <= low_airspeed || plane.aparm.airspeed_min <= low_airspeed) {
4787 : // no scaling
4788 15764 : return;
4789 : }
4790 :
4791 201445 : const float angR_scale = linear_interpolate(mc_angR, fw_angR,
4792 : aspeed,
4793 201445 : low_airspeed, plane.aparm.airspeed_min) / mc_angR;
4794 201445 : const float angP_scale = linear_interpolate(mc_angP, fw_angP,
4795 : aspeed,
4796 201445 : low_airspeed, plane.aparm.airspeed_min) / mc_angP;
4797 201445 : const Vector3f gain_scale{angR_scale, angP_scale, 1.0};
4798 201445 : attitude_control->set_angle_P_scale(gain_scale);
4799 : }
4800 :
4801 : /*
4802 : abort landing, used by scripting for payload place and ship landing abort
4803 : will return false if not in a landing descent
4804 : */
4805 0 : bool QuadPlane::abort_landing(void)
4806 : {
4807 0 : if (poscontrol.get_state() == QPOS_LAND_ABORT ||
4808 0 : !(plane.control_mode == &plane.mode_auto)) {
4809 : // already aborted or not in AUTO?
4810 0 : return false;
4811 : }
4812 :
4813 : // special case for payload place with full landing
4814 : const bool payload_place_landed =
4815 0 : plane.in_auto_mission_id(MAV_CMD_NAV_PAYLOAD_PLACE) &&
4816 0 : poscontrol.get_state() == QPOS_LAND_COMPLETE;
4817 :
4818 0 : if (!payload_place_landed && !in_vtol_land_descent()) {
4819 0 : return false;
4820 : }
4821 0 : poscontrol.set_state(QuadPlane::QPOS_LAND_ABORT);
4822 0 : return true;
4823 : }
4824 :
4825 : // Should we allow stick mixing from the pilot
4826 4132815 : bool QuadPlane::allow_stick_mixing() const
4827 : {
4828 4132815 : if (!available()) {
4829 : // Quadplane not enabled
4830 1167735 : return true;
4831 : }
4832 : // Ask transition logic
4833 2965080 : return transition->allow_stick_mixing();
4834 : }
4835 :
4836 : /*
4837 : return true if we should disable TECS in the current flight state
4838 : this ensures that TECS resets when we change height in a VTOL mode
4839 : */
4840 3282535 : bool QuadPlane::should_disable_TECS() const
4841 : {
4842 3282535 : if (in_vtol_land_descent()) {
4843 117695 : return true;
4844 : }
4845 3164840 : if (plane.control_mode == &plane.mode_guided &&
4846 243992 : plane.auto_state.vtol_loiter) {
4847 62132 : return true;
4848 : }
4849 3102708 : return false;
4850 : }
4851 :
4852 : // Get pilot throttle input with deadzone, this will return 50% throttle in failsafe!
4853 : // This is a re-implmentation of Plane::get_throttle_input
4854 : // Ignoring the no_deadzone case means we don't need to check for valid RC
4855 : // This is handled by Plane::control_failsafe setting of control in
4856 2920456 : float QuadPlane::get_throttle_input() const
4857 : {
4858 2920456 : float ret = plane.channel_throttle->get_control_in();
4859 2920456 : if (plane.reversed_throttle) {
4860 : // RC option for reverse throttle has been set
4861 0 : ret = -ret;
4862 : }
4863 2920456 : return ret;
4864 : }
4865 :
4866 : // return true if forward throttle from forward_throttle_pct() should be used
4867 5483490 : bool QuadPlane::allow_forward_throttle_in_vtol_mode() const
4868 : {
4869 5483490 : return in_vtol_mode() && motors->armed() && (motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN);
4870 : }
4871 :
4872 0 : void QuadPlane::Log_Write_AttRate()
4873 : {
4874 0 : attitude_control->Write_ANG();
4875 0 : attitude_control->Write_Rate(*pos_control);
4876 :
4877 0 : }
4878 :
4879 : // check for loss of thrust and trigger thrust boost in motors library
4880 2737526 : void QuadPlane::thrust_loss_check(bool reset)
4881 : {
4882 : // Clear counter if reset
4883 2737526 : if (reset) {
4884 439255 : thrust_loss.counter = 0;
4885 2726181 : return;
4886 : }
4887 :
4888 : // Return if disabled, either completly or in the current flight mode
4889 4596542 : if (thrust_loss.option_is_set(ThrustLoss::Option::DISABLED) ||
4890 2298271 : (thrust_loss.option_is_set(ThrustLoss::Option::VTOL_ONLY) && !in_vtol_mode())) {
4891 0 : thrust_loss.counter = 0;
4892 0 : return;
4893 : }
4894 :
4895 : // return if already engaged, disarmed, not flying, or motors not active
4896 2298271 : if (motors->get_thrust_boost() || !motors->armed() || !plane.is_flying() || (motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED)) {
4897 17514 : thrust_loss.counter = 0;
4898 17514 : return;
4899 : }
4900 :
4901 : // check for desired angle under 15 degrees
4902 2280757 : const Vector3f& angle_target_rad = attitude_control->get_att_target_euler_rad();
4903 2280757 : if (angle_target_rad.xy().length_squared() > sq(radians(15.0))) {
4904 184071 : thrust_loss.counter = 0;
4905 184071 : return;
4906 : }
4907 :
4908 : // check for throttle over 90% or throttle saturation
4909 2096686 : if ((attitude_control->get_throttle_in() < 0.9) && (!motors->limit.throttle_upper)) {
4910 2047327 : thrust_loss.counter = 0;
4911 2047327 : return;
4912 : }
4913 :
4914 : // check throttle is over 25% to prevent checks triggering from thrust limitations caused by low commanded throttle
4915 49359 : if ((attitude_control->get_throttle_in() < 0.25f)) {
4916 745 : thrust_loss.counter = 0;
4917 745 : return;
4918 : }
4919 :
4920 : // check for descent
4921 48614 : Vector3f vel_NED;
4922 48614 : if (!ahrs.get_velocity_NED(vel_NED) || !is_positive(vel_NED.z)) {
4923 : // we have no vertical velocity estimate and/or we are not descending
4924 37269 : thrust_loss.counter = 0;
4925 37269 : return;
4926 : }
4927 :
4928 : // check for angle error over 30 degrees to ensure the aircraft has attitude control
4929 11345 : const float angle_error = attitude_control->get_att_error_angle_deg();
4930 11345 : if (angle_error >= 30.0) {
4931 0 : thrust_loss.counter = 0;
4932 0 : return;
4933 : }
4934 :
4935 : // the aircraft is descending with low requested roll and pitch, at full available throttle, with attitude control
4936 : // we may have lost thrust
4937 11345 : thrust_loss.counter++;
4938 :
4939 : // check if thrust loss for 1 second
4940 11345 : if (thrust_loss.counter >= plane.scheduler.get_loop_rate_hz()) {
4941 : // reset counter
4942 5 : thrust_loss.counter = 0;
4943 5 : LOGGER_WRITE_ERROR(LogErrorSubsystem::THRUST_LOSS_CHECK, LogErrorCode::FAILSAFE_OCCURRED);
4944 : // send message to gcs
4945 5 : gcs().send_text(MAV_SEVERITY_EMERGENCY, "Potential VTOL Thrust Loss (%u)", motors->get_lost_motor() + 1);
4946 : // enable thrust loss handling
4947 5 : motors->set_thrust_boost(true);
4948 : // the motors library disables this when it is no longer needed to achieve the commanded output
4949 : }
4950 : }
4951 :
4952 : #endif // HAL_QUADPLANE_ENABLED
|
