Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : control code for tailsitters. Enabled by setting Q_FRAME_CLASS=10
17 : or by setting Q_TAILSIT_MOTMX nonzero and Q_FRAME_CLASS and Q_FRAME_TYPE
18 : to a configuration supported by AP_MotorsMatrix
19 : */
20 : #include "tailsitter.h"
21 : #include "Plane.h"
22 :
23 : #if HAL_QUADPLANE_ENABLED
24 :
25 : const AP_Param::GroupInfo Tailsitter::var_info[] = {
26 :
27 : // @Param: ENABLE
28 : // @DisplayName: Enable Tailsitter
29 : // @Values: 0:Disable, 1:Enable, 2:Enable Always
30 : // @Description: This enables Tailsitter functionality. A value of 2 forces Qassist active and always stabilize in forward flight with airmode for stabilisation at 0 throttle, for use on vehicles with no control surfaces, vehicle will not arm in forward flight modes, see also Q_OPTIONS "Mtrs_Only_Qassist"
31 : // @User: Standard
32 : // @RebootRequired: True
33 : AP_GROUPINFO_FLAGS("ENABLE", 1, Tailsitter, enable, 0, AP_PARAM_FLAG_ENABLE),
34 :
35 : // @Param: ANGLE
36 : // @DisplayName: Tailsitter fixed wing transition angle
37 : // @Description: This is the pitch angle at which tailsitter aircraft will change from VTOL control to fixed wing control.
38 : // @Units: deg
39 : // @Range: 5 80
40 : AP_GROUPINFO("ANGLE", 2, Tailsitter, transition_angle_fw, 45),
41 :
42 : // @Param: ANG_VT
43 : // @DisplayName: Tailsitter VTOL transition angle
44 : // @Description: This is the pitch angle at which tailsitter aircraft will change from fixed wing control to VTOL control, if zero Q_TAILSIT_ANGLE will be used
45 : // @Units: deg
46 : // @Range: 5 80
47 : AP_GROUPINFO("ANG_VT", 3, Tailsitter, transition_angle_vtol, 0),
48 :
49 : // @Param: INPUT
50 : // @DisplayName: Tailsitter input type bitmask
51 : // @Description: This controls whether stick input when hovering as a tailsitter follows the conventions for fixed wing hovering or multicopter hovering. When PlaneMode is not enabled (bit0 = 0) the roll stick will roll the aircraft in earth frame and yaw stick will yaw in earth frame. When PlaneMode input is enabled, the roll and yaw sticks are swapped so that the roll stick controls earth-frame yaw and rudder controls earth-frame roll. When body-frame roll is enabled (bit1 = 1), the yaw stick controls earth-frame yaw rate and the roll stick controls roll in the tailsitter's body frame when flying level.
52 : // @Bitmask: 0:PlaneMode,1:BodyFrameRoll
53 : AP_GROUPINFO("INPUT", 4, Tailsitter, input_type, 0),
54 :
55 : // 5 was MASK
56 : // 6 was MASKCH
57 :
58 : // @Param: VFGAIN
59 : // @DisplayName: Tailsitter vector thrust gain in forward flight
60 : // @Description: This controls the amount of vectored thrust control used in forward flight for a vectored tailsitter
61 : // @Range: 0 1
62 : // @Increment: 0.01
63 : AP_GROUPINFO("VFGAIN", 7, Tailsitter, vectored_forward_gain, 0),
64 :
65 : // @Param: VHGAIN
66 : // @DisplayName: Tailsitter vector thrust gain in hover
67 : // @Description: This controls the amount of vectored thrust control used in hover for a vectored tailsitter
68 : // @Range: 0 1
69 : // @Increment: 0.01
70 : AP_GROUPINFO("VHGAIN", 8, Tailsitter, vectored_hover_gain, 0.5),
71 :
72 : // @Param: VHPOW
73 : // @DisplayName: Tailsitter vector thrust gain power
74 : // @Description: This controls the amount of extra pitch given to the vectored control when at high pitch errors
75 : // @Range: 0 4
76 : // @Increment: 0.1
77 : AP_GROUPINFO("VHPOW", 9, Tailsitter, vectored_hover_power, 2.5),
78 :
79 : // @Param: GSCMAX
80 : // @DisplayName: Maximum tailsitter gain scaling
81 : // @Description: Maximum gain scaling for tailsitter Q_TAILSIT_GSCMSK options
82 : // @Range: 1 5
83 : // @User: Standard
84 : AP_GROUPINFO("GSCMAX", 10, Tailsitter, throttle_scale_max, 2),
85 :
86 : // @Param: RLL_MX
87 : // @DisplayName: Maximum Roll angle
88 : // @Description: Maximum Allowed roll angle for tailsitters. If this is zero then Q_A_ANGLE_MAX is used.
89 : // @Units: deg
90 : // @Range: 0 80
91 : // @User: Standard
92 : AP_GROUPINFO("RLL_MX", 11, Tailsitter, max_roll_angle, 0),
93 :
94 : // @Param: MOTMX
95 : // @DisplayName: Tailsitter motor mask
96 : // @Description: Bitmask of motors to remain active in forward flight for a 'Copter' tailsitter. Non-zero indicates airframe is a Copter tailsitter and uses copter style motor layouts determined by Q_FRAME_CLASS and Q_FRAME_TYPE. This should be zero for non-Copter tailsitters.
97 : // @User: Standard
98 : // @Bitmask: 0:Motor 1, 1:Motor 2, 2:Motor 3, 3:Motor 4, 4:Motor 5, 5:Motor 6, 6:Motor 7, 7:Motor 8, 8:Motor 9, 9:Motor 10, 10:Motor 11, 11:Motor 12
99 : AP_GROUPINFO("MOTMX", 12, Tailsitter, motor_mask, 0),
100 :
101 : // @Param: GSCMSK
102 : // @DisplayName: Tailsitter gain scaling mask
103 : // @Description: Bitmask of gain scaling methods to be applied: Throttle: scale gains with throttle, ATT_THR: reduce gain at high throttle/tilt, 2:Disk theory velocity calculation, requires Q_TAILSIT_DSKLD to be set, ATT_THR must not be set, 3:Altitude correction, scale with air density
104 : // @User: Standard
105 : // @Bitmask: 0:Throttle,1:ATT_THR,2:Disk Theory,3:Altitude correction
106 : AP_GROUPINFO("GSCMSK", 13, Tailsitter, gain_scaling_mask, TAILSITTER_GSCL_THROTTLE),
107 :
108 : // @Param: GSCMIN
109 : // @DisplayName: Minimum tailsitter gain scaling
110 : // @Description: Minimum gain scaling for tailsitter Q_TAILSIT_GSCMSK options
111 : // @Range: 0.1 1
112 : // @User: Standard
113 : AP_GROUPINFO("GSCMIN", 14, Tailsitter, gain_scaling_min, 0.4),
114 :
115 : // @Param: DSKLD
116 : // @DisplayName: Tailsitter disk loading
117 : // @Description: This is the vehicle weight in kg divided by the total disk area of all propellers in m^2. Only used with Q_TAILSIT_GSCMSK = 4
118 : // @Units: kg/m/m
119 : // @Range: 0 50
120 : // @User: Standard
121 : AP_GROUPINFO("DSKLD", 15, Tailsitter, disk_loading, 0),
122 :
123 : // @Param: RAT_FW
124 : // @DisplayName: Tailsitter VTOL to forward flight transition rate
125 : // @Description: The pitch rate at which tailsitter aircraft will pitch down in the transition from VTOL to forward flight
126 : // @Units: deg/s
127 : // @Range: 10 500
128 : AP_GROUPINFO("RAT_FW", 16, Tailsitter, transition_rate_fw, 50),
129 :
130 : // @Param: RAT_VT
131 : // @DisplayName: Tailsitter forward flight to VTOL transition rate
132 : // @Description: The pitch rate at which tailsitter aircraft will pitch up in the transition from forward flight to VTOL
133 : // @Units: deg/s
134 : // @Range: 10 500
135 : AP_GROUPINFO("RAT_VT", 17, Tailsitter, transition_rate_vtol, 50),
136 :
137 : // @Param: THR_VT
138 : // @DisplayName: Tailsitter forward flight to VTOL transition throttle
139 : // @Description: Throttle used during FW->VTOL transition, -1 uses hover throttle
140 : // @Units: %
141 : // @Range: -1 100
142 : AP_GROUPINFO("THR_VT", 18, Tailsitter, transition_throttle_vtol, -1),
143 :
144 : // @Param: VT_R_P
145 : // @DisplayName: Tailsitter VTOL control surface roll gain
146 : // @Description: Scale from PID output to control surface, for use where a single axis is actuated by both motors and Tilt/control surface on a copter style tailsitter, increase to favor control surfaces and reduce motor output by reducing gains
147 : // @Range: 0 2
148 : AP_GROUPINFO("VT_R_P", 19, Tailsitter, VTOL_roll_scale, 1),
149 :
150 : // @Param: VT_P_P
151 : // @DisplayName: Tailsitter VTOL control surface pitch gain
152 : // @Description: Scale from PID output to control surface, for use where a single axis is actuated by both motors and Tilt/control surface on a copter style tailsitter, increase to favor control surfaces and reduce motor output by reducing gains
153 : // @Range: 0 2
154 : AP_GROUPINFO("VT_P_P", 20, Tailsitter, VTOL_pitch_scale, 1),
155 :
156 : // @Param: VT_Y_P
157 : // @DisplayName: Tailsitter VTOL control surface yaw gain
158 : // @Description: Scale from PID output to control surface, for use where a single axis is actuated by both motors and Tilt/control surface on a copter style tailsitter, increase to favor control surfaces and reduce motor output by reducing gains
159 : // @Range: 0 2
160 : AP_GROUPINFO("VT_Y_P", 21, Tailsitter, VTOL_yaw_scale, 1),
161 :
162 : // @Param: MIN_VO
163 : // @DisplayName: Tailsitter Disk loading minimum outflow speed
164 : // @Description: Use in conjunction with disk theory gain scaling and Q_TAILSIT_DSKLD to specify minimum airspeed over control surfaces, this will be used to boost throttle, when descending for example, 0 disables
165 : // @Range: 0 15
166 : AP_GROUPINFO("MIN_VO", 22, Tailsitter, disk_loading_min_outflow, 0),
167 :
168 : AP_GROUPEND
169 : };
170 :
171 : /*
172 : defaults for tailsitters
173 : */
174 : static const struct AP_Param::defaults_table_struct defaults_table_tailsitter[] = {
175 : { "KFF_RDDRMIX", 0.02 },
176 : { "Q_A_RAT_PIT_FF", 0.2 },
177 : { "Q_A_RAT_YAW_FF", 0.2 },
178 : { "Q_A_RAT_YAW_I", 0.18 },
179 : { "Q_A_ANGLE_BOOST", 0 },
180 : { "PTCH_LIM_MAX_DEG", 30 },
181 : { "PTCH_LIM_MIN_DEG", -30 },
182 : { "MIXING_GAIN", 1.0 },
183 : { "RUDD_DT_GAIN", 10 },
184 : { "Q_TRANSITION_MS", 2000 },
185 : { "Q_TRANS_DECEL", 6 },
186 : { "Q_A_ACC_P_MAX", 300},
187 : { "Q_A_ACC_R_MAX", 300},
188 : { "Q_P_NE_POS_P", 0.5},
189 : { "Q_P_NE_VEL_P", 1.0},
190 : { "Q_P_NE_VEL_I", 0.5},
191 : { "Q_P_NE_VEL_D", 0.25},
192 :
193 : };
194 :
195 692 : Tailsitter::Tailsitter(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors):quadplane(_quadplane),motors(_motors)
196 : {
197 692 : AP_Param::setup_object_defaults(this, var_info);
198 692 : }
199 :
200 201 : void Tailsitter::setup()
201 : {
202 : // Set tailsitter enable flag based on old heuristics
203 201 : if (!enable.configured() && (((quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) || (motor_mask != 0)) && (quadplane.tiltrotor.type != Tiltrotor::TILT_TYPE_BICOPTER))) {
204 2 : enable.set_and_save(1);
205 : }
206 :
207 201 : if (enable <= 0) {
208 195 : return;
209 : }
210 :
211 6 : quadplane.thrust_type = QuadPlane::ThrustType::TAILSITTER;
212 :
213 : // Set tailsitter transition rate to match old calculation
214 6 : if (!transition_rate_fw.configured()) {
215 3 : transition_rate_fw.set_and_save(transition_angle_fw / (quadplane.transition_time_ms/2000.0f));
216 : }
217 :
218 : // TODO: update this if servo function assignments change
219 : // used by relax_attitude_control() to control special behavior for vectored tailsitters
220 7 : _is_vectored = (quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) &&
221 2 : (!is_zero(vectored_hover_gain) &&
222 2 : (SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft) ||
223 1 : SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorRight)));
224 :
225 6 : _have_elevator = SRV_Channels::function_assigned(SRV_Channel::k_elevator);
226 6 : _have_aileron = SRV_Channels::function_assigned(SRV_Channel::k_aileron);
227 6 : _have_rudder = SRV_Channels::function_assigned(SRV_Channel::k_rudder);
228 6 : _have_elevon = SRV_Channels::function_assigned(SRV_Channel::k_elevon_left) || SRV_Channels::function_assigned(SRV_Channel::k_elevon_right);
229 6 : _have_v_tail = SRV_Channels::function_assigned(SRV_Channel::k_vtail_left) || SRV_Channels::function_assigned(SRV_Channel::k_vtail_right);
230 :
231 : // set defaults for dual/single motor tailsitter
232 6 : if (quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER) {
233 1 : AP_Param::set_defaults_from_table(defaults_table_tailsitter, ARRAY_SIZE(defaults_table_tailsitter));
234 : }
235 :
236 : // Setup for control surface less operation
237 6 : if (enable == 2) {
238 0 : quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
239 0 : quadplane.air_mode = AirMode::ASSISTED_FLIGHT_ONLY;
240 :
241 : // Do not allow arming in forward flight modes
242 : // motors will become active due to assisted flight airmode, the vehicle will try very hard to get level
243 0 : quadplane.options.set(quadplane.options.get() | int32_t(QuadPlane::Option::ONLY_ARM_IN_QMODE_OR_AUTO));
244 : }
245 :
246 6 : transition = NEW_NOTHROW Tailsitter_Transition(quadplane, motors, *this);
247 6 : if (!transition) {
248 0 : AP_BoardConfig::allocation_error("tailsitter transition");
249 : }
250 6 : quadplane.transition = transition;
251 :
252 6 : setup_complete = true;
253 : }
254 :
255 : /*
256 : return true when flying a control surface only tailsitter
257 : */
258 2638252 : bool Tailsitter::is_control_surface_tailsitter(void) const
259 : {
260 2638252 : return quadplane.frame_class == AP_Motors::MOTOR_FRAME_TAILSITTER
261 2638252 : && ( is_zero(vectored_hover_gain) || !SRV_Channels::function_assigned(SRV_Channel::k_tiltMotorLeft));
262 : }
263 :
264 : /*
265 : check if we are flying as a tailsitter
266 : */
267 17514218 : bool Tailsitter::active(void)
268 : {
269 17514218 : if (!enabled()) {
270 17063232 : return false;
271 : }
272 450986 : if (quadplane.in_vtol_mode()) {
273 252064 : return true;
274 : }
275 : // check if we are in ANGLE_WAIT fixed wing transition
276 198922 : if (transition->transition_state == Tailsitter_Transition::State::ANGLE_WAIT_FW) {
277 6666 : return true;
278 : }
279 192256 : return false;
280 : }
281 :
282 : /*
283 : run output for tailsitters
284 : */
285 8438671 : void Tailsitter::output(void)
286 : {
287 8438671 : if (!enabled() || quadplane.motor_test.running || !quadplane.initialised) {
288 : // if motor test is running we don't want to overwrite it with output_motor_mask or motors_output
289 8340314 : return;
290 : }
291 :
292 275963 : if (!hal.util->get_soft_armed() ||
293 118099 : SRV_Channels::get_emergency_stop()) {
294 : // Ensure motors stop on disarm
295 39765 : motors->output_min();
296 : }
297 :
298 : // throttle 0 to 1
299 157864 : float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01;
300 :
301 : // handle forward flight modes and transition to VTOL modes
302 157864 : if (!active() || in_vtol_transition()) {
303 : // get FW controller throttle demand and mask of motors enabled during forward flight
304 62421 : if (plane.arming.is_armed_and_safety_off() && in_vtol_transition() && !quadplane.throttle_wait) {
305 : /*
306 : during transitions to vtol mode set the throttle to hover thrust, center the rudder
307 : */
308 1214 : if (!is_negative(transition_throttle_vtol)) {
309 : // Q_TAILSIT_THR_VT is positive use it until transition is complete
310 0 : throttle = motors->thr_lin.actuator_to_thrust(MIN(transition_throttle_vtol*0.01,1.0));
311 : } else {
312 1214 : throttle = motors->get_throttle_hover();
313 : // work out equivalent motors throttle level for cruise
314 1214 : throttle = MAX(throttle,motors->thr_lin.actuator_to_thrust(plane.aparm.throttle_cruise.get() * 0.01));
315 : }
316 :
317 1214 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0.0);
318 1214 : plane.rudder_dt = 0;
319 :
320 : // in assisted flight this is done in the normal motor output path
321 1214 : if (!quadplane.assisted_flight) {
322 :
323 : // keep attitude control throttle level upto date, this value should never be output to motors
324 : // it is used to re-set the accel Z integrator term allowing for a smooth transfer of control
325 1214 : quadplane.attitude_control->set_throttle_out(throttle, false, 0);
326 :
327 : // convert the hover throttle to the same output that would result if used via AP_Motors
328 : // apply expo, battery scaling and SPIN min/max.
329 1214 : throttle = motors->thr_lin.thrust_to_actuator(throttle);
330 :
331 : // override AP_MotorsTailsitter throttles during back transition
332 :
333 : // apply PWM min and MAX to throttle left and right, just as via AP_Motors
334 1214 : uint16_t throttle_pwm = motors->get_pwm_output_min() + (motors->get_pwm_output_max() - motors->get_pwm_output_min()) * throttle;
335 1214 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, throttle_pwm);
336 1214 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, throttle_pwm);
337 :
338 : // throttle output is not used by AP_Motors so might have different PWM range, set scaled
339 1214 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle * 100.0);
340 : }
341 : }
342 :
343 62421 : if (!quadplane.assisted_flight) {
344 : // set AP_MotorsMatrix throttles for forward flight
345 : // Output throttle to masked motors and zero all others
346 59507 : const uint32_t mask = uint32_t(motor_mask.get());
347 59507 : motors->output_motor_mask(0.0, ~mask, 0.0);
348 59507 : motors->output_motor_mask(throttle, mask, plane.rudder_dt);
349 :
350 : // No tilt output unless forward gain is set
351 59507 : float tilt_left = 0.0;
352 59507 : float tilt_right = 0.0;
353 :
354 : // in forward flight: set motor tilt servos and throttles using FW controller
355 59507 : if (vectored_forward_gain > 0) {
356 : // remove scaling from surface speed scaling and apply throttle scaling
357 0 : const float scaler = plane.control_mode == &plane.mode_manual?1:(quadplane.FW_vector_throttle_scaling() / plane.get_speed_scaler());
358 : // thrust vectoring in fixed wing flight
359 0 : float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
360 0 : float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
361 0 : tilt_left = (elevator + aileron) * vectored_forward_gain * scaler;
362 0 : tilt_right = (elevator - aileron) * vectored_forward_gain * scaler;
363 : }
364 59507 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
365 59507 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
366 59507 : return;
367 : }
368 : }
369 :
370 : // handle Copter controller
371 : // the MultiCopter rate controller has already been run in an earlier call
372 : // to motors_output() from quadplane.update(), unless we are in assisted flight
373 : // tailsitter in Tailsitter_Transition::State::ANGLE_WAIT_FW is not really in assisted flight, its still in a VTOL mode
374 98357 : if (quadplane.assisted_flight && (transition->transition_state != Tailsitter_Transition::State::ANGLE_WAIT_FW)) {
375 2914 : quadplane.hold_stabilize(throttle);
376 2914 : quadplane.motors_output(true);
377 :
378 2914 : if (quadplane.option_is_set(QuadPlane::Option::TAILSIT_Q_ASSIST_MOTORS_ONLY)) {
379 : // only use motors for Q assist, control surfaces remain under plane control. Zero copter I terms and use plane.
380 : // Smoothly relax to zero so there is no step change in output, must also set limit flags so integrator cannot build faster than the relax.
381 : // Assume there is always roll control surfaces, otherwise motors only assist should not be set.
382 0 : const float dt = quadplane.attitude_control->get_dt_s();
383 :
384 : // VTOL yaw / FW roll
385 0 : quadplane.attitude_control->get_rate_yaw_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC);
386 0 : motors->limit.yaw = true;
387 :
388 : // VTOL and FW pitch
389 0 : if (_have_elevator || _have_elevon || _have_v_tail) {
390 : // have pitch control surfaces, use them
391 0 : quadplane.attitude_control->get_rate_pitch_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC);
392 0 : motors->limit.pitch = true;
393 : } else {
394 : // no pitch control surfaces, zero plane I terms and use motors
395 : // We skip the outputting to surfaces for this axis from the copter controller but there are none setup
396 0 : plane.pitchController.reset_I();
397 : }
398 :
399 : // VTOL roll / FW yaw
400 0 : if (_have_rudder || _have_v_tail) {
401 : // there are yaw control surfaces, zero motor I term
402 0 : quadplane.attitude_control->get_rate_roll_pid().relax_integrator(0.0, dt, AC_ATTITUDE_RATE_RELAX_TC);
403 0 : motors->limit.roll = true;
404 : } else {
405 : // no yaw control surfaces, zero plane I terms and use motors
406 : // We skip the outputting to surfaces for this axis from the copter controller but there are none setup
407 0 : plane.yawController.reset_I();
408 : }
409 :
410 : // output tilt motors
411 :
412 : // No output unless hover gain is set
413 0 : float tilt_left = 0.0;
414 0 : float tilt_right = 0.0;
415 :
416 0 : if (vectored_hover_gain > 0) {
417 0 : const float hover_throttle = motors->get_throttle_hover();
418 0 : const float output_throttle = motors->get_throttle();
419 0 : float throttle_scaler = throttle_scale_max;
420 0 : if (is_positive(output_throttle)) {
421 0 : throttle_scaler = constrain_float(hover_throttle / output_throttle, gain_scaling_min, throttle_scale_max);
422 : }
423 0 : tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft) * vectored_hover_gain * throttle_scaler;
424 0 : tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight) * vectored_hover_gain * throttle_scaler;
425 : }
426 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
427 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
428 :
429 :
430 : // skip remainder of the function that overwrites plane control surface outputs with copter
431 0 : return;
432 : }
433 2914 : } else {
434 95443 : quadplane.motors_output(false);
435 : }
436 :
437 : // In full Q assist it is better to use copter I and zero plane
438 98357 : plane.pitchController.reset_I();
439 98357 : plane.rollController.reset_I();
440 98357 : plane.yawController.reset_I();
441 :
442 : // pull in copter control outputs
443 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, (motors->get_yaw()+motors->get_yaw_ff())*-SERVO_MAX*VTOL_yaw_scale);
444 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, (motors->get_pitch()+motors->get_pitch_ff())*SERVO_MAX*VTOL_pitch_scale);
445 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, (motors->get_roll()+motors->get_roll_ff())*SERVO_MAX*VTOL_roll_scale);
446 :
447 98357 : if (plane.arming.is_armed_and_safety_off()) {
448 : // scale surfaces for throttle
449 78495 : speed_scaling();
450 19862 : } else if (tailsitter_motors != nullptr) {
451 18764 : tailsitter_motors->set_min_throttle(0.0);
452 : }
453 :
454 : // No tilt output unless hover gain is set
455 98357 : float tilt_left = 0.0;
456 98357 : float tilt_right = 0.0;
457 :
458 98357 : if (vectored_hover_gain > 0) {
459 : // thrust vectoring VTOL modes
460 98357 : tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);
461 98357 : tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight);
462 : /*
463 : apply extra elevator when at high pitch errors, using a
464 : power law. This allows the motors to point straight up for
465 : takeoff without integrator windup
466 : */
467 98357 : float des_pitch_cd = quadplane.attitude_control->get_att_target_euler_cd().y;
468 98357 : int32_t pitch_error_cd = (des_pitch_cd - quadplane.ahrs_view->pitch_sensor) * 0.5;
469 98357 : float extra_pitch = constrain_float(pitch_error_cd, -SERVO_MAX, SERVO_MAX) / SERVO_MAX;
470 98357 : float extra_sign = extra_pitch > 0?1:-1;
471 98357 : float extra_elevator = 0;
472 98357 : if (!is_zero(extra_pitch) && quadplane.in_vtol_mode()) {
473 55630 : extra_elevator = extra_sign * powf(fabsf(extra_pitch), vectored_hover_power) * SERVO_MAX;
474 : }
475 98357 : tilt_left = extra_elevator + tilt_left * vectored_hover_gain;
476 98357 : tilt_right = extra_elevator + tilt_right * vectored_hover_gain;
477 : }
478 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
479 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
480 :
481 : // Check for saturated limits
482 98357 : bool tilt_lim = _is_vectored && ((fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Function::k_tiltMotorLeft)) >= SERVO_MAX) || (fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Function::k_tiltMotorRight)) >= SERVO_MAX));
483 98357 : bool roll_lim = _have_rudder && (fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Function::k_rudder)) >= SERVO_MAX);
484 98357 : bool pitch_lim = _have_elevator && (fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Function::k_elevator)) >= SERVO_MAX);
485 98357 : bool yaw_lim = _have_aileron && (fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Function::k_aileron)) >= SERVO_MAX);
486 :
487 : // Mix elevons and V-tail, always giving full priority to pitch
488 98357 : float elevator_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (100.0 - plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
489 98357 : float aileron_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * (100.0 + plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
490 98357 : float rudder_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) * (100.0 + plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
491 :
492 98357 : const float headroom = SERVO_MAX - fabsf(elevator_mix);
493 98357 : if (is_positive(headroom)) {
494 97633 : if (fabsf(aileron_mix) > headroom) {
495 121 : aileron_mix *= headroom / fabsf(aileron_mix);
496 121 : yaw_lim |= _have_elevon;
497 : }
498 97633 : if (fabsf(rudder_mix) > headroom) {
499 712 : rudder_mix *= headroom / fabsf(rudder_mix);
500 712 : roll_lim |= _have_v_tail;
501 : }
502 : } else {
503 724 : aileron_mix = 0.0;
504 724 : rudder_mix = 0.0;
505 724 : yaw_lim |= _have_elevon;
506 724 : pitch_lim |= _have_elevon || _have_v_tail;
507 724 : roll_lim |= _have_v_tail;
508 : }
509 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, elevator_mix - aileron_mix);
510 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, elevator_mix + aileron_mix);
511 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_vtail_right, elevator_mix - rudder_mix);
512 98357 : SRV_Channels::set_output_scaled(SRV_Channel::k_vtail_left, elevator_mix + rudder_mix);
513 :
514 98357 : if (roll_lim) {
515 2793 : motors->limit.roll = true;
516 : }
517 98357 : if (pitch_lim || tilt_lim) {
518 6400 : motors->limit.pitch = true;
519 : }
520 98357 : if (yaw_lim || tilt_lim) {
521 0 : motors->limit.yaw = true;
522 : }
523 :
524 : }
525 :
526 :
527 : /*
528 : return true when we have completed enough of a transition to switch to fixed wing control
529 : */
530 2107 : bool Tailsitter::transition_fw_complete(void)
531 : {
532 2107 : if (!plane.arming.is_armed_and_safety_off()) {
533 : // instant transition when disarmed, no message
534 0 : return true;
535 : }
536 2107 : if (labs(quadplane.ahrs_view->pitch_sensor) > transition_angle_fw*100) {
537 3 : gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done");
538 3 : return true;
539 : }
540 2104 : if (labs(quadplane.ahrs_view->roll_sensor) > MAX(4500, plane.roll_limit_cd + 500)) {
541 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Transition FW done, roll error");
542 0 : return true;
543 : }
544 2104 : if (AP_HAL::millis() - transition->fw_transition_start_ms > ((transition_angle_fw+(transition->fw_transition_initial_pitch*0.01f))/transition_rate_fw)*1500) {
545 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Transition FW done, timeout");
546 0 : return true;
547 : }
548 : // still waiting
549 2104 : return false;
550 : }
551 :
552 :
553 : /*
554 : return true when we have completed enough of a transition to switch to VTOL control
555 : */
556 1220 : bool Tailsitter::transition_vtol_complete(void) const
557 : {
558 1220 : if (!plane.arming.is_armed_and_safety_off()) {
559 : // instant transition when disarmed, no message
560 1 : return true;
561 : }
562 : // for vectored tailsitters at zero pilot throttle
563 1219 : if ((quadplane.get_pilot_throttle() < 0.05f) && _is_vectored) {
564 : // if we are not moving (hence on the ground?) or don't know
565 : // transition immediately to tilt motors up and prevent prop strikes
566 0 : if (quadplane.ahrs.groundspeed() < 1.0f) {
567 0 : gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done, zero throttle");
568 0 : return true;
569 : }
570 : }
571 1219 : const float trans_angle = get_transition_angle_vtol();
572 1219 : if (labs(plane.ahrs.pitch_sensor) > trans_angle*100) {
573 2 : gcs().send_text(MAV_SEVERITY_INFO, "Transition VTOL done");
574 2 : return true;
575 : }
576 1217 : int32_t roll_cd = labs(plane.ahrs.roll_sensor);
577 1217 : if (plane.fly_inverted()) {
578 0 : roll_cd = 18000 - roll_cd;
579 : }
580 1217 : if (roll_cd > MAX(4500, plane.roll_limit_cd + 500)) {
581 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, roll error");
582 0 : return true;
583 : }
584 1217 : if (AP_HAL::millis() - transition->vtol_transition_start_ms > ((trans_angle-(transition->vtol_transition_initial_pitch*0.01f))/transition_rate_vtol)*1500) {
585 3 : gcs().send_text(MAV_SEVERITY_WARNING, "Transition VTOL done, timeout");
586 3 : return true;
587 : }
588 1214 : return false;
589 : }
590 :
591 : // handle different tailsitter input types
592 2710433 : void Tailsitter::check_input(void)
593 : {
594 2710433 : if (active() && (input_type & TAILSITTER_INPUT_PLANE)) {
595 : // the user has asked for body frame controls when tailsitter
596 : // is active. We switch around the control_in value for the
597 : // channels to do this, as that ensures the value is
598 : // consistent throughout the code
599 0 : int16_t roll_in = plane.channel_roll->get_control_in();
600 0 : int16_t yaw_in = plane.channel_rudder->get_control_in();
601 0 : plane.channel_roll->set_control_in(yaw_in);
602 0 : plane.channel_rudder->set_control_in(-roll_in);
603 : }
604 2710433 : }
605 :
606 : /*
607 : return true if we are a tailsitter transitioning to VTOL flight
608 : */
609 7785772 : bool Tailsitter::in_vtol_transition(uint32_t now) const
610 : {
611 7785772 : if (!enabled() || !quadplane.in_vtol_mode()) {
612 7234125 : return false;
613 : }
614 551647 : if (transition->transition_state == Tailsitter_Transition::State::ANGLE_WAIT_VTOL) {
615 7687 : return true;
616 : }
617 543960 : if ((now != 0) && ((now - transition->last_vtol_mode_ms) > 1000)) {
618 : // only just come out of forward flight
619 16 : return true;
620 : }
621 543944 : return false;
622 : }
623 :
624 : /*
625 : return true if we are a tailsitter in FW flight
626 : */
627 10219728 : bool Tailsitter::is_in_fw_flight(void) const
628 : {
629 10219728 : return enabled() && !quadplane.in_vtol_mode() && transition->transition_state == Tailsitter_Transition::State::DONE;
630 : }
631 :
632 : /*
633 : return the tailsitter.transition_angle_vtol value if non zero, otherwise returns the tailsitter.transition_angle_fw value.
634 : */
635 1219 : int8_t Tailsitter::get_transition_angle_vtol() const
636 : {
637 1219 : if (transition_angle_vtol == 0) {
638 1219 : return transition_angle_fw;
639 : }
640 0 : return transition_angle_vtol;
641 : }
642 :
643 :
644 : /*
645 : account for speed scaling of control surfaces in VTOL modes
646 : */
647 78495 : void Tailsitter::speed_scaling(void)
648 : {
649 78495 : const float hover_throttle = motors->get_throttle_hover();
650 78495 : const float throttle = motors->get_throttle_out();
651 78495 : float spd_scaler = 1.0f;
652 78495 : float disk_loading_min_throttle = 0.0;
653 :
654 : // Scaling with throttle
655 78495 : float throttle_scaler = throttle_scale_max;
656 78495 : if (is_positive(throttle)) {
657 76945 : throttle_scaler = constrain_float(hover_throttle / throttle, gain_scaling_min, throttle_scale_max);
658 : }
659 :
660 78495 : if ((gain_scaling_mask & TAILSITTER_GSCL_ATT_THR) != 0) {
661 : // reduce gains when flying at high speed in Q modes:
662 :
663 : // critical parameter: violent oscillations if too high
664 : // sudden loss of attitude control if too low
665 0 : const float min_scale = gain_scaling_min;
666 0 : float tthr = 1.25f * hover_throttle;
667 :
668 : // reduce control surface throws at large tilt
669 : // angles (assuming high airspeed)
670 : // ramp down from 1 to max_atten at tilt angles over trans_angle
671 : // (angles here are represented by their cosines)
672 :
673 : // Note that the cosf call will be necessary if trans_angle becomes a parameter
674 : // but the C language spec does not guarantee that trig functions can be used
675 : // in constant expressions, even though gcc currently allows it.
676 0 : constexpr float c_trans_angle = 0.9238795; // cosf(.125f * M_PI)
677 :
678 : // alpha = (1 - max_atten) / (c_trans_angle - cosf(radians(90)));
679 0 : const float alpha = (1 - min_scale) / c_trans_angle;
680 0 : const float beta = 1 - alpha * c_trans_angle;
681 :
682 0 : const float c_tilt = quadplane.ahrs_view->get_rotation_body_to_ned().c.z;
683 0 : if (c_tilt < c_trans_angle) {
684 0 : spd_scaler = constrain_float(beta + alpha * c_tilt, min_scale, 1.0f);
685 : // reduce throttle attenuation threshold too
686 0 : tthr = 0.5f * hover_throttle;
687 : }
688 : // if throttle is above hover thrust, apply additional attenuation
689 0 : if (throttle > tthr) {
690 0 : const float throttle_atten = 1 - (throttle - tthr) / (1 - tthr);
691 0 : spd_scaler *= throttle_atten;
692 0 : spd_scaler = constrain_float(spd_scaler, min_scale, 1.0f);
693 : }
694 :
695 : // limit positive and negative slew rates of applied speed scaling
696 0 : constexpr float posTC = 2.0f; // seconds
697 0 : constexpr float negTC = 1.0f; // seconds
698 0 : const float posdelta = plane.G_Dt / posTC;
699 0 : const float negdelta = plane.G_Dt / negTC;
700 0 : spd_scaler = constrain_float(spd_scaler, last_spd_scaler - negdelta, last_spd_scaler + posdelta);
701 0 : last_spd_scaler = spd_scaler;
702 :
703 : // also apply throttle scaling if enabled
704 0 : if ((spd_scaler >= 1.0f) && ((gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0)) {
705 0 : spd_scaler = MAX(throttle_scaler,1.0f);
706 : }
707 :
708 78495 : } else if (((gain_scaling_mask & TAILSITTER_GSCL_DISK_THEORY) != 0) && is_positive(disk_loading.get())) {
709 : // Use disk theory to estimate the velocity over the control surfaces
710 : // https://web.mit.edu/16.unified/www/FALL/thermodynamics/notes/node86.html
711 :
712 : float airspeed;
713 0 : if (!quadplane.ahrs.airspeed_EAS(airspeed)) {
714 : // No airspeed estimate, use throttle scaling
715 0 : spd_scaler = throttle_scaler;
716 :
717 : } else {
718 :
719 :
720 : // use the equation: T = 0.5 * rho * A (Ue^2 - U0^2) solved for Ue^2:
721 : // Ue^2 = (T / (0.5 * rho *A)) + U0^2
722 : // We don't know thrust or disk area, use T = (throttle/throttle_hover) * weight
723 : // ((t / t_h ) * weight) / (0.5 * rho * A) = ((t / t_h) * mass * 9.81) / (0.5 * rho * A)
724 : // (mass / A) is disk loading DL so:
725 : // Ue^2 = (((t / t_h) * DL * 9.81)/(0.5 * rho)) + U0^2
726 :
727 0 : const float rho = SSL_AIR_DENSITY * quadplane.ahrs.get_air_density_ratio();
728 0 : float hover_rho = rho;
729 0 : if ((gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
730 : // if applying altitude correction use sea level density for hover case
731 0 : hover_rho = SSL_AIR_DENSITY;
732 : }
733 :
734 : // hover case: (t / t_h) = 1 and U0 = 0
735 0 : const float sq_hover_outflow = (disk_loading.get() * GRAVITY_MSS) / (0.5f * hover_rho);
736 :
737 :
738 : // calculate the true outflow speed
739 0 : const float sq_outflow = (((throttle/hover_throttle) * disk_loading.get() * GRAVITY_MSS) / (0.5f * rho)) + sq(MAX(airspeed,0));
740 :
741 : // Scale by the ratio of squared hover outflow velocity to squared actual outflow velocity
742 0 : spd_scaler = throttle_scale_max;
743 0 : if (is_positive(sq_outflow)) {
744 0 : spd_scaler = constrain_float(sq_hover_outflow / sq_outflow, gain_scaling_min.get(), throttle_scale_max.get());
745 : }
746 :
747 0 : if (is_positive(disk_loading_min_outflow)) {
748 : // calculate throttle required to give minimum outflow speed over control surfaces
749 0 : if (is_positive(airspeed)) {
750 0 : disk_loading_min_throttle = (((sq(disk_loading_min_outflow) - sq(airspeed)) * (0.5 * rho)) / (disk_loading.get() * GRAVITY_MSS)) * hover_throttle;
751 : } else {
752 : // estimate backwards airspeed
753 0 : float reverse_airspeed = 0.0;
754 0 : Vector3f vel;
755 0 : if (quadplane.ahrs.get_velocity_NED(vel)) {
756 0 : Vector3f wind;
757 : // use the estimate even if it is not marked
758 : // valid, to preserve existing behaviour
759 0 : IGNORE_RETURN(quadplane.ahrs.get_wind(wind));
760 0 : reverse_airspeed = quadplane.ahrs.earth_to_body(vel - wind).x;
761 : }
762 : // make sure actually negative
763 0 : reverse_airspeed = MIN(reverse_airspeed, 0.0);
764 0 : disk_loading_min_throttle = (((sq(disk_loading_min_outflow) + sq(reverse_airspeed)) * (0.5 * rho)) / (disk_loading.get() * GRAVITY_MSS)) * hover_throttle;
765 : }
766 0 : disk_loading_min_throttle = MAX(disk_loading_min_throttle, 0.0);
767 : }
768 : }
769 :
770 78495 : } else if ((gain_scaling_mask & TAILSITTER_GSCL_THROTTLE) != 0) {
771 78495 : spd_scaler = throttle_scaler;
772 : }
773 :
774 78495 : if ((gain_scaling_mask & TAILSITTER_GSCL_ALTITUDE) != 0) {
775 : // air density correction
776 0 : spd_scaler /= quadplane.ahrs.get_air_density_ratio();
777 : }
778 :
779 78495 : const SRV_Channel::Function functions[] = {
780 : SRV_Channel::Function::k_aileron,
781 : SRV_Channel::Function::k_elevator,
782 : SRV_Channel::Function::k_rudder,
783 : SRV_Channel::Function::k_tiltMotorLeft,
784 : SRV_Channel::Function::k_tiltMotorRight};
785 470970 : for (uint8_t i=0; i<ARRAY_SIZE(functions); i++) {
786 392475 : float v = SRV_Channels::get_output_scaled(functions[i]);
787 392475 : if ((functions[i] == SRV_Channel::Function::k_tiltMotorLeft) || (functions[i] == SRV_Channel::Function::k_tiltMotorRight)) {
788 : // always apply throttle scaling to tilts
789 156990 : v *= throttle_scaler;
790 : } else {
791 235485 : v *= spd_scaler;
792 : }
793 392475 : SRV_Channels::set_output_scaled(functions[i], v);
794 : }
795 :
796 78495 : if (tailsitter_motors != nullptr) {
797 0 : tailsitter_motors->set_min_throttle(disk_loading_min_throttle);
798 : }
799 :
800 : // Record for log
801 78495 : log_data.throttle_scaler = throttle_scaler;
802 78495 : log_data.speed_scaler = spd_scaler;
803 78495 : log_data.min_throttle = disk_loading_min_throttle;
804 :
805 78495 : }
806 :
807 : #if HAL_LOGGING_ENABLED
808 : // Write tailsitter specific log
809 91778 : void Tailsitter::write_log()
810 : {
811 91778 : if (!enabled()) {
812 89159 : return;
813 : }
814 :
815 2619 : struct log_tailsitter pkt = {
816 : LOG_PACKET_HEADER_INIT(LOG_TSIT_MSG),
817 2619 : time_us : AP_HAL::micros64(),
818 2619 : throttle_scaler : log_data.throttle_scaler,
819 2619 : speed_scaler : log_data.speed_scaler,
820 2619 : min_throttle : log_data.min_throttle,
821 2619 : };
822 2619 : plane.logger.WriteBlock(&pkt, sizeof(pkt));
823 : }
824 : #endif // HAL_LOGGING_ENABLED
825 :
826 : // return true if pitch control should be relaxed
827 : // on vectored belly sitters the pitch control is not relaxed in order to keep motors pointing and avoid risk of props hitting the ground
828 : // always relax after a transition
829 318754 : bool Tailsitter::relax_pitch()
830 : {
831 318754 : return !enabled() || !_is_vectored || (transition->vtol_limit_start_ms != 0);
832 : }
833 :
834 : /*
835 : update for transition from quadplane to fixed wing mode
836 : */
837 62355 : void Tailsitter_Transition::update()
838 : {
839 62355 : const uint32_t now = millis();
840 :
841 : float aspeed;
842 62355 : bool have_airspeed = quadplane.ahrs.airspeed_EAS(aspeed);
843 62355 : quadplane.assisted_flight = quadplane.assist.should_assist(aspeed, have_airspeed);
844 :
845 62355 : if (transition_state != State::DONE) {
846 : // during transition we ask TECS to use a synthetic
847 : // airspeed. Otherwise the pitch limits will throw off the
848 : // throttle calculation which is driven by pitch
849 2107 : plane.TECS_controller.use_synthetic_airspeed();
850 : }
851 :
852 62355 : switch (transition_state) {
853 :
854 2107 : case State::ANGLE_WAIT_FW: {
855 2107 : if (tailsitter.transition_fw_complete()) {
856 3 : transition_state = State::DONE;
857 3 : if (plane.arming.is_armed_and_safety_off()) {
858 3 : fw_limit_start_ms = now;
859 3 : fw_limit_initial_pitch = constrain_float(quadplane.ahrs.pitch_sensor,-8500,8500);
860 3 : plane.nav_pitch_cd = fw_limit_initial_pitch;
861 3 : plane.nav_roll_cd = 0;
862 : }
863 3 : break;
864 : }
865 2104 : quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
866 2104 : quadplane.assisted_flight = true;
867 2104 : uint32_t dt = now - fw_transition_start_ms;
868 : // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
869 2104 : plane.nav_pitch_cd = constrain_float(fw_transition_initial_pitch - (quadplane.tailsitter.transition_rate_fw * dt) * 0.1f * (plane.fly_inverted()?-1.0f:1.0f), -8500, 8500);
870 2104 : plane.nav_roll_cd = 0;
871 2104 : quadplane.disable_yaw_rate_time_constant();
872 2104 : quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(plane.nav_roll_cd,
873 2104 : plane.nav_pitch_cd,
874 : 0);
875 : // set throttle at either hover throttle or current throttle, whichever is higher, through the transition
876 2104 : quadplane.attitude_control->set_throttle_out(MAX(motors->get_throttle_hover(),quadplane.attitude_control->get_throttle_in()), true, 0);
877 2104 : quadplane.motors_output();
878 2104 : break;
879 : }
880 :
881 0 : case State::ANGLE_WAIT_VTOL:
882 : // nothing to do, this is handled in the fixed wing attitude controller
883 0 : break;
884 :
885 60248 : case State::DONE:
886 60248 : break;
887 : }
888 62355 : }
889 :
890 94901 : void Tailsitter_Transition::VTOL_update()
891 : {
892 94901 : const uint32_t now = AP_HAL::millis();
893 :
894 94901 : if ((now - last_vtol_mode_ms) > 1000) {
895 : /*
896 : we are just entering a VTOL mode as a tailsitter, set
897 : our transition state so the fixed wing controller brings
898 : the nose up before we start trying to fly as a
899 : multicopter
900 : */
901 6 : transition_state = State::ANGLE_WAIT_VTOL;
902 : }
903 94901 : last_vtol_mode_ms = now;
904 :
905 94901 : if (transition_state == State::ANGLE_WAIT_VTOL) {
906 : float aspeed;
907 1220 : bool have_airspeed = quadplane.ahrs.airspeed_EAS(aspeed);
908 : // provide assistance in forward flight portion of tailsitter transition
909 1220 : quadplane.assisted_flight = quadplane.assist.should_assist(aspeed, have_airspeed);
910 1220 : if (!quadplane.tailsitter.transition_vtol_complete()) {
911 1214 : return;
912 : }
913 : // transition to VTOL complete, if armed set vtol rate limit starting point
914 6 : if (plane.arming.is_armed_and_safety_off()) {
915 5 : vtol_limit_start_ms = now;
916 5 : vtol_limit_initial_pitch = quadplane.ahrs_view->pitch_sensor;
917 : }
918 :
919 : // clear inverted flight flag to make behaviour consistent
920 : // with other quadplane types
921 6 : plane.inverted_flight = false;
922 : } else {
923 : // Keep assistance reset while not checking
924 93681 : quadplane.assist.reset();
925 : }
926 93687 : restart();
927 : }
928 :
929 : // return true if we should show VTOL view
930 107046 : bool Tailsitter_Transition::show_vtol_view() const
931 : {
932 107046 : bool show_vtol = quadplane.in_vtol_mode();
933 :
934 107046 : if (show_vtol && (transition_state == State::ANGLE_WAIT_VTOL)) {
935 : // in a vtol mode but still transitioning from forward flight
936 1435 : return false;
937 : }
938 105611 : if (!show_vtol && (transition_state == State::ANGLE_WAIT_FW)) {
939 : // not in VTOL mode but still transitioning from VTOL
940 2494 : return true;
941 : }
942 103117 : return show_vtol;
943 : }
944 :
945 157760 : void Tailsitter_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd)
946 : {
947 157760 : uint32_t now = AP_HAL::millis();
948 157760 : if (tailsitter.in_vtol_transition(now)) {
949 : /*
950 : during transition to vtol in a tailsitter try to raise the
951 : nose while keeping the wings level
952 : */
953 1220 : uint32_t dt = now - vtol_transition_start_ms;
954 : // multiply by 0.1 to convert (degrees/second * milliseconds) to centi degrees
955 1220 : nav_pitch_cd = constrain_float(vtol_transition_initial_pitch + (tailsitter.transition_rate_vtol * dt) * 0.1f, -8500, 8500);
956 1220 : nav_roll_cd = 0;
957 :
958 156540 : } else if (transition_state == State::DONE) {
959 : // still in FW, reset transition starting point
960 60752 : vtol_transition_start_ms = now;
961 60752 : vtol_transition_initial_pitch = constrain_float(plane.nav_pitch_cd,-8500,8500);
962 :
963 : // rate limit initial pitch down
964 60752 : if (fw_limit_start_ms != 0) {
965 2240 : const float pitch_limit_cd = fw_limit_initial_pitch - (now - fw_limit_start_ms) * tailsitter.transition_rate_fw * 0.1;
966 2240 : if ((pitch_limit_cd <= 0) || (nav_pitch_cd >= pitch_limit_cd)) {
967 : // never limit past 0, never limit to a smaller pitch angle
968 3 : fw_limit_start_ms = 0;
969 : } else {
970 2237 : nav_pitch_cd = pitch_limit_cd;
971 2237 : nav_roll_cd = 0;
972 : }
973 : }
974 : }
975 157760 : }
976 :
977 55978 : bool Tailsitter_Transition::allow_stick_mixing() const
978 : {
979 : // Transitioning into VTOL flight, initial pitch up
980 55978 : if (tailsitter.in_vtol_transition()) {
981 409 : return false;
982 : }
983 : // Transitioning into fixed wing flight, levelling off
984 55569 : if ((transition_state == State::DONE) && (fw_limit_start_ms != 0)) {
985 2237 : return false;
986 : }
987 53332 : return true;
988 : }
989 :
990 104173 : bool Tailsitter_Transition::set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd)
991 : {
992 104173 : if (vtol_limit_start_ms == 0) {
993 103165 : return false;
994 : }
995 : // prevent pitching towards 0 too quickly
996 1008 : const float pitch_change_cd = (AP_HAL::millis() - vtol_limit_start_ms) * tailsitter.transition_rate_vtol * 0.1;
997 1008 : if (pitch_change_cd > fabsf(vtol_limit_initial_pitch)) {
998 : // limit has passed 0, nothing to do
999 2 : vtol_limit_start_ms = 0;
1000 2 : return false;
1001 : }
1002 : // continue limiting while limit angle is larger than desired angle
1003 1006 : if (is_negative(vtol_limit_initial_pitch)) {
1004 1006 : const float pitch_limit = vtol_limit_initial_pitch + pitch_change_cd;
1005 1006 : if (nav_pitch_cd > pitch_limit) {
1006 1005 : nav_pitch_cd = pitch_limit;
1007 1005 : nav_roll_cd = 0;
1008 1005 : return true;
1009 : }
1010 : } else {
1011 0 : const float pitch_limit = vtol_limit_initial_pitch - pitch_change_cd;
1012 0 : if (nav_pitch_cd < pitch_limit) {
1013 0 : nav_pitch_cd = pitch_limit;
1014 0 : nav_roll_cd = 0;
1015 0 : return true;
1016 : }
1017 : }
1018 1 : vtol_limit_start_ms = 0;
1019 1 : return false;
1020 : }
1021 :
1022 : // setup for the transition back to fixed wing
1023 93689 : void Tailsitter_Transition::restart()
1024 : {
1025 93689 : transition_state = State::ANGLE_WAIT_FW;
1026 93689 : fw_transition_start_ms = AP_HAL::millis();
1027 93689 : fw_transition_initial_pitch = constrain_float(quadplane.attitude_control->get_attitude_target_quat().get_euler_pitch() * degrees(100.0),-8500,8500);
1028 93689 : }
1029 :
1030 : // force state to FW and setup for the transition back to VTOL
1031 510 : void Tailsitter_Transition::force_transition_complete()
1032 : {
1033 510 : transition_state = State::DONE;
1034 510 : vtol_transition_start_ms = AP_HAL::millis();
1035 510 : vtol_transition_initial_pitch = constrain_float(plane.nav_pitch_cd,-8500,8500);
1036 510 : fw_limit_start_ms = 0;
1037 :
1038 510 : quadplane.assist.reset();
1039 510 : }
1040 :
1041 0 : MAV_VTOL_STATE Tailsitter_Transition::get_mav_vtol_state() const
1042 : {
1043 0 : switch (transition_state) {
1044 0 : case State::ANGLE_WAIT_VTOL:
1045 0 : return MAV_VTOL_STATE_TRANSITION_TO_MC;
1046 :
1047 0 : case State::DONE:
1048 0 : return MAV_VTOL_STATE_FW;
1049 :
1050 0 : case State::ANGLE_WAIT_FW: {
1051 0 : if (quadplane.in_vtol_mode()) {
1052 0 : return MAV_VTOL_STATE_MC;
1053 : }
1054 0 : return MAV_VTOL_STATE_TRANSITION_TO_FW;
1055 : }
1056 : }
1057 :
1058 0 : return MAV_VTOL_STATE_UNDEFINED;
1059 : }
1060 :
1061 : // only allow to weathervane once transition is complete and desired pitch has been reached
1062 73885 : bool Tailsitter_Transition::allow_weathervane()
1063 : {
1064 73885 : return !tailsitter.in_vtol_transition() && (vtol_limit_start_ms == 0);
1065 : }
1066 :
1067 : #endif // HAL_QUADPLANE_ENABLED
|
