Line data Source code
1 : #include "tiltrotor.h"
2 : #include "Plane.h"
3 :
4 : #if HAL_QUADPLANE_ENABLED
5 : const AP_Param::GroupInfo Tiltrotor::var_info[] = {
6 :
7 : // @Param: ENABLE
8 : // @DisplayName: Enable Tiltrotor functionality
9 : // @Values: 0:Disable, 1:Enable
10 : // @Description: This enables Tiltrotor functionality
11 : // @User: Standard
12 : // @RebootRequired: True
13 : AP_GROUPINFO_FLAGS("ENABLE", 1, Tiltrotor, enable, 0, AP_PARAM_FLAG_ENABLE),
14 :
15 : // @Param: MASK
16 : // @DisplayName: Tiltrotor mask
17 : // @Description: This is a bitmask of motors that are tiltable in a tiltrotor (or tiltwing). The mask is in terms of the standard motor order for the frame type.
18 : // @User: Standard
19 : // @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
20 : AP_GROUPINFO("MASK", 2, Tiltrotor, tilt_mask, 0),
21 :
22 : // @Param: RATE_UP
23 : // @DisplayName: Tiltrotor upwards tilt rate
24 : // @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from forward flight to hover
25 : // @Units: deg/s
26 : // @Increment: 1
27 : // @Range: 10 300
28 : // @User: Standard
29 : AP_GROUPINFO("RATE_UP", 3, Tiltrotor, max_rate_up_dps, 40),
30 :
31 : // @Param: MAX
32 : // @DisplayName: Tiltrotor maximum VTOL angle
33 : // @Description: This is the maximum angle of the tiltable motors at which multicopter control will be enabled. Beyond this angle the plane will fly solely as a fixed wing aircraft and the motors will tilt to their maximum angle at the TILT_RATE
34 : // @Units: deg
35 : // @Increment: 1
36 : // @Range: 20 80
37 : // @User: Standard
38 : AP_GROUPINFO("MAX", 4, Tiltrotor, max_angle_deg, 45),
39 :
40 : // @Param: TYPE
41 : // @DisplayName: Tiltrotor type
42 : // @Description: This is the type of tiltrotor when TILT_MASK is non-zero. A continuous tiltrotor can tilt the rotors to any angle on demand. A binary tiltrotor assumes a retract style servo where the servo is either fully forward or fully up. In both cases the servo can't move faster than Q_TILT_RATE. A vectored yaw tiltrotor will use the tilt of the motors to control yaw in hover, Bicopter tiltrotor must use the tailsitter frame class (10)
43 : // @Values: 0:Continuous,1:Binary,2:VectoredYaw,3:Bicopter
44 : AP_GROUPINFO("TYPE", 5, Tiltrotor, type, TILT_TYPE_CONTINUOUS),
45 :
46 : // @Param: RATE_DN
47 : // @DisplayName: Tiltrotor downwards tilt rate
48 : // @Description: This is the maximum speed at which the motor angle will change for a tiltrotor when moving from hover to forward flight. When this is zero the Q_TILT_RATE_UP value is used.
49 : // @Units: deg/s
50 : // @Increment: 1
51 : // @Range: 10 300
52 : // @User: Standard
53 : AP_GROUPINFO("RATE_DN", 6, Tiltrotor, max_rate_down_dps, 0),
54 :
55 : // @Param: YAW_ANGLE
56 : // @DisplayName: Tilt minimum angle for vectored yaw
57 : // @Description: This is the angle of the tilt servos when in VTOL mode and at minimum output (fully back). This needs to be set in addition to Q_TILT_TYPE=2, to enable vectored control for yaw in tilt quadplanes. This is also used to limit the forward travel of bicopter tilts(Q_TILT_TYPE=3) when in VTOL modes.
58 : // @Range: 0 30
59 : AP_GROUPINFO("YAW_ANGLE", 7, Tiltrotor, tilt_yaw_angle, 0),
60 :
61 : // @Param: FIX_ANGLE
62 : // @DisplayName: Fixed wing tiltrotor angle
63 : // @Description: This is the angle the motors tilt down when at maximum output for forward flight. Set this to a non-zero value to enable vectoring for roll/pitch in forward flight on tilt-vectored aircraft
64 : // @Units: deg
65 : // @Range: 0 30
66 : // @User: Standard
67 : AP_GROUPINFO("FIX_ANGLE", 8, Tiltrotor, fixed_angle, 0),
68 :
69 : // @Param: FIX_GAIN
70 : // @DisplayName: Fixed wing tiltrotor gain
71 : // @Description: This is the gain for use of tilting motors in fixed wing flight for tilt vectored quadplanes
72 : // @Range: 0 1
73 : // @User: Standard
74 : AP_GROUPINFO("FIX_GAIN", 9, Tiltrotor, fixed_gain, 0),
75 :
76 : // @Param: WING_FLAP
77 : // @DisplayName: Tiltrotor tilt angle that will be used as flap
78 : // @Description: For use on tilt wings, the wing will tilt up to this angle for flap, transition will be complete when the wing reaches this angle from the forward fight position, 0 disables
79 : // @Units: deg
80 : // @Increment: 1
81 : // @Range: 0 15
82 : // @User: Standard
83 : AP_GROUPINFO("WING_FLAP", 10, Tiltrotor, flap_angle_deg, 0),
84 :
85 : AP_GROUPEND
86 : };
87 :
88 : /*
89 : control code for tiltrotors and tiltwings. Enabled by setting
90 : Q_TILT_MASK to a non-zero value
91 : */
92 :
93 692 : Tiltrotor::Tiltrotor(QuadPlane& _quadplane, AP_MotorsMulticopter*& _motors):quadplane(_quadplane),motors(_motors)
94 : {
95 692 : AP_Param::setup_object_defaults(this, var_info);
96 692 : }
97 :
98 201 : void Tiltrotor::setup()
99 : {
100 :
101 201 : if (!enable.configured() && ((tilt_mask != 0) || (type == TILT_TYPE_BICOPTER))) {
102 3 : enable.set_and_save(1);
103 : }
104 :
105 201 : if (enable <= 0) {
106 188 : return;
107 : }
108 :
109 13 : quadplane.thrust_type = QuadPlane::ThrustType::TILTROTOR;
110 :
111 13 : _is_vectored = tilt_mask != 0 && type == TILT_TYPE_VECTORED_YAW;
112 :
113 : // true if a fixed forward motor is configured, either throttle, throttle left or throttle right.
114 : // bicopter tiltrotors use throttle left and right as tilting motors, so they don't count in that case.
115 29 : _have_fw_motor = SRV_Channels::function_assigned(SRV_Channel::k_throttle) ||
116 16 : ((SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) || SRV_Channels::function_assigned(SRV_Channel::k_throttleRight))
117 0 : && (type != TILT_TYPE_BICOPTER));
118 :
119 :
120 : // check if there are any permanent VTOL motors
121 129 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
122 126 : if (motors->is_motor_enabled(i) && !is_motor_tilting(i)) {
123 : // enabled motor not set in tilt mask
124 10 : _have_vtol_motor = true;
125 10 : break;
126 : }
127 : }
128 :
129 13 : if (_is_vectored) {
130 : // we will be using vectoring for yaw
131 7 : motors->disable_yaw_torque();
132 : }
133 :
134 13 : if (tilt_mask != 0) {
135 : // setup tilt compensation
136 13 : motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&Tiltrotor::tilt_compensate, void, float *, uint8_t));
137 13 : if (type == TILT_TYPE_VECTORED_YAW) {
138 : // setup tilt servos for vectored yaw
139 7 : SRV_Channels::set_range(SRV_Channel::k_tiltMotorLeft, 1000);
140 7 : SRV_Channels::set_range(SRV_Channel::k_tiltMotorRight, 1000);
141 7 : SRV_Channels::set_range(SRV_Channel::k_tiltMotorRear, 1000);
142 7 : SRV_Channels::set_range(SRV_Channel::k_tiltMotorRearLeft, 1000);
143 7 : SRV_Channels::set_range(SRV_Channel::k_tiltMotorRearRight, 1000);
144 : }
145 : }
146 :
147 13 : transition = NEW_NOTHROW Tiltrotor_Transition(quadplane, motors, *this);
148 13 : if (!transition) {
149 0 : AP_BoardConfig::allocation_error("tiltrotor transition");
150 : }
151 13 : quadplane.transition = transition;
152 :
153 13 : setup_complete = true;
154 : }
155 :
156 : /*
157 : calculate maximum tilt change as a proportion from 0 to 1 of tilt
158 : */
159 1096220 : float Tiltrotor::tilt_max_change(bool up, bool in_flap_range) const
160 : {
161 : float rate;
162 1096220 : if (up || max_rate_down_dps <= 0) {
163 892202 : rate = max_rate_up_dps;
164 : } else {
165 204018 : rate = max_rate_down_dps;
166 : }
167 1096220 : if (type != TILT_TYPE_BINARY && !up && !in_flap_range) {
168 895116 : bool fast_tilt = false;
169 895116 : if (plane.control_mode == &plane.mode_manual) {
170 14486 : fast_tilt = true;
171 : }
172 895116 : if (plane.arming.is_armed_and_safety_off() && !quadplane.in_vtol_mode() && !quadplane.assisted_flight) {
173 341532 : fast_tilt = true;
174 : }
175 895116 : if (fast_tilt) {
176 : // allow a minimum of 90 DPS in manual or if we are not
177 : // stabilising, to give fast control
178 355300 : rate = MAX(rate, 90);
179 : }
180 : }
181 1096220 : return rate * plane.G_Dt * (1/90.0);
182 : }
183 :
184 : /*
185 : output a slew limited tiltrotor angle. tilt is from 0 to 1
186 : */
187 502958 : void Tiltrotor::slew(float newtilt)
188 : {
189 502958 : float max_change = tilt_max_change(newtilt<current_tilt, newtilt > get_fully_forward_tilt());
190 502958 : current_tilt = constrain_float(newtilt, current_tilt-max_change, current_tilt+max_change);
191 :
192 502958 : angle_achieved = is_equal(newtilt, current_tilt);
193 :
194 : // translate to 0..1000 range and output
195 502958 : SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * current_tilt);
196 502958 : }
197 :
198 : // return the current tilt value that represents forward flight
199 : // tilt wings can sustain forward flight with some amount of wing tilt
200 969903 : float Tiltrotor::get_fully_forward_tilt() const
201 : {
202 969903 : return 1.0 - (flap_angle_deg * (1/90.0));
203 : }
204 :
205 : // return the target tilt value for forward flight
206 713069 : float Tiltrotor::get_forward_flight_tilt() const
207 : {
208 713069 : return 1.0 - ((flap_angle_deg * (1/90.0)) * SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) * 0.01);
209 : }
210 :
211 : /*
212 : update motor tilt for continuous tilt servos
213 : */
214 502958 : void Tiltrotor::continuous_update(void)
215 : {
216 : // default to inactive
217 502958 : _motors_active = false;
218 :
219 : // the maximum rate of throttle change
220 : float max_change;
221 :
222 502958 : if (!quadplane.in_vtol_mode() && (!plane.arming.is_armed_and_safety_off() || !quadplane.assisted_flight)) {
223 : // we are in pure fixed wing mode. Move the tiltable motors all the way forward and run them as
224 : // a forward motor
225 :
226 : // option set then if disarmed move to VTOL position to prevent ground strikes, allow tilt forward in manual mode for testing
227 201600 : const bool disarmed_tilt_up = !plane.arming.is_armed_and_safety_off() && (plane.control_mode != &plane.mode_manual) && quadplane.option_is_set(QuadPlane::Option::DISARMED_TILT_UP);
228 201600 : slew(disarmed_tilt_up ? 0.0 : get_forward_flight_tilt());
229 :
230 201600 : max_change = tilt_max_change(false);
231 :
232 201600 : float new_throttle = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01, 0, 1);
233 201600 : if (current_tilt < get_fully_forward_tilt()) {
234 4671 : current_throttle = constrain_float(new_throttle,
235 : current_throttle-max_change,
236 : current_throttle+max_change);
237 : } else {
238 196929 : current_throttle = new_throttle;
239 : }
240 201600 : if (!plane.arming.is_armed_and_safety_off()) {
241 30834 : current_throttle = 0;
242 : } else {
243 : // prevent motor shutdown
244 170766 : _motors_active = true;
245 : }
246 201600 : if (!quadplane.motor_test.running) {
247 : // the motors are all the way forward, start using them for fwd thrust
248 201600 : const uint32_t mask = is_zero(current_throttle) ? 0U : tilt_mask.get();
249 201600 : motors->output_motor_mask(current_throttle, mask, plane.rudder_dt);
250 : }
251 201600 : return;
252 : }
253 :
254 : // remember the throttle level we're using for VTOL flight
255 301358 : float motors_throttle = motors->get_throttle();
256 301358 : max_change = tilt_max_change(motors_throttle<current_throttle);
257 301358 : current_throttle = constrain_float(motors_throttle,
258 : current_throttle-max_change,
259 : current_throttle+max_change);
260 :
261 : /*
262 : we are in a VTOL mode. We need to work out how much tilt is
263 : needed. There are 5 strategies we will use:
264 :
265 : 1) With use of a forward throttle controlled by Q_FWD_THR_GAIN in
266 : VTOL modes except Q_AUTOTUNE determined by Q_FWD_THR_USE. We set the angle based on a calculated
267 : forward throttle.
268 :
269 : 2) With manual forward throttle control we set the angle based on the
270 : RC input demanded forward throttle for QACRO, QSTABILIZE and QHOVER.
271 :
272 : 3) Without a RC input or calculated forward throttle value, the angle
273 : will be set to zero in QAUTOTUNE, QACRO, QSTABILIZE and QHOVER.
274 : This enables these modes to be used as a safe recovery mode.
275 :
276 : 4) In fixed wing assisted flight or velocity controlled modes we will
277 : set the angle based on the demanded forward throttle, with a maximum
278 : tilt given by Q_TILT_MAX. This relies on Q_FWD_THR_GAIN or Q_VFWD_GAIN
279 : being set.
280 :
281 : 5) if we are in TRANSITION_TIMER mode then we are transitioning
282 : to forward flight and should put the rotors all the way forward
283 : */
284 :
285 : #if QAUTOTUNE_ENABLED
286 301358 : if (plane.control_mode == &plane.mode_qautotune) {
287 0 : slew(0);
288 0 : return;
289 : }
290 : #endif
291 :
292 268561 : if (!quadplane.assisted_flight &&
293 569919 : quadplane.get_vfwd_method() == QuadPlane::ActiveFwdThr::NEW &&
294 0 : quadplane.is_flying_vtol())
295 : {
296 : // We are using the rotor tilt functionality controlled by Q_FWD_THR_GAIN which can
297 : // operate in all VTOL modes except Q_AUTOTUNE. Forward rotor tilt is used to produce
298 : // forward thrust equivalent to what would have been produced by a forward thrust motor
299 : // set to quadplane.forward_throttle_pct()
300 0 : const float fwd_g_demand = 0.01 * quadplane.forward_throttle_pct();
301 0 : const float fwd_tilt_deg = MIN(degrees(atanf(fwd_g_demand)), (float)max_angle_deg);
302 0 : slew(MIN(fwd_tilt_deg * (1/90.0), get_forward_flight_tilt()));
303 0 : return;
304 301358 : } else if (!quadplane.assisted_flight &&
305 268561 : (plane.control_mode == &plane.mode_qacro ||
306 268561 : plane.control_mode == &plane.mode_qstabilize ||
307 268561 : plane.control_mode == &plane.mode_qhover))
308 : {
309 112322 : if (quadplane.rc_fwd_thr_ch == nullptr) {
310 : // no manual throttle control, set angle to zero
311 112322 : slew(0);
312 : } else {
313 : // manual control of forward throttle up to max VTOL angle
314 0 : float settilt = 0.01f * quadplane.forward_throttle_pct();
315 0 : slew(MIN(settilt * max_angle_deg * (1/90.0), get_forward_flight_tilt()));
316 : }
317 112322 : return;
318 : }
319 :
320 189036 : if (quadplane.assisted_flight &&
321 32797 : transition->transition_state >= Tiltrotor_Transition::State::TIMER) {
322 : // we are transitioning to fixed wing - tilt the motors all
323 : // the way forward
324 4430 : slew(get_forward_flight_tilt());
325 : } else {
326 : // until we have completed the transition we limit the tilt to
327 : // Q_TILT_MAX. Anything above 50% throttle gets
328 : // Q_TILT_MAX. Below 50% throttle we decrease linearly. This
329 : // relies heavily on Q_VFWD_GAIN being set appropriately.
330 184606 : float settilt = constrain_float((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)-MAX(plane.aparm.throttle_min.get(),0)) * 0.02, 0, 1);
331 184606 : slew(MIN(settilt * max_angle_deg * (1/90.0), get_forward_flight_tilt()));
332 : }
333 : }
334 :
335 :
336 : /*
337 : output a slew limited tiltrotor angle. tilt is 0 or 1
338 : */
339 90304 : void Tiltrotor::binary_slew(bool forward)
340 : {
341 : // The servo output is binary, not slew rate limited
342 90304 : SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, forward?1000:0);
343 :
344 : // rate limiting current_tilt has the effect of delaying throttle in tiltrotor_binary_update
345 90304 : float max_change = tilt_max_change(!forward);
346 90304 : if (forward) {
347 41733 : current_tilt = constrain_float(current_tilt+max_change, 0, 1);
348 : } else {
349 48571 : current_tilt = constrain_float(current_tilt-max_change, 0, 1);
350 : }
351 90304 : }
352 :
353 : /*
354 : update motor tilt for binary tilt servos
355 : */
356 90304 : void Tiltrotor::binary_update(void)
357 : {
358 : // motors always active
359 90304 : _motors_active = true;
360 :
361 90304 : if (!quadplane.in_vtol_mode()) {
362 : // we are in pure fixed wing mode. Move the tiltable motors
363 : // all the way forward and run them as a forward motor
364 41733 : binary_slew(true);
365 :
366 41733 : float new_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)*0.01f;
367 41733 : if (current_tilt >= 1) {
368 34767 : const uint32_t mask = is_zero(new_throttle) ? 0 : tilt_mask.get();
369 : // the motors are all the way forward, start using them for fwd thrust
370 34767 : motors->output_motor_mask(new_throttle, mask, plane.rudder_dt);
371 : }
372 : } else {
373 48571 : binary_slew(false);
374 : }
375 90304 : }
376 :
377 :
378 : /*
379 : update motor tilt
380 : */
381 6498054 : void Tiltrotor::update(void)
382 : {
383 6498054 : if (!enabled() || tilt_mask == 0) {
384 : // no motors to tilt
385 5904792 : return;
386 : }
387 :
388 593262 : if (type == TILT_TYPE_BINARY) {
389 90304 : binary_update();
390 : } else {
391 502958 : continuous_update();
392 : }
393 :
394 593262 : if (type == TILT_TYPE_VECTORED_YAW) {
395 319053 : vectoring();
396 : }
397 : }
398 :
399 : #if HAL_LOGGING_ENABLED
400 : // Write tiltrotor specific log
401 205625 : void Tiltrotor::write_log()
402 : {
403 : // Only valid on a tiltrotor
404 205625 : if (!enabled()) {
405 184686 : return;
406 : }
407 :
408 20939 : struct log_tiltrotor pkt {
409 : LOG_PACKET_HEADER_INIT(LOG_TILT_MSG),
410 20939 : time_us : AP_HAL::micros64(),
411 20939 : current_tilt : current_tilt * 90.0,
412 20939 : };
413 :
414 20939 : if (type != TILT_TYPE_VECTORED_YAW) {
415 : // Left and right tilt are invalid
416 9632 : pkt.front_left_tilt = AP_Logger::quiet_nanf();
417 9632 : pkt.front_right_tilt = AP_Logger::quiet_nanf();
418 :
419 : } else {
420 : // Calculate tilt angle from servo outputs
421 11307 : const float total_angle = 90.0 + tilt_yaw_angle + fixed_angle;
422 11307 : const float scale = total_angle * 0.001;
423 11307 : pkt.front_left_tilt = (SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft) * scale) - tilt_yaw_angle;
424 11307 : pkt.front_right_tilt = (SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight) * scale) - tilt_yaw_angle;
425 : }
426 :
427 20939 : plane.logger.WriteBlock(&pkt, sizeof(pkt));
428 : }
429 : #endif
430 :
431 : /*
432 : tilt compensation for angle of tilt. When the rotors are tilted the
433 : roll effect of differential thrust on the tilted rotors is decreased
434 : and the yaw effect increased
435 : We have two factors we apply.
436 :
437 : 1) when we are transitioning to fwd flight we scale the tilted rotors by 1/cos(angle). This pushes us towards more flight speed
438 :
439 : 2) when we are transitioning to hover we scale the non-tilted rotors by cos(angle). This pushes us towards lower fwd thrust
440 :
441 : We also apply an equalisation to the tilted motors in proportion to
442 : how much tilt we have. This smoothly reduces the impact of the roll
443 : gains as we tilt further forward.
444 :
445 : For yaw, we apply differential thrust in proportion to the demanded
446 : yaw control and sin of the tilt angle
447 :
448 : Finally we ensure no requested thrust is over 1 by scaling back all
449 : motors so the largest thrust is at most 1.0
450 : */
451 303465 : void Tiltrotor::tilt_compensate_angle(float *thrust, uint8_t num_motors, float non_tilted_mul, float tilted_mul)
452 : {
453 303465 : float tilt_total = 0;
454 303465 : uint8_t tilt_count = 0;
455 :
456 : // apply tilt_factors first
457 6131921 : for (uint8_t i=0; i<num_motors; i++) {
458 5828456 : if (!is_motor_tilting(i)) {
459 4989254 : thrust[i] *= non_tilted_mul;
460 : } else {
461 839202 : thrust[i] *= tilted_mul;
462 839202 : tilt_total += thrust[i];
463 839202 : tilt_count++;
464 : }
465 : }
466 :
467 303465 : float largest_tilted = 0;
468 303465 : const float sin_tilt = sinf(radians(current_tilt*90));
469 : // yaw_gain relates the amount of differential thrust we get from
470 : // tilt, so that the scaling of the yaw control is the same at any
471 : // tilt angle
472 303465 : const float yaw_gain = sinf(radians(tilt_yaw_angle));
473 303465 : const float avg_tilt_thrust = tilt_total / tilt_count;
474 :
475 6131921 : for (uint8_t i=0; i<num_motors; i++) {
476 5828456 : if (is_motor_tilting(i)) {
477 : // as we tilt we need to reduce the impact of the roll
478 : // controller. This simple method keeps the same average,
479 : // but moves us to no roll control as the angle increases
480 839202 : thrust[i] = current_tilt * avg_tilt_thrust + thrust[i] * (1-current_tilt);
481 : // add in differential thrust for yaw control, scaled by tilt angle
482 839202 : const float diff_thrust = motors->get_roll_factor(i) * (motors->get_yaw()+motors->get_yaw_ff()) * sin_tilt * yaw_gain;
483 839202 : thrust[i] += diff_thrust;
484 839202 : largest_tilted = MAX(largest_tilted, thrust[i]);
485 : }
486 : }
487 :
488 : // if we are saturating one of the motors then reduce all motors
489 : // to keep them in proportion to the original thrust. This helps
490 : // maintain stability when tilted at a large angle
491 303465 : if (largest_tilted > 1.0f) {
492 5235 : float scale = 1.0f / largest_tilted;
493 77695 : for (uint8_t i=0; i<num_motors; i++) {
494 72460 : thrust[i] *= scale;
495 : }
496 : }
497 303465 : }
498 :
499 : /*
500 : choose up or down tilt compensation based on flight mode When going
501 : to a fixed wing mode we use tilt_compensate_down, when going to a
502 : VTOL mode we use tilt_compensate_up
503 : */
504 593262 : void Tiltrotor::tilt_compensate(float *thrust, uint8_t num_motors)
505 : {
506 593262 : if (current_tilt <= 0) {
507 : // the motors are not tilted, no compensation needed
508 289797 : return;
509 : }
510 303465 : if (quadplane.in_vtol_mode()) {
511 : // we are transitioning to VTOL flight
512 27361 : const float tilt_factor = cosf(radians(current_tilt*90));
513 27361 : tilt_compensate_angle(thrust, num_motors, tilt_factor, 1);
514 : } else {
515 : float inv_tilt_factor;
516 276104 : if (current_tilt > 0.98f) {
517 232181 : inv_tilt_factor = 1.0 / cosf(radians(0.98f*90));
518 : } else {
519 43923 : inv_tilt_factor = 1.0 / cosf(radians(current_tilt*90));
520 : }
521 276104 : tilt_compensate_angle(thrust, num_motors, 1, inv_tilt_factor);
522 : }
523 : }
524 :
525 : /*
526 : return true if the rotors are fully tilted forward
527 : */
528 3408218 : bool Tiltrotor::fully_fwd(void) const
529 : {
530 3408218 : if (!enabled() || (tilt_mask == 0)) {
531 3147010 : return false;
532 : }
533 261208 : return (current_tilt >= get_fully_forward_tilt());
534 : }
535 :
536 : /*
537 : return true if the rotors are fully tilted up
538 : */
539 0 : bool Tiltrotor::fully_up(void) const
540 : {
541 0 : if (!enabled() || (tilt_mask == 0)) {
542 0 : return false;
543 : }
544 0 : return (current_tilt <= 0);
545 : }
546 :
547 : /*
548 : control vectoring for tilt multicopters
549 : */
550 319053 : void Tiltrotor::vectoring(void)
551 : {
552 : // total angle the tilt can go through
553 319053 : const float total_angle = 90 + tilt_yaw_angle + fixed_angle;
554 : // output value (0 to 1) to get motors pointed straight up
555 319053 : const float zero_out = tilt_yaw_angle / total_angle;
556 319053 : const float fixed_tilt_limit = fixed_angle / total_angle;
557 319053 : const float level_out = 1.0 - fixed_tilt_limit;
558 :
559 : // calculate the basic tilt amount from current_tilt
560 319053 : float base_output = zero_out + (current_tilt * (level_out - zero_out));
561 : // for testing when disarmed, apply vectored yaw in proportion to rudder stick
562 : // Wait TILT_DELAY_MS after disarming to allow props to spin down first.
563 319053 : constexpr uint32_t TILT_DELAY_MS = 3000;
564 319053 : uint32_t now = AP_HAL::millis();
565 319053 : if (!plane.arming.is_armed_and_safety_off() && plane.quadplane.option_is_set(QuadPlane::Option::DISARMED_TILT)) {
566 : // this test is subject to wrapping at ~49 days, but the consequences are insignificant
567 0 : if ((now - hal.util->get_last_armed_change()) > TILT_DELAY_MS) {
568 0 : if (quadplane.in_vtol_mode()) {
569 0 : float yaw_out = plane.channel_rudder->get_control_in();
570 0 : yaw_out /= plane.channel_rudder->get_range();
571 0 : float yaw_range = zero_out;
572 :
573 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
574 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
575 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output,0,1));
576 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft, 1000 * constrain_float(base_output + yaw_out * yaw_range,0,1));
577 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight, 1000 * constrain_float(base_output - yaw_out * yaw_range,0,1));
578 : } else {
579 : // fixed wing tilt
580 0 : const float gain = fixed_gain * fixed_tilt_limit;
581 : // base the tilt on elevon mixing, which means it
582 : // takes account of the MIXING_GAIN. The rear tilt is
583 : // based on elevator
584 0 : const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) * (1/4500.0);
585 0 : const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) * (1/4500.0);
586 0 : const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (1/4500.0);
587 : // front tilt is effective canards, so need to swap and use negative. Rear motors are treated live elevons.
588 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1));
589 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1));
590 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft,1000 * constrain_float(base_output + left,0,1));
591 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight,1000 * constrain_float(base_output + right,0,1));
592 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output + mid,0,1));
593 : }
594 : }
595 0 : return;
596 : }
597 :
598 319053 : const bool no_yaw = tilt_over_max_angle();
599 319053 : if (no_yaw) {
600 : // fixed wing We need to apply inverse scaling with throttle, and remove the surface speed scaling as
601 : // we don't want tilt impacted by airspeed
602 128430 : const float scaler = plane.control_mode == &plane.mode_manual?1:(quadplane.FW_vector_throttle_scaling() / plane.get_speed_scaler());
603 128430 : const float gain = fixed_gain * fixed_tilt_limit * scaler;
604 128430 : const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) * (1/4500.0);
605 128430 : const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) * (1/4500.0);
606 128430 : const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (1/4500.0);
607 128430 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1));
608 128430 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1));
609 128430 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft,1000 * constrain_float(base_output + left,0,1));
610 128430 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight,1000 * constrain_float(base_output + right,0,1));
611 128430 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000 * constrain_float(base_output + mid,0,1));
612 : } else {
613 190623 : const float yaw_out = motors->get_yaw()+motors->get_yaw_ff();
614 : // the MotorsMatrix library normalises roll factor to 0.5, so
615 : // we need to use the same factor here to keep the same roll
616 : // gains when tilted as we have when not tilted
617 190623 : const float roll_out = (motors->get_roll()+motors->get_roll_ff()) * 0.5;
618 190623 : const float yaw_range = zero_out;
619 :
620 : // Scaling yaw with throttle
621 190623 : const float throttle = motors->get_throttle_out();
622 190623 : const float scale_min = 0.5;
623 190623 : const float scale_max = 2.0;
624 190623 : float throttle_scaler = scale_max;
625 190623 : if (is_positive(throttle)) {
626 139241 : throttle_scaler = constrain_float(motors->get_throttle_hover() / throttle, scale_min, scale_max);
627 : }
628 :
629 : // now apply vectored thrust for yaw and roll.
630 190623 : const float tilt_rad = radians(current_tilt*90);
631 190623 : const float sin_tilt = sinf(tilt_rad);
632 190623 : const float cos_tilt = cosf(tilt_rad);
633 190623 : float tilt_scale = throttle_scaler * (yaw_out * cos_tilt - roll_out * sin_tilt);
634 :
635 190623 : if (fabsf(tilt_scale) > 1.0) {
636 0 : tilt_scale = constrain_float(tilt_scale, -1.0, 1.0);
637 0 : motors->limit.yaw = true;
638 : }
639 :
640 190623 : const float tilt_offset = tilt_scale * yaw_range;
641 :
642 190623 : float left_tilt = base_output + tilt_offset;
643 190623 : float right_tilt = base_output - tilt_offset;
644 :
645 : // if output saturation of both left and right then set yaw limit flag
646 190623 : if (((left_tilt > 1.0) || (left_tilt < 0.0)) &&
647 0 : ((right_tilt > 1.0) || (right_tilt < 0.0))) {
648 0 : motors->limit.yaw = true;
649 : }
650 :
651 : // constrain and scale to output range
652 190623 : left_tilt = constrain_float(left_tilt,0.0,1.0) * 1000.0;
653 190623 : right_tilt = constrain_float(right_tilt,0.0,1.0) * 1000.0;
654 :
655 190623 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, left_tilt);
656 190623 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, right_tilt);
657 190623 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, 1000.0 * constrain_float(base_output,0.0,1.0));
658 190623 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft, left_tilt);
659 190623 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight, right_tilt);
660 : }
661 : }
662 :
663 : /*
664 : control bicopter tiltrotors
665 : */
666 8438671 : void Tiltrotor::bicopter_output(void)
667 : {
668 8438671 : if (type != TILT_TYPE_BICOPTER || quadplane.motor_test.running) {
669 : // don't override motor test with motors_output
670 8438671 : return;
671 : }
672 :
673 0 : if (!quadplane.in_vtol_mode() && fully_fwd()) {
674 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, -SERVO_MAX);
675 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, -SERVO_MAX);
676 0 : return;
677 : }
678 :
679 0 : float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
680 0 : if (quadplane.assisted_flight) {
681 0 : quadplane.hold_stabilize(throttle * 0.01f);
682 0 : quadplane.motors_output(true);
683 : } else {
684 0 : quadplane.motors_output(false);
685 : }
686 :
687 : // bicopter assumes that trim is up so we scale down so match
688 0 : float tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);
689 0 : float tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight);
690 :
691 0 : if (is_negative(tilt_left)) {
692 0 : tilt_left *= tilt_yaw_angle * (1/90.0);
693 : }
694 0 : if (is_negative(tilt_right)) {
695 0 : tilt_right *= tilt_yaw_angle * (1/90.0);
696 : }
697 :
698 : // reduce authority of bicopter as motors are tilted forwards
699 0 : const float scaling = cosf(current_tilt * M_PI_2);
700 0 : tilt_left *= scaling;
701 0 : tilt_right *= scaling;
702 :
703 : // add current tilt and constrain
704 0 : tilt_left = constrain_float(-(current_tilt * SERVO_MAX) + tilt_left, -SERVO_MAX, SERVO_MAX);
705 0 : tilt_right = constrain_float(-(current_tilt * SERVO_MAX) + tilt_right, -SERVO_MAX, SERVO_MAX);
706 :
707 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left);
708 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right);
709 : }
710 :
711 : /*
712 : when doing a forward transition of a tilt-vectored quadplane we use
713 : euler angle control to maintain good yaw. This updates the yaw
714 : target based on pilot input and target roll
715 : */
716 23061 : void Tiltrotor::update_yaw_target(void)
717 : {
718 23061 : uint32_t now = AP_HAL::millis();
719 46111 : if (now - transition_yaw_set_ms > 100 ||
720 23050 : !is_zero(quadplane.get_pilot_input_yaw_rate_cds())) {
721 : // lock initial yaw when transition is started or when
722 : // pilot commands a yaw change. This allows us to track
723 : // straight in transitions for tilt-vectored planes, but
724 : // allows for turns when level transition is not wanted
725 11 : transition_yaw_cd = quadplane.ahrs.yaw_sensor;
726 : }
727 :
728 : /*
729 : now calculate the equivalent yaw rate for a coordinated turn for
730 : the desired bank angle given the airspeed
731 : */
732 : float aspeed;
733 23061 : bool have_airspeed = quadplane.ahrs.airspeed_EAS(aspeed);
734 23061 : if (have_airspeed && labs(plane.nav_roll_cd)>1000) {
735 6790 : float dt = (now - transition_yaw_set_ms) * 0.001;
736 : // calculate the yaw rate to achieve the desired turn rate
737 6790 : const float airspeed_min = MAX(plane.aparm.airspeed_min,5);
738 6790 : const float yaw_rate_cds = fixedwing_turn_rate(plane.nav_roll_cd*0.01, MAX(aspeed,airspeed_min))*100;
739 6790 : transition_yaw_cd += yaw_rate_cds * dt;
740 : }
741 23061 : transition_yaw_set_ms = now;
742 23061 : }
743 :
744 :
745 : /*
746 : control use of multirotor rate control in forward transition
747 : */
748 100595 : bool Tiltrotor_Transition::use_multirotor_control_in_fwd_transition() const
749 : {
750 100595 : if (!tiltrotor.is_vectored()) {
751 41572 : return false;
752 : }
753 59023 : switch (transition_state) {
754 59023 : case State::AIRSPEED_WAIT:
755 : case State::TIMER:
756 59023 : return true;
757 0 : case State::DONE:
758 0 : return false;
759 : }
760 0 : return false;
761 : }
762 :
763 38761 : bool Tiltrotor_Transition::update_yaw_target(float& yaw_target_cd)
764 : {
765 38761 : if (!use_multirotor_control_in_fwd_transition()) {
766 15700 : return false;
767 : }
768 23061 : tiltrotor.update_yaw_target();
769 23061 : yaw_target_cd = tiltrotor.transition_yaw_cd;
770 23061 : return true;
771 : }
772 :
773 : // return true if we should show VTOL view
774 370525 : bool Tiltrotor_Transition::show_vtol_view() const
775 : {
776 370525 : bool show_vtol = quadplane.in_vtol_mode();
777 :
778 370525 : if (!show_vtol && tiltrotor.is_vectored() && transition_state <= State::TIMER) {
779 : // we use multirotor controls during fwd transition for
780 : // vectored yaw vehicles
781 27536 : return true;
782 : }
783 :
784 342989 : return show_vtol;
785 : }
786 :
787 : // Return true if forward throttle should be allowed for position control, see Q_FWD_THR_USE
788 914714 : bool Tiltrotor_Transition::allow_vfwd() const
789 : {
790 : // Don't allow forward throttle (tilt) if a tilting motor has failed which would result in a yaw imbalance
791 :
792 : // No resultant yaw imbalance if not vectored
793 914714 : if (!tiltrotor.is_vectored()) {
794 401228 : return true;
795 : }
796 :
797 : // No failed motor
798 513486 : if (!motors->get_thrust_boost()) {
799 485163 : return true;
800 : }
801 :
802 : // Get index of failed motor
803 28323 : const uint8_t lost_motor = motors->get_lost_motor();
804 :
805 : // Failed motor is not tilting
806 28323 : if (!tiltrotor.is_motor_tilting(lost_motor)) {
807 10824 : return true;
808 : }
809 :
810 : // Failed tilting motor is on the center line, so will not cause a yaw imbalance
811 17499 : if (is_zero(motors->get_roll_factor(lost_motor))) {
812 0 : return true;
813 : }
814 :
815 : // Disable forward throttle (tilt)
816 17499 : return false;
817 : }
818 :
819 : // return true if we are tilted over the max angle threshold
820 322433 : bool Tiltrotor::tilt_over_max_angle(void) const
821 : {
822 322433 : const float tilt_threshold = (max_angle_deg/90.0f);
823 322433 : return (current_tilt > MIN(tilt_threshold, get_forward_flight_tilt()));
824 : }
825 :
826 : // throttle of forward flight motors including any tilting motors
827 125 : bool Tiltrotor::get_forward_throttle(float &throttle) const
828 : {
829 125 : if (!enabled() || !_is_vectored) {
830 118 : return false;
831 : }
832 7 : const float throttle_range = motors->thr_lin.get_spin_max() - motors->thr_lin.get_spin_min();
833 7 : if (!is_positive(throttle_range)) {
834 0 : return false;
835 : }
836 7 : float throttle_sum = 0.0f;
837 7 : uint8_t num_vectored_motors = 0;
838 231 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; ++i) {
839 224 : if (is_motor_tilting(i)) {
840 : float thrust;
841 20 : if (motors->get_thrust(i, thrust)) {
842 20 : throttle_sum += (motors->thr_lin.thrust_to_actuator(thrust) - motors->thr_lin.get_spin_min()) / throttle_range;
843 20 : num_vectored_motors ++;
844 : }
845 : }
846 : }
847 7 : if (num_vectored_motors > 0) {
848 7 : throttle = throttle_sum / (float)num_vectored_motors;
849 7 : return true;
850 : }
851 0 : return false;
852 : }
853 :
854 : #endif // HAL_QUADPLANE_ENABLED
|
