Line data Source code
1 : #pragma once
2 :
3 : #include <AP_HAL/AP_HAL_Boards.h>
4 :
5 : #ifndef HAL_QUADPLANE_ENABLED
6 : #define HAL_QUADPLANE_ENABLED 1
7 : #endif
8 :
9 : #if HAL_QUADPLANE_ENABLED
10 :
11 : #include <AP_Motors/AP_Motors.h>
12 : #include <AC_PID/AC_PID.h>
13 : #include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
14 : #include <AC_AttitudeControl/AC_CommandModel.h>
15 : #include <AP_InertialNav/AP_InertialNav.h>
16 : #include <AC_AttitudeControl/AC_PosControl.h>
17 : #include <AC_AttitudeControl/AC_WeatherVane.h>
18 : #include <AC_WPNav/AC_WPNav.h>
19 : #include <AC_WPNav/AC_Loiter.h>
20 : #include <AC_Avoidance/AC_Avoid.h>
21 : #include <AP_Logger/LogStructure.h>
22 : #include <AP_Mission/AP_Mission.h>
23 : #include <AP_Proximity/AP_Proximity.h>
24 : #include "qautotune.h"
25 : #include "defines.h"
26 : #include "tailsitter.h"
27 : #include "tiltrotor.h"
28 : #include "transition.h"
29 : #include "VTOL_Assist.h"
30 :
31 : /*
32 : QuadPlane specific functionality
33 : */
34 : class QuadPlane
35 : {
36 : public:
37 : friend class Plane;
38 : friend class AP_Tuning_Plane;
39 : friend class GCS_MAVLINK_Plane;
40 : friend class AP_AdvancedFailsafe_Plane;
41 : friend class QAutoTune;
42 : friend class AP_Arming_Plane;
43 : friend class RC_Channel_Plane;
44 : friend class RC_Channel;
45 : friend class Tailsitter;
46 : friend class Tiltrotor;
47 : friend class SLT_Transition;
48 : friend class Tailsitter_Transition;
49 : friend class VTOL_Assist;
50 :
51 : friend class Mode;
52 : friend class ModeManual;
53 : friend class ModeAuto;
54 : friend class ModeRTL;
55 : friend class ModeAvoidADSB;
56 : friend class ModeGuided;
57 : friend class ModeQHover;
58 : friend class ModeQLand;
59 : friend class ModeQLoiter;
60 : friend class ModeQRTL;
61 : friend class ModeQStabilize;
62 : friend class ModeQAutotune;
63 : friend class ModeQAcro;
64 : friend class ModeLoiterAltQLand;
65 : friend class ModeAutoLand;
66 : friend class AP_SystemID;
67 :
68 : QuadPlane(AP_AHRS &_ahrs);
69 :
70 920 : static QuadPlane *get_singleton() {
71 920 : return _singleton;
72 : }
73 :
74 : // var_info for holding Parameter information
75 : static const struct AP_Param::GroupInfo var_info[];
76 : static const struct AP_Param::GroupInfo var_info2[];
77 :
78 : void control_auto(void);
79 : bool setup(void);
80 :
81 : enum class ThrustType : uint8_t {
82 : SLT=0, // Traditional quadplane, with a pusher motor and independent multicopter lift.
83 : TAILSITTER,
84 : TILTROTOR,
85 : };
86 467918 : ThrustType get_thrust_type(void) {return thrust_type;}
87 :
88 : void vtol_position_controller(void);
89 : void setup_target_position(void);
90 : void takeoff_controller(void);
91 : void waypoint_controller(void);
92 : void update_land_positioning(void);
93 :
94 : void update_throttle_mix(void);
95 :
96 : // update transition handling
97 : void update(void);
98 :
99 : // set motor arming
100 : void set_armed(bool armed);
101 :
102 : // is VTOL available?
103 446321166 : bool available(void) const {
104 446321166 : return initialised;
105 : }
106 :
107 : // is quadplane assisting?
108 9740534 : bool in_assisted_flight(void) const {
109 9740534 : return available() && assisted_flight;
110 : }
111 :
112 : // abort landing, only valid when in a VTOL landing descent
113 : bool abort_landing(void);
114 :
115 : bool in_frwd_transition(void) const;
116 :
117 : bool handle_do_vtol_transition(enum MAV_VTOL_STATE state) const;
118 :
119 : bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
120 : bool do_vtol_land(const AP_Mission::Mission_Command& cmd);
121 : bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
122 : bool verify_vtol_land(void);
123 : bool in_vtol_auto(void) const;
124 : bool in_vtol_mode(void) const;
125 : bool in_vtol_takeoff(void) const;
126 : bool in_vtol_posvel_mode(void) const;
127 : void update_throttle_hover();
128 : bool show_vtol_view() const;
129 :
130 : // vtol help for is_flying()
131 : bool is_flying(void);
132 :
133 : // return desired forward throttle percentage
134 : float forward_throttle_pct();
135 : float get_weathervane_yaw_rate_cds(void);
136 :
137 : // see if we are flying from vtol point of view
138 : bool is_flying_vtol(void) const;
139 :
140 : // user initiated takeoff for guided mode
141 : bool do_user_takeoff(float takeoff_altitude);
142 :
143 : // return true if the wp_nav controller is being updated
144 : bool using_wp_nav(void) const;
145 :
146 : // return true if the user has set ENABLE
147 53694 : bool enabled(void) const { return enable != 0; }
148 :
149 : // is throttle controlled landing descent active?
150 : bool thr_ctrl_land;
151 :
152 : uint16_t get_pilot_velocity_z_max_dn_m() const;
153 :
154 : struct PACKED log_QControl_Tuning {
155 : LOG_PACKET_HEADER;
156 : uint64_t time_us;
157 : float throttle_in;
158 : float angle_boost;
159 : float throttle_out;
160 : float throttle_hover;
161 : float desired_alt;
162 : float inav_alt;
163 : int32_t baro_alt;
164 : int16_t target_climb_rate;
165 : int16_t climb_rate;
166 : float throttle_mix;
167 : uint8_t transition_state;
168 : uint8_t assist;
169 : };
170 :
171 : MAV_TYPE get_mav_type(void) const;
172 :
173 : // called when we change mode (for any mode, not just Q modes)
174 : void mode_enter(void);
175 :
176 : // Check if servo auto trim is allowed
177 : bool allow_servo_auto_trim();
178 :
179 : /*
180 : are we in the descent phase of a VTOL landing?
181 : */
182 : bool in_vtol_land_descent(void) const;
183 :
184 : // Should we allow stick mixing from the pilot
185 : bool allow_stick_mixing() const;
186 :
187 : /*
188 : should we disable the TECS controller?
189 : only called when in an auto-throttle mode
190 : */
191 : bool should_disable_TECS() const;
192 :
193 : // Get pilot throttle input with deadzone, this will return 50% throttle in failsafe!
194 : float get_throttle_input() const;
195 :
196 : void Log_Write_AttRate();
197 :
198 : private:
199 : AP_AHRS &ahrs;
200 :
201 : AP_InertialNav inertial_nav{ahrs};
202 :
203 : AP_Enum<AP_Motors::motor_frame_class> frame_class;
204 : AP_Enum<AP_Motors::motor_frame_type> frame_type;
205 :
206 : // Types of different "quadplane" configurations.
207 : ThrustType thrust_type;
208 :
209 : // Initialise motors to allow passing it to tailsitter in its constructor
210 : AP_MotorsMulticopter *motors = nullptr;
211 : const struct AP_Param::GroupInfo *motors_var_info;
212 :
213 : AC_AttitudeControl_Multi *attitude_control;
214 : AC_PosControl *pos_control;
215 : AC_WPNav *wp_nav;
216 : AC_Loiter *loiter_nav;
217 :
218 : // maximum vertical velocity the pilot may request
219 : AP_Float pilot_speed_z_max_up_ms;
220 : AP_Float pilot_speed_z_max_dn_ms;
221 :
222 : // vertical acceleration the pilot may request
223 : AP_Float pilot_accel_z_mss;
224 :
225 : // air mode state: OFF, ON, ASSISTED_FLIGHT_ONLY
226 : AirMode air_mode;
227 :
228 : // Command model parameter class
229 : // Default max rate, default expo, default time constant
230 : AC_CommandModel command_model_pilot{100.0, 0.25, 0.25};
231 : // helper functions to set and disable time constant from command model
232 : void set_pilot_yaw_rate_time_constant();
233 : void disable_yaw_rate_time_constant();
234 :
235 : // return true if airmode should be active
236 : bool air_mode_active() const;
237 :
238 : // hold hover (for transition)
239 : void hold_hover(float target_climb_rate_cms);
240 :
241 : // hold stabilize (for transition)
242 : void hold_stabilize(float throttle_in);
243 :
244 : // set climb rate in position controller
245 : void set_climb_rate_ms(float target_climb_rate_ms);
246 :
247 : // get pilot desired yaw rate in cd/s
248 : float get_pilot_input_yaw_rate_cds(void) const;
249 :
250 : // get overall desired yaw rate in cd/s
251 : float get_desired_yaw_rate_cds(bool weathervane=true);
252 :
253 : // get desired climb rate in cm/s
254 : float get_pilot_desired_climb_rate_cms(void) const;
255 :
256 : // get pilot lean angle
257 : void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const;
258 :
259 : // get pilot throttle in for landing code. Return value on scale of 0 to 1
260 : float get_pilot_land_throttle(void) const;
261 :
262 : // initialise throttle_wait when entering mode
263 : void init_throttle_wait();
264 :
265 : // use multicopter rate controller
266 : void multicopter_attitude_rate_update(float yaw_rate_cds);
267 :
268 : float get_pilot_throttle(void);
269 : void control_hover(void);
270 : void relax_attitude_control();
271 :
272 : bool check_land_complete(void);
273 : bool land_detector(uint32_t timeout_ms);
274 : bool check_land_final(void);
275 :
276 : float assist_climb_rate_cms(void) const;
277 :
278 : // calculate desired yaw rate for assistance
279 : float desired_auto_yaw_rate_cds(bool body_frame = false) const;
280 :
281 : bool should_relax(void);
282 : void motors_output(bool run_rate_controller = true);
283 : void Log_Write_QControl_Tuning();
284 : void log_QPOS(void);
285 : float landing_descent_rate_ms(float height_above_ground_m);
286 :
287 : // setup correct aux channels for frame class
288 : void setup_default_channels(uint8_t num_motors);
289 :
290 : void guided_start(void);
291 : void guided_update(void);
292 :
293 : void update_throttle_suppression(void);
294 :
295 : void run_z_controller(void);
296 : void run_xy_controller(float accel_limit_mss = 0.0);
297 :
298 : void setup_defaults(void);
299 :
300 : // calculate a stopping distance for fixed-wing to vtol transitions
301 : float stopping_distance_m(float ground_speed_squared_m) const;
302 : float accel_needed(float stop_distance, float ground_speed_squared) const;
303 : float stopping_distance_m(void);
304 :
305 : // distance below which we don't do approach, based on stopping
306 : // distance for cruise speed
307 : float transition_threshold_m(void);
308 :
309 : AP_Int16 transition_time_ms;
310 : AP_Int16 back_trans_pitch_limit_ms;
311 :
312 : // transition deceleration, m/s/s
313 : AP_Float transition_decel_mss;
314 :
315 : // transition failure handling
316 : struct TRANS_FAIL {
317 : enum ACTION {
318 : QLAND,
319 : QRTL
320 : };
321 : AP_Int16 timeout;
322 : AP_Enum<ACTION> action;
323 : bool warned;
324 : } transition_failure;
325 :
326 :
327 : // Quadplane trim, degrees
328 : AP_Float ahrs_trim_pitch;
329 : float _last_ahrs_trim_pitch;
330 :
331 : // fw landing approach radius
332 : AP_Float fw_land_approach_radius_m;
333 :
334 : AP_Int16 rc_speed;
335 :
336 : // VTOL assistance in a forward flight mode
337 : VTOL_Assist assist {*this};
338 :
339 : // landing speed in m/s
340 : AP_Float land_final_speed_ms;
341 :
342 : // QRTL start altitude, meters
343 : AP_Int16 qrtl_alt_m;
344 : AP_Int16 qrtl_alt_min_m;
345 :
346 : // alt to switch to QLAND_FINAL
347 : AP_Float land_final_alt_m;
348 : AP_Float vel_forward_alt_cutoff_m;
349 :
350 : AP_Int8 enable;
351 : AP_Int8 transition_pitch_max;
352 :
353 : // control if a VTOL RTL will be used
354 : AP_Int8 rtl_mode;
355 : enum RTL_MODE{
356 : NONE,
357 : SWITCH_QRTL,
358 : VTOL_APPROACH_QRTL,
359 : QRTL_ALWAYS,
360 : };
361 :
362 : // control if a VTOL GUIDED will be used
363 : AP_Int8 guided_mode;
364 :
365 : // control ESC throttle calibration
366 : AP_Int8 esc_calibration;
367 : void run_esc_calibration(void);
368 :
369 : // ICEngine control on landing
370 : AP_Int8 land_icengine_cut;
371 :
372 : // HEARTBEAT mav_type override
373 : AP_Int8 mav_type;
374 :
375 : // manual throttle curve expo strength
376 : AP_Float throttle_expo;
377 :
378 : // manual forward throttle input
379 : AP_Float fwd_thr_max;
380 : RC_Channel *rc_fwd_thr_ch;
381 :
382 : // QACRO mode max roll/pitch/yaw rates
383 : AP_Float acro_roll_rate;
384 : AP_Float acro_pitch_rate;
385 : AP_Float acro_yaw_rate;
386 :
387 : // gain from forward acceleration to forward throttle
388 : AP_Float q_fwd_thr_gain;
389 :
390 : // limit applied to forward pitch to prevent wing producing negative lift
391 : AP_Float q_fwd_pitch_lim;
392 :
393 : // limit applied to back pitch to prevent wing producing excessive lift
394 : AP_Float q_bck_pitch_lim;
395 :
396 : // which fwd throttle handling method is active
397 : enum class ActiveFwdThr : uint8_t {
398 : NONE = 0,
399 : OLD = 1,
400 : NEW = 2,
401 : };
402 : // override with AUX function
403 : bool vfwd_enable_active;
404 :
405 : // specifies when the feature controlled by q_fwd_thr_gain and q_fwd_pitch_lim is used
406 : enum class FwdThrUse : uint8_t {
407 : OFF = 0,
408 : POSCTRL = 1,
409 : ALL = 2,
410 : };
411 : AP_Enum<FwdThrUse> q_fwd_thr_use;
412 :
413 : // return which vfwd method to use
414 : ActiveFwdThr get_vfwd_method(void) const;
415 :
416 : struct {
417 : AP_Float gain;
418 : float integrator;
419 : uint32_t last_ms;
420 : float last_pct;
421 : } vel_forward;
422 :
423 : AC_WeatherVane *weathervane;
424 :
425 : bool initialised;
426 :
427 : Location last_auto_target;
428 :
429 : float q_fwd_throttle; // forward throttle used in q modes
430 : float q_fwd_pitch_lim_cd; // forward pitch limit applied when using q_fwd_throttle
431 : float q_bck_pitch_lim_cd; // backward pitch limit applied when using Q_BCK_PIT_LIM
432 : uint32_t q_pitch_limit_update_ms; // last time the backward pitch limit was updated
433 :
434 : // when did we last run the attitude controller?
435 : uint32_t last_att_control_ms;
436 :
437 : // transition logic
438 : Transition *transition = nullptr;
439 :
440 : // true when waiting for pilot throttle
441 : bool throttle_wait;
442 :
443 : // true when quad is assisting a fixed wing mode
444 : bool assisted_flight;
445 :
446 : // are we in a guided takeoff?
447 : bool guided_takeoff;
448 :
449 : /* if we arm in guided mode when we arm then go into a "waiting
450 : for takeoff command" state. In this state we are waiting for
451 : one of the following:
452 :
453 : 1) disarm
454 : 2) guided takeoff command
455 : 3) change to AUTO with a takeoff waypoint as first nav waypoint
456 : 4) change to another mode
457 :
458 : while in this state we don't go to throttle unlimited, and will
459 : refuse a change to AUTO mode if the first waypoint is not a
460 : takeoff. If we try to switch to RTL then we will instead use
461 : QLAND
462 :
463 : This state is needed to cope with the takeoff sequence used
464 : by QGC on common controllers such as the MX16, which do this on a "takeoff" swipe:
465 :
466 : - changes mode to GUIDED
467 : - arms
468 : - changes mode to AUTO
469 : */
470 : bool guided_wait_takeoff;
471 : bool guided_wait_takeoff_on_mode_enter;
472 :
473 : struct {
474 : // time when motors reached lower limit
475 : uint32_t lower_limit_start_ms;
476 : uint32_t land_start_ms;
477 : float vpos_start_m;
478 :
479 : // landing detection threshold in meters
480 : AP_Float detect_alt_change_m;
481 : } landing_detect;
482 :
483 : // throttle mix acceleration filter
484 692 : LowPassFilterVector3f throttle_mix_accel_ef_filter{1.0};
485 :
486 : // time we last set the loiter target
487 : uint32_t last_loiter_ms;
488 :
489 : enum position_control_state {
490 : QPOS_NONE = 0,
491 : QPOS_APPROACH,
492 : QPOS_AIRBRAKE,
493 : QPOS_POSITION1,
494 : QPOS_POSITION2,
495 : QPOS_LAND_DESCEND,
496 : QPOS_LAND_ABORT,
497 : QPOS_LAND_FINAL,
498 : QPOS_LAND_COMPLETE
499 : };
500 : class PosControlState {
501 : public:
502 219100642 : enum position_control_state get_state() const {
503 219100642 : return state;
504 : }
505 : void set_state(enum position_control_state s);
506 25979 : uint32_t time_since_state_start_ms() const {
507 25979 : return AP_HAL::millis() - last_state_change_ms;
508 : }
509 : Vector3p target_ned_m;
510 : Vector2f correction_ne_m;
511 : Vector3f target_vel_ms;
512 : bool slow_descent;
513 : bool pilot_correction_active;
514 : bool pilot_correction_done;
515 : uint32_t thrust_loss_start_ms;
516 : uint32_t last_log_ms;
517 : bool reached_wp_speed;
518 : uint32_t last_run_ms;
519 : float pos1_speed_limit_ms;
520 : bool done_accel_init;
521 : Vector2f velocity_match_ms;
522 : uint32_t last_velocity_match_ms;
523 : float target_speed_ms;
524 : float target_accel_mss;
525 : uint16_t ahrs_position_NE_reset_count;
526 : bool overshoot;
527 :
528 : float override_descent_rate_ms;
529 : uint32_t last_override_descent_ms;
530 : private:
531 : uint32_t last_state_change_ms;
532 : enum position_control_state state;
533 : } poscontrol;
534 :
535 : struct {
536 : bool running;
537 : uint32_t start_ms; // system time the motor test began
538 : uint32_t timeout_ms = 0; // test will timeout this many milliseconds after the motor_test_start_ms
539 : uint8_t seq = 0; // motor sequence number of motor being tested
540 : uint8_t throttle_type = 0; // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
541 : uint16_t throttle_value = 0; // throttle to be sent to motor, value depends upon it's type
542 : uint8_t motor_count; // number of motors to cycle
543 : } motor_test;
544 :
545 : // time of last MOTB log message
546 : uint32_t last_motb_log_ms;
547 :
548 : // time of last QTUN log message
549 : uint32_t last_qtun_log_ms;
550 :
551 : // Tiltrotor control
552 : Tiltrotor tiltrotor{*this, motors};
553 :
554 : // tailsitter control
555 : Tailsitter tailsitter{*this, motors};
556 :
557 : // the attitude view of the VTOL attitude controller
558 : AP_AHRS_View *ahrs_view;
559 :
560 : // time when motors were last active
561 : uint32_t last_motors_active_ms;
562 :
563 : // time when we last ran the vertical accel controller
564 : uint32_t last_pidz_active_ms;
565 : uint32_t last_pidz_init_ms;
566 :
567 : // throttle scaling for vectored motors in FW flight
568 : float FW_vector_throttle_scaling(void);
569 :
570 : void afs_terminate(void);
571 : bool guided_mode_enabled(void);
572 :
573 : // set altitude target to current altitude
574 : void set_alt_target_current(void);
575 :
576 : // additional options
577 : AP_Int32 options;
578 : enum class Option {
579 : LEVEL_TRANSITION=(1<<0),
580 : ALLOW_FW_TAKEOFF=(1<<1),
581 : ALLOW_FW_LAND=(1<<2),
582 : RESPECT_TAKEOFF_FRAME=(1<<3),
583 : MISSION_LAND_FW_APPROACH=(1<<4),
584 : FS_QRTL=(1<<5),
585 : IDLE_GOV_MANUAL=(1<<6),
586 : Q_ASSIST_FORCE_ENABLE=(1<<7),
587 : TAILSIT_Q_ASSIST_MOTORS_ONLY=(1<<8),
588 : AIRMODE_UNUSED=(1<<9),
589 : DISARMED_TILT=(1<<10),
590 : DELAY_ARMING=(1<<11),
591 : DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12),
592 : DISABLE_GROUND_EFFECT_COMP=(1<<13),
593 : INGORE_FW_ANGLE_LIMITS_IN_Q_MODES=(1<<14),
594 : THR_LANDING_CONTROL=(1<<15),
595 : DISABLE_APPROACH=(1<<16),
596 : REPOSITION_LANDING=(1<<17),
597 : ONLY_ARM_IN_QMODE_OR_AUTO=(1<<18),
598 : TRANS_FAIL_TO_FW=(1<<19),
599 : FS_RTL=(1<<20),
600 : DISARMED_TILT_UP=(1<<21),
601 : SCALE_FF_ANGLE_P=(1<<22),
602 : };
603 67662348 : bool option_is_set(Option option) const {
604 67662348 : return (options.get() & int32_t(option)) != 0;
605 : }
606 :
607 : // minimum distance to be from destination to use approach logic
608 : AP_Float approach_distance_m;
609 :
610 : AP_Float takeoff_failure_scalar;
611 : AP_Float maximum_takeoff_airspeed_ms;
612 : uint32_t takeoff_start_time_ms;
613 : uint32_t takeoff_time_limit_ms;
614 :
615 : float last_land_final_agl_m;
616 :
617 : // AHRS alt for land abort and package place, meters
618 : float land_descend_start_alt_m;
619 :
620 : #if HAL_WITH_ESC_TELEM
621 : // optionally block auto takeoff until all motors are spinning as expected
622 : AP_Int16 takeoff_rpm_min;
623 : AP_Int16 takeoff_rpm_max;
624 : #endif
625 :
626 : // min alt for navigation in takeoff
627 : AP_Float takeoff_navalt_min_m;
628 : uint32_t takeoff_last_run_ms;
629 : float takeoff_start_alt_m;
630 :
631 : // should we force use of fixed wing controller for attitude upset recovery?
632 : bool force_fw_control_recovery;
633 :
634 : // are we in spin recovery?
635 : bool in_spin_recovery;
636 :
637 : /*
638 : return true if current mission item is a vtol takeoff
639 : */
640 : bool is_vtol_takeoff(uint16_t id) const;
641 :
642 : /*
643 : return true if current mission item is a vtol landing
644 : */
645 : bool is_vtol_land(uint16_t id) const;
646 :
647 : #if QAUTOTUNE_ENABLED
648 : // qautotune mode
649 : QAutoTune qautotune;
650 : #endif
651 :
652 : /*
653 : are we in the approach phase of a VTOL landing?
654 : */
655 : bool in_vtol_land_approach(void) const;
656 :
657 : /*
658 : are we in the final landing phase of a VTOL landing?
659 : */
660 : bool in_vtol_land_final(void) const;
661 :
662 : /*
663 : are we in any of the phases of a VTOL landing?
664 : */
665 : bool in_vtol_land_sequence(void) const;
666 :
667 : /*
668 : see if we are in the VTOL position control phase of a landing
669 : */
670 : bool in_vtol_land_poscontrol(void) const;
671 :
672 : /*
673 : are we in the airbrake phase of a VTOL landing?
674 : */
675 : bool in_vtol_airbrake(void) const;
676 :
677 : // returns true if the vehicle should currently be doing a spiral landing
678 : bool landing_with_fixed_wing_spiral_approach(void) const;
679 :
680 : /*
681 : return true if we should use the fixed wing attitude control loop
682 : */
683 : bool use_fw_attitude_controllers(void) const;
684 :
685 : /*
686 : get the airspeed for landing approach
687 : */
688 : float get_land_airspeed_ms(void);
689 :
690 : /*
691 : setup for landing approach
692 : */
693 : void poscontrol_init_approach(void);
694 :
695 : /*
696 : calculate our closing velocity vector on the landing
697 : point. Takes account of the landing point having a velocity
698 : */
699 : Vector2f landing_closing_velocity_NE_ms();
700 :
701 : /*
702 : calculate our desired closing velocity vector on the landing point.
703 : */
704 : Vector2f landing_desired_closing_velocity_NE_ms();
705 :
706 : /*
707 : change spool state, providing easy hook for catching changes in debug
708 : */
709 : void set_desired_spool_state(AP_Motors::DesiredSpoolState state);
710 :
711 : /*
712 : limit forward pitch demand if using rotor tilt or forward flight motor to provide forward acceleration.
713 : */
714 : void assign_tilt_to_fwd_thr(void);
715 :
716 : /*
717 : get a scaled Q_WP_SPD based on direction of movement
718 : */
719 : float get_scaled_wp_speed(float target_bearing_deg) const;
720 :
721 : /*
722 : setup scaling of roll and pitch angle P gains to match fixed wing gains
723 : */
724 : void setup_rp_fw_angle_gains(void);
725 :
726 : /*
727 : return true if forward throttle from forward_throttle_pct() should be used
728 : */
729 : bool allow_forward_throttle_in_vtol_mode() const;
730 :
731 : public:
732 : void motor_test_output();
733 : MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,
734 : uint16_t throttle_value, float timeout_sec,
735 : uint8_t motor_count);
736 : private:
737 : void motor_test_stop();
738 :
739 : // check for loss of thrust and trigger thrust boost in motors library
740 : void thrust_loss_check(bool reset);
741 :
742 : // Thrust loss variables
743 : struct ThrustLoss {
744 : // number of iterations vehicle may have lost thrust
745 : uint16_t counter;
746 :
747 : // Options parameter and helper
748 : AP_Int32 options;
749 : enum class Option {
750 : DISABLED = (1<<0),
751 : VTOL_ONLY = (1<<1),
752 : };
753 4596542 : bool option_is_set(Option option) const {
754 4596542 : return (options.get() & int32_t(option)) != 0;
755 : }
756 : } thrust_loss;
757 :
758 : static QuadPlane *_singleton;
759 : };
760 :
761 : #endif // HAL_QUADPLANE_ENABLED
|
