Line data Source code
1 : #pragma once
2 :
3 : #include <AP_Param/AP_Param.h>
4 : #include <AP_Common/Location.h>
5 : #include <stdint.h>
6 : #include <AP_Soaring/AP_Soaring.h>
7 : #include <AP_ADSB/AP_ADSB.h>
8 : #include <AP_Vehicle/ModeReason.h>
9 : #include "quadplane.h"
10 : #include <AP_AHRS/AP_AHRS.h>
11 : #include <AP_Mission/AP_Mission.h>
12 : #include "config.h"
13 : #include "pullup.h"
14 : #include "systemid.h"
15 :
16 : #ifndef AP_QUICKTUNE_ENABLED
17 : #define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED
18 : #endif
19 :
20 : #ifndef MODE_AUTOLAND_ENABLED
21 : #define MODE_AUTOLAND_ENABLED 1
22 : #endif
23 :
24 : #include <AP_Quicktune/AP_Quicktune.h>
25 :
26 : class AC_PosControl;
27 : class AC_AttitudeControl_Multi;
28 : class AC_Loiter;
29 : class Mode
30 : {
31 : public:
32 :
33 : /* Do not allow copies */
34 : CLASS_NO_COPY(Mode);
35 :
36 : // Auto Pilot modes
37 : // ----------------
38 : enum Number : uint8_t {
39 : MANUAL = 0,
40 : CIRCLE = 1,
41 : STABILIZE = 2,
42 : TRAINING = 3,
43 : ACRO = 4,
44 : FLY_BY_WIRE_A = 5,
45 : FLY_BY_WIRE_B = 6,
46 : CRUISE = 7,
47 : AUTOTUNE = 8,
48 : AUTO = 10,
49 : RTL = 11,
50 : LOITER = 12,
51 : TAKEOFF = 13,
52 : AVOID_ADSB = 14,
53 : GUIDED = 15,
54 : INITIALISING = 16,
55 : #if HAL_QUADPLANE_ENABLED
56 : QSTABILIZE = 17,
57 : QHOVER = 18,
58 : QLOITER = 19,
59 : QLAND = 20,
60 : QRTL = 21,
61 : #if QAUTOTUNE_ENABLED
62 : QAUTOTUNE = 22,
63 : #endif
64 : QACRO = 23,
65 : #endif
66 : THERMAL = 24,
67 : #if HAL_QUADPLANE_ENABLED
68 : LOITER_ALT_QLAND = 25,
69 : #endif
70 : #if MODE_AUTOLAND_ENABLED
71 : AUTOLAND = 26,
72 : #endif
73 :
74 : // Mode number 30 reserved for "offboard" for external/lua control.
75 : };
76 :
77 : // Constructor
78 : Mode();
79 :
80 : // enter this mode, always returns true/success
81 : bool enter();
82 :
83 : // perform any cleanups required:
84 : void exit();
85 :
86 : // run controllers specific to this mode
87 : virtual void run();
88 :
89 : // returns a unique number specific to this mode
90 : virtual Number mode_number() const = 0;
91 :
92 : // returns full text name
93 : virtual const char *name() const = 0;
94 :
95 : // returns a string for this flightmode, exactly 4 bytes
96 : virtual const char *name4() const = 0;
97 :
98 : // returns true if the vehicle can be armed in this mode
99 : bool pre_arm_checks(size_t buflen, char *buffer) const;
100 :
101 : // Reset rate and steering and TECS controllers
102 : void reset_controllers();
103 :
104 : //
105 : // methods that sub classes should override to affect movement of the vehicle in this mode
106 : //
107 :
108 : // convert user input to targets, implement high level control for this mode
109 : virtual void update() = 0;
110 :
111 : // true for all q modes
112 48467605 : virtual bool is_vtol_mode() const { return false; }
113 : virtual bool is_vtol_man_throttle() const;
114 598554 : virtual bool is_vtol_man_mode() const { return false; }
115 :
116 : // guided or adsb mode
117 47836161 : virtual bool is_guided_mode() const { return false; }
118 :
119 : // true if mode can have terrain following disabled by switch
120 940 : virtual bool allows_terrain_disable() const { return false; }
121 :
122 : // true if automatic switch to thermal mode is supported.
123 3411 : virtual bool does_automatic_thermal_switch() const {return false; }
124 :
125 : // subclasses override this if they require navigation.
126 160874 : virtual void navigate() { return; }
127 :
128 : // this allows certain flight modes to mix RC input with throttle
129 : // depending on airspeed_nudge_cm
130 191988 : virtual bool allows_throttle_nudging() const { return false; }
131 :
132 : // true if the mode sets the vehicle destination, which controls
133 : // whether control input is ignored with STICK_MIXING=0
134 589556 : virtual bool does_auto_navigation() const { return false; }
135 :
136 : // true if the mode sets the vehicle destination, which controls
137 : // whether control input is ignored with STICK_MIXING=0
138 15298967 : virtual bool does_auto_throttle() const { return false; }
139 :
140 : // true if the mode supports autotuning (via switch for modes other
141 : // that AUTOTUNE itself
142 0 : virtual bool mode_allows_autotuning() const { return false; }
143 :
144 : // method for mode specific target altitude profiles
145 : virtual void update_target_altitude();
146 :
147 : // handle a guided target request from GCS
148 1 : virtual bool handle_guided_request(Location target_loc) { return false; }
149 :
150 : // true if is landing
151 2820 : virtual bool is_landing() const { return false; }
152 :
153 : // true if is taking
154 : virtual bool is_taking_off() const;
155 :
156 : // true if throttle min/max limits should be applied
157 : virtual bool use_throttle_limits() const;
158 :
159 : // true if voltage correction should be applied to throttle
160 : virtual bool use_battery_compensation() const;
161 :
162 : #if MODE_AUTOLAND_ENABLED
163 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
164 9865 : virtual bool allows_autoland_direction_capture() const { return false; }
165 : #endif
166 :
167 : #if AP_QUICKTUNE_ENABLED
168 : // does this mode support VTOL quicktune?
169 2260751 : virtual bool supports_quicktune() const { return false; }
170 : #endif
171 :
172 : #if AP_PLANE_SYSTEMID_ENABLED
173 : // does this mode support quadplane vtol systemid?
174 0 : virtual bool supports_vtol_systemid() const { return false; }
175 :
176 : // does this mode support plane or quadplane fixed wing systemid?
177 0 : virtual bool supports_fw_systemid() const { return false; }
178 :
179 : // Return true if fixed wing system ID should be allowed
180 : bool allow_fw_systemid() const;
181 : #endif
182 :
183 : protected:
184 :
185 : // subclasses override this to perform checks before entering the mode
186 1275 : virtual bool _enter() { return true; }
187 :
188 : // subclasses override this to perform any required cleanup when exiting the mode
189 1804 : virtual void _exit() { return; }
190 :
191 : // mode specific pre-arm checks
192 : virtual bool _pre_arm_checks(size_t buflen, char *buffer) const;
193 :
194 : // Helper to output to both k_rudder and k_steering servo functions
195 : void output_rudder_and_steering(float val);
196 :
197 : // Output pilot throttle, this is used in stabilized modes without auto throttle control
198 : void output_pilot_throttle();
199 :
200 : // makes the initialiser list in the constructor manageable
201 : uint8_t unused_integer;
202 :
203 : #if HAL_QUADPLANE_ENABLED
204 : // References for convenience, used by QModes
205 : AC_PosControl*& pos_control;
206 : AC_AttitudeControl_Multi*& attitude_control;
207 : AC_Loiter*& loiter_nav;
208 : QuadPlane& quadplane;
209 : QuadPlane::PosControlState &poscontrol;
210 : #endif
211 : AP_AHRS& ahrs;
212 : };
213 :
214 :
215 : class ModeAcro : public Mode
216 : {
217 : friend class ModeQAcro;
218 : public:
219 :
220 1700 : Mode::Number mode_number() const override { return Mode::Number::ACRO; }
221 0 : const char *name() const override { return "Acro"; }
222 4 : const char *name4() const override { return "ACRO"; }
223 :
224 : // methods that affect movement of the vehicle in this mode
225 : void update() override;
226 :
227 : void run() override;
228 :
229 : void stabilize();
230 :
231 : void stabilize_quaternion();
232 :
233 : #if MODE_AUTOLAND_ENABLED
234 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
235 110 : bool allows_autoland_direction_capture() const override { return true; }
236 : #endif
237 :
238 : protected:
239 :
240 : // ACRO controller state
241 : struct {
242 : bool locked_roll;
243 : bool locked_pitch;
244 : float locked_roll_err;
245 : int32_t locked_pitch_cd;
246 : Quaternion q;
247 : bool roll_active_last;
248 : bool pitch_active_last;
249 : bool yaw_active_last;
250 : } acro_state;
251 :
252 : bool _enter() override;
253 : };
254 :
255 : class ModeAuto : public Mode
256 : {
257 : public:
258 : friend class Plane;
259 :
260 1631135 : Number mode_number() const override { return Number::AUTO; }
261 89 : const char *name() const override { return "Auto"; }
262 192 : const char *name4() const override { return "AUTO"; }
263 :
264 42978 : bool does_automatic_thermal_switch() const override { return true; }
265 :
266 : // methods that affect movement of the vehicle in this mode
267 : void update() override;
268 :
269 : void navigate() override;
270 :
271 246387 : bool allows_throttle_nudging() const override { return true; }
272 :
273 : bool does_auto_navigation() const override;
274 :
275 : bool does_auto_throttle() const override;
276 :
277 0 : bool mode_allows_autotuning() const override { return true; }
278 :
279 : bool is_landing() const override;
280 :
281 : void do_nav_delay(const AP_Mission::Mission_Command& cmd);
282 : bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
283 :
284 : bool verify_altitude_wait(const AP_Mission::Mission_Command& cmd);
285 :
286 : void run() override;
287 :
288 : #if MODE_AUTOLAND_ENABLED
289 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
290 228124 : bool allows_autoland_direction_capture() const override { return true; }
291 : #endif
292 :
293 : #if AP_PLANE_GLIDER_PULLUP_ENABLED
294 3282535 : bool in_pullup() const { return pullup.in_pullup(); }
295 : #endif
296 :
297 : #if AP_PLANE_SYSTEMID_ENABLED
298 : // does this mode support fixed wing systemid?
299 0 : bool supports_fw_systemid() const override { return true; }
300 : #endif
301 :
302 : protected:
303 :
304 : bool _enter() override;
305 : void _exit() override;
306 : bool _pre_arm_checks(size_t buflen, char *buffer) const override;
307 :
308 : private:
309 :
310 : // Delay the next navigation command
311 : struct {
312 : uint32_t time_max_ms;
313 : uint32_t time_start_ms;
314 : } nav_delay;
315 :
316 : // wiggle state and timer for NAV_ALTITUDE_WAIT
317 : void wiggle_servos();
318 : struct {
319 : uint8_t stage;
320 : uint32_t last_ms;
321 : } wiggle;
322 :
323 : #if AP_PLANE_GLIDER_PULLUP_ENABLED
324 : GliderPullup pullup;
325 : #endif // AP_PLANE_GLIDER_PULLUP_ENABLED
326 : };
327 :
328 :
329 : class ModeAutoTune : public Mode
330 : {
331 : public:
332 :
333 24199 : Number mode_number() const override { return Number::AUTOTUNE; }
334 0 : const char *name() const override { return "Autotune"; }
335 3 : const char *name4() const override { return "ATUN"; }
336 :
337 : // methods that affect movement of the vehicle in this mode
338 : void update() override;
339 :
340 0 : bool mode_allows_autotuning() const override { return true; }
341 :
342 : void run() override;
343 :
344 : #if MODE_AUTOLAND_ENABLED
345 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
346 0 : bool allows_autoland_direction_capture() const override { return true; }
347 : #endif
348 :
349 : protected:
350 :
351 : bool _enter() override;
352 : };
353 :
354 : class ModeGuided : public Mode
355 : {
356 : public:
357 :
358 272389 : Number mode_number() const override { return Number::GUIDED; }
359 0 : const char *name() const override { return "Guided"; }
360 58 : const char *name4() const override { return "GUID"; }
361 :
362 : // methods that affect movement of the vehicle in this mode
363 : void update() override;
364 :
365 : void navigate() override;
366 :
367 9521702 : virtual bool is_guided_mode() const override { return true; }
368 :
369 40665 : bool allows_throttle_nudging() const override { return true; }
370 :
371 1462973 : bool does_auto_navigation() const override { return true; }
372 :
373 6605436 : bool does_auto_throttle() const override { return true; }
374 :
375 : // handle a guided target request from GCS
376 : bool handle_guided_request(Location target_loc) override;
377 :
378 : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
379 : // handle a guided airspeed command, typically from companion computer
380 : bool handle_change_airspeed(const float airspeed, const float acceleration);
381 : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
382 :
383 : void set_radius_and_direction(const float radius, const bool direction_is_ccw);
384 :
385 : void update_target_altitude() override;
386 :
387 : #if AP_PLANE_SYSTEMID_ENABLED
388 : // does this mode support fixed wing systemid?
389 0 : bool supports_fw_systemid() const override { return true; }
390 : #endif
391 :
392 : protected:
393 :
394 : bool _enter() override;
395 173 : bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; }
396 : #if AP_QUICKTUNE_ENABLED
397 188232 : bool supports_quicktune() const override { return true; }
398 : #endif
399 :
400 : private:
401 : float active_radius_m;
402 : };
403 :
404 : class ModeCircle: public Mode
405 : {
406 : public:
407 :
408 25051 : Number mode_number() const override { return Number::CIRCLE; }
409 5 : const char *name() const override { return "Circle"; }
410 16 : const char *name4() const override { return "CIRC"; }
411 :
412 : // methods that affect movement of the vehicle in this mode
413 : void update() override;
414 :
415 38808 : bool does_auto_navigation() const override { return true; }
416 :
417 192897 : bool does_auto_throttle() const override { return true; }
418 :
419 : #if AP_PLANE_SYSTEMID_ENABLED
420 : // does this mode support fixed wing systemid?
421 0 : bool supports_fw_systemid() const override { return true; }
422 : #endif
423 :
424 : protected:
425 :
426 : bool _enter() override;
427 : };
428 :
429 : class ModeLoiter : public Mode
430 : {
431 : public:
432 :
433 135184 : Number mode_number() const override { return Number::LOITER; }
434 0 : const char *name() const override { return "Loiter"; }
435 37 : const char *name4() const override { return "LOIT"; }
436 :
437 : // methods that affect movement of the vehicle in this mode
438 : void update() override;
439 :
440 : void navigate() override;
441 :
442 : bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc);
443 : bool isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t heading_cd);
444 : bool isHeadingLinedUp_cd(const int32_t bearing_cd);
445 :
446 21432 : bool allows_throttle_nudging() const override { return true; }
447 :
448 617931 : bool does_auto_navigation() const override { return true; }
449 :
450 2090762 : bool does_auto_throttle() const override { return true; }
451 :
452 1 : bool allows_terrain_disable() const override { return true; }
453 :
454 : void update_target_altitude() override;
455 :
456 0 : bool mode_allows_autotuning() const override { return true; }
457 :
458 : #if AP_PLANE_SYSTEMID_ENABLED
459 : // does this mode support fixed wing systemid?
460 0 : bool supports_fw_systemid() const override { return true; }
461 : #endif
462 :
463 : protected:
464 :
465 : bool _enter() override;
466 : };
467 :
468 : #if HAL_QUADPLANE_ENABLED
469 : class ModeLoiterAltQLand : public ModeLoiter
470 : {
471 : public:
472 :
473 3370 : Number mode_number() const override { return Number::LOITER_ALT_QLAND; }
474 0 : const char *name() const override { return "Loiter to QLand"; }
475 2 : const char *name4() const override { return "L2QL"; }
476 :
477 : // handle a guided target request from GCS
478 : bool handle_guided_request(Location target_loc) override;
479 :
480 : protected:
481 : bool _enter() override;
482 :
483 : void navigate() override;
484 :
485 : private:
486 : void switch_qland();
487 :
488 : };
489 : #endif // HAL_QUADPLANE_ENABLED
490 :
491 : class ModeManual : public Mode
492 : {
493 : public:
494 :
495 505575 : Number mode_number() const override { return Number::MANUAL; }
496 2 : const char *name() const override { return "Manual"; }
497 873 : const char *name4() const override { return "MANU"; }
498 :
499 : // methods that affect movement of the vehicle in this mode
500 : void update() override;
501 :
502 : void run() override;
503 :
504 : // true if throttle min/max limits should be applied
505 : bool use_throttle_limits() const override;
506 :
507 : // true if voltage correction should be applied to throttle
508 613998 : bool use_battery_compensation() const override { return false; }
509 :
510 : #if MODE_AUTOLAND_ENABLED
511 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
512 349881 : bool allows_autoland_direction_capture() const override { return true; }
513 : #endif
514 :
515 : };
516 :
517 :
518 : class ModeRTL : public Mode
519 : {
520 : public:
521 :
522 51435 : Number mode_number() const override { return Number::RTL; }
523 195 : const char *name() const override { return "RTL"; }
524 68 : const char *name4() const override { return "RTL "; }
525 :
526 : // methods that affect movement of the vehicle in this mode
527 : void update() override;
528 :
529 : void navigate() override;
530 :
531 7649 : bool allows_throttle_nudging() const override { return true; }
532 :
533 180742 : bool does_auto_navigation() const override { return true; }
534 :
535 870939 : bool does_auto_throttle() const override { return true; }
536 :
537 : protected:
538 :
539 : bool _enter() override;
540 188 : bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
541 :
542 : private:
543 :
544 : // Switch to QRTL if enabled and within radius
545 : bool switch_QRTL();
546 : };
547 :
548 : class ModeStabilize : public Mode
549 : {
550 : public:
551 :
552 508 : Number mode_number() const override { return Number::STABILIZE; }
553 0 : const char *name() const override { return "Stabilize"; }
554 3 : const char *name4() const override { return "STAB"; }
555 :
556 : // methods that affect movement of the vehicle in this mode
557 : void update() override;
558 :
559 : void run() override;
560 :
561 : #if MODE_AUTOLAND_ENABLED
562 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
563 99 : bool allows_autoland_direction_capture() const override { return true; }
564 : #endif
565 :
566 : private:
567 : void stabilize_stick_mixing_direct();
568 :
569 : };
570 :
571 : class ModeTraining : public Mode
572 : {
573 : public:
574 :
575 295 : Number mode_number() const override { return Number::TRAINING; }
576 0 : const char *name() const override { return "Training"; }
577 1 : const char *name4() const override { return "TRAN"; }
578 :
579 : // methods that affect movement of the vehicle in this mode
580 : void update() override;
581 :
582 : void run() override;
583 :
584 : #if MODE_AUTOLAND_ENABLED
585 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
586 0 : bool allows_autoland_direction_capture() const override { return true; }
587 : #endif
588 : };
589 :
590 : class ModeInitializing : public Mode
591 : {
592 : public:
593 :
594 4721 : Number mode_number() const override { return Number::INITIALISING; }
595 0 : const char *name() const override { return "Initialising"; }
596 692 : const char *name4() const override { return "INIT"; }
597 :
598 0 : bool _enter() override { return false; }
599 :
600 : // methods that affect movement of the vehicle in this mode
601 0 : void update() override { }
602 :
603 0 : bool allows_throttle_nudging() const override { return true; }
604 :
605 57642 : bool does_auto_throttle() const override { return true; }
606 :
607 : protected:
608 0 : bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
609 :
610 : };
611 :
612 : class ModeFBWA : public Mode
613 : {
614 : public:
615 :
616 320139 : Number mode_number() const override { return Number::FLY_BY_WIRE_A; }
617 7 : const char *name() const override { return "FBWA"; }
618 398 : const char *name4() const override { return "FBWA"; }
619 :
620 : // methods that affect movement of the vehicle in this mode
621 : void update() override;
622 :
623 0 : bool mode_allows_autotuning() const override { return true; }
624 :
625 : void run() override;
626 :
627 : #if AP_PLANE_SYSTEMID_ENABLED
628 : // does this mode support fixed wing systemid?
629 0 : bool supports_fw_systemid() const override { return true; }
630 : #endif
631 :
632 : #if MODE_AUTOLAND_ENABLED
633 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
634 32465 : bool allows_autoland_direction_capture() const override { return true; }
635 : #endif
636 :
637 : };
638 :
639 : class ModeFBWB : public Mode
640 : {
641 : public:
642 :
643 22045 : Number mode_number() const override { return Number::FLY_BY_WIRE_B; }
644 0 : const char *name() const override { return "FBWB"; }
645 6 : const char *name4() const override { return "FBWB"; }
646 :
647 0 : bool allows_terrain_disable() const override { return true; }
648 :
649 8760 : bool does_automatic_thermal_switch() const override { return true; }
650 :
651 : // methods that affect movement of the vehicle in this mode
652 : void update() override;
653 :
654 165022 : bool does_auto_throttle() const override { return true; }
655 :
656 0 : bool mode_allows_autotuning() const override { return true; }
657 :
658 3235 : void update_target_altitude() override {};
659 :
660 : protected:
661 :
662 : bool _enter() override;
663 : };
664 :
665 : class ModeCruise : public Mode
666 : {
667 : public:
668 :
669 75667 : Number mode_number() const override { return Number::CRUISE; }
670 11 : const char *name() const override { return "Cruise"; }
671 59 : const char *name4() const override { return "CRUS"; }
672 :
673 0 : bool allows_terrain_disable() const override { return true; }
674 :
675 0 : bool does_automatic_thermal_switch() const override { return true; }
676 :
677 : // methods that affect movement of the vehicle in this mode
678 : void update() override;
679 :
680 : void navigate() override;
681 :
682 : bool get_target_heading_cd(int32_t &target_heading) const;
683 :
684 1591586 : bool does_auto_throttle() const override { return true; }
685 :
686 9808 : void update_target_altitude() override {};
687 :
688 : #if AP_PLANE_SYSTEMID_ENABLED
689 : // does this mode support fixed wing systemid?
690 0 : bool supports_fw_systemid() const override { return true; }
691 : #endif
692 :
693 : protected:
694 :
695 : bool _enter() override;
696 :
697 : bool locked_heading;
698 : int32_t locked_heading_cd;
699 : uint32_t lock_timer_ms;
700 : };
701 :
702 : #if HAL_ADSB_ENABLED
703 : class ModeAvoidADSB : public Mode
704 : {
705 : public:
706 :
707 3523 : Number mode_number() const override { return Number::AVOID_ADSB; }
708 0 : const char *name() const override { return "Avoid ADSB"; }
709 3 : const char *name4() const override { return "AVOI"; }
710 :
711 : // methods that affect movement of the vehicle in this mode
712 : void update() override;
713 :
714 : void navigate() override;
715 :
716 19168 : virtual bool is_guided_mode() const override { return true; }
717 :
718 34764 : bool does_auto_throttle() const override { return true; }
719 :
720 : protected:
721 :
722 : bool _enter() override;
723 : };
724 : #endif
725 :
726 : #if HAL_QUADPLANE_ENABLED
727 : class ModeQStabilize : public Mode
728 : {
729 : public:
730 :
731 3235 : Number mode_number() const override { return Number::QSTABILIZE; }
732 0 : const char *name() const override { return "QStabilize"; }
733 3 : const char *name4() const override { return "QSTB"; }
734 :
735 232001 : bool is_vtol_mode() const override { return true; }
736 38831 : bool is_vtol_man_throttle() const override { return true; }
737 2662 : virtual bool is_vtol_man_mode() const override { return true; }
738 508 : bool allows_throttle_nudging() const override { return true; }
739 :
740 : // methods that affect movement of the vehicle in this mode
741 : void update() override;
742 :
743 : // used as a base class for all Q modes
744 : bool _enter() override;
745 :
746 : void run() override;
747 :
748 : #if AP_PLANE_SYSTEMID_ENABLED
749 : // does this mode support quadplane vtol systemid?
750 0 : bool supports_vtol_systemid() const override { return true; }
751 : #endif
752 :
753 : protected:
754 : private:
755 :
756 : void set_tailsitter_roll_pitch(const float roll_input, const float pitch_input);
757 : void set_limited_roll_pitch(const float roll_input, const float pitch_input);
758 :
759 : };
760 :
761 : class ModeQHover : public Mode
762 : {
763 : public:
764 :
765 71127 : Number mode_number() const override { return Number::QHOVER; }
766 0 : const char *name() const override { return "QHover"; }
767 26 : const char *name4() const override { return "QHOV"; }
768 :
769 4860425 : bool is_vtol_mode() const override { return true; }
770 16296 : virtual bool is_vtol_man_mode() const override { return true; }
771 :
772 : // methods that affect movement of the vehicle in this mode
773 : void update() override;
774 :
775 : void run() override;
776 :
777 : #if AP_PLANE_SYSTEMID_ENABLED
778 : // does this mode support quadplane vtol systemid?
779 0 : bool supports_vtol_systemid() const override { return true; }
780 : #endif
781 :
782 : protected:
783 :
784 : bool _enter() override;
785 : #if AP_QUICKTUNE_ENABLED
786 48398 : bool supports_quicktune() const override { return true; }
787 : #endif
788 : };
789 :
790 : class ModeQLoiter : public Mode
791 : {
792 : friend class QuadPlane;
793 : friend class ModeQLand;
794 : friend class Plane;
795 :
796 : public:
797 :
798 107877 : Number mode_number() const override { return Number::QLOITER; }
799 0 : const char *name() const override { return "QLoiter"; }
800 76 : const char *name4() const override { return "QLOT"; }
801 :
802 7481165 : bool is_vtol_mode() const override { return true; }
803 69015 : virtual bool is_vtol_man_mode() const override { return true; }
804 :
805 : // methods that affect movement of the vehicle in this mode
806 : void update() override;
807 :
808 : void run() override;
809 :
810 : #if AP_PLANE_SYSTEMID_ENABLED
811 : // does this mode support quadplane vtol systemid?
812 0 : bool supports_vtol_systemid() const override { return true; }
813 : #endif
814 :
815 : protected:
816 :
817 : bool _enter() override;
818 : uint32_t last_target_loc_set_ms;
819 :
820 : #if AP_QUICKTUNE_ENABLED
821 69409 : bool supports_quicktune() const override { return true; }
822 : #endif
823 : };
824 :
825 : class ModeQLand : public Mode
826 : {
827 : public:
828 61360 : Number mode_number() const override { return Number::QLAND; }
829 78 : const char *name() const override { return "QLand"; }
830 17 : const char *name4() const override { return "QLND"; }
831 :
832 4780247 : bool is_vtol_mode() const override { return true; }
833 :
834 : // methods that affect movement of the vehicle in this mode
835 : void update() override;
836 :
837 : void run() override;
838 :
839 : protected:
840 :
841 : bool _enter() override;
842 78 : bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
843 : };
844 :
845 : class ModeQRTL : public Mode
846 : {
847 : public:
848 :
849 86389 : Number mode_number() const override { return Number::QRTL; }
850 122 : const char *name() const override { return "QRTL"; }
851 19 : const char *name4() const override { return "QRTL"; }
852 :
853 501215 : bool is_vtol_mode() const override { return true; }
854 :
855 : // methods that affect movement of the vehicle in this mode
856 : void update() override;
857 :
858 : void run() override;
859 :
860 2045621 : bool does_auto_throttle() const override { return true; }
861 :
862 : void update_target_altitude() override;
863 :
864 : bool allows_throttle_nudging() const override;
865 :
866 : float get_VTOL_return_radius() const;
867 :
868 : protected:
869 :
870 : bool _enter() override;
871 122 : bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
872 :
873 : private:
874 :
875 : enum class SubMode {
876 : climb,
877 : RTL,
878 : } submode;
879 :
880 : // current height above the QRTL destination altitude, in the same
881 : // frame (terrain-relative if QRTL is terrain following, else absolute)
882 : // the approach altitude ramp is applied in. Returns false, leaving
883 : // alt_delta_m unchanged, if no usable altitude data is available
884 : bool calc_alt_delta_m(float &alt_delta_m) const;
885 :
886 : // state latched at the start of the fixed wing approach, used to
887 : // gradually ramp the target altitude down to RTL_ALTITUDE
888 : struct {
889 : bool valid;
890 : float alt_delta_m; // height above the QRTL destination altitude
891 : float dist_m; // distance to the QRTL destination
892 : } approach_start;
893 : };
894 :
895 : class ModeQAcro : public Mode
896 : {
897 : public:
898 :
899 2507 : Number mode_number() const override { return Number::QACRO; }
900 0 : const char *name() const override { return "QAcro"; }
901 3 : const char *name4() const override { return "QACO"; }
902 :
903 139896 : bool is_vtol_mode() const override { return true; }
904 21271 : bool is_vtol_man_throttle() const override { return true; }
905 788 : virtual bool is_vtol_man_mode() const override { return true; }
906 :
907 : // methods that affect movement of the vehicle in this mode
908 : void update() override;
909 :
910 : void run() override;
911 :
912 : protected:
913 :
914 : bool _enter() override;
915 : };
916 :
917 : #if QAUTOTUNE_ENABLED
918 : class ModeQAutotune : public Mode
919 : {
920 : public:
921 :
922 28026 : Number mode_number() const override { return Number::QAUTOTUNE; }
923 0 : const char *name() const override { return "QAutotune"; }
924 1 : const char *name4() const override { return "QATN"; }
925 :
926 2143517 : bool is_vtol_mode() const override { return true; }
927 2263 : virtual bool is_vtol_man_mode() const override { return true; }
928 :
929 : void run() override;
930 :
931 : // methods that affect movement of the vehicle in this mode
932 : void update() override;
933 :
934 : protected:
935 :
936 : bool _enter() override;
937 : void _exit() override;
938 : };
939 : #endif // QAUTOTUNE_ENABLED
940 :
941 : #endif // HAL_QUADPLANE_ENABLED
942 :
943 : class ModeTakeoff: public Mode
944 : {
945 : public:
946 : ModeTakeoff();
947 :
948 113428 : Number mode_number() const override { return Number::TAKEOFF; }
949 0 : const char *name() const override { return "Takeoff"; }
950 53 : const char *name4() const override { return "TKOF"; }
951 :
952 : // methods that affect movement of the vehicle in this mode
953 : void update() override;
954 :
955 : void navigate() override;
956 :
957 17731 : bool allows_throttle_nudging() const override { return true; }
958 :
959 275505 : bool does_auto_navigation() const override { return true; }
960 :
961 1335429 : bool does_auto_throttle() const override { return true; }
962 :
963 : #if MODE_AUTOLAND_ENABLED
964 : // true if mode allows landing direction to be set on first takeoff after arm in this mode
965 26548 : bool allows_autoland_direction_capture() const override { return true; }
966 : #endif
967 :
968 : // var_info for holding parameter information
969 : static const struct AP_Param::GroupInfo var_info[];
970 :
971 : AP_Int16 target_alt;
972 : AP_Int16 level_alt;
973 : AP_Float ground_pitch;
974 :
975 : protected:
976 : AP_Int16 target_dist;
977 : AP_Int8 level_pitch;
978 :
979 : bool takeoff_mode_setup;
980 : Location start_loc;
981 :
982 : bool _enter() override;
983 :
984 : private:
985 :
986 : // flag that we have already called autoenable fences once in MODE TAKEOFF
987 : bool have_autoenabled_fences;
988 :
989 : };
990 : #if MODE_AUTOLAND_ENABLED
991 : class ModeAutoLand: public Mode
992 : {
993 : public:
994 : ModeAutoLand();
995 :
996 35649 : Number mode_number() const override { return Number::AUTOLAND; }
997 0 : const char *name() const override { return "Autoland"; }
998 4 : const char *name4() const override { return "ALND"; }
999 :
1000 : // methods that affect movement of the vehicle in this mode
1001 : void update() override;
1002 :
1003 : void navigate() override;
1004 :
1005 5574 : bool allows_throttle_nudging() const override { return true; }
1006 :
1007 58281 : bool does_auto_navigation() const override { return true; }
1008 :
1009 270680 : bool does_auto_throttle() const override { return true; }
1010 :
1011 : bool is_landing() const override;
1012 :
1013 : void check_takeoff_direction(void);
1014 :
1015 : // return true when lined up correctly from the LOITER_TO_ALT
1016 : bool landing_lined_up(void);
1017 :
1018 : // see if we should capture the direction
1019 : void arm_check(void);
1020 :
1021 : // var_info for holding parameter information
1022 : static const struct AP_Param::GroupInfo var_info[];
1023 :
1024 : AP_Int16 final_wp_alt;
1025 : AP_Int16 final_wp_dist;
1026 : AP_Int16 landing_dir_off;
1027 : AP_Int8 options;
1028 : AP_Int16 terrain_alt_min;
1029 :
1030 : // Bitfields of AUTOLAND_OPTIONS
1031 : enum class AutoLandOption {
1032 : AUTOLAND_DIR_ON_ARM = (1U << 0), // set dir for autoland on arm if compass in use.
1033 : };
1034 :
1035 : enum class AutoLandStage {
1036 : CLIMB,
1037 : LOITER,
1038 : LANDING
1039 : };
1040 :
1041 413 : bool autoland_option_is_set(AutoLandOption option) const {
1042 413 : return (options & int8_t(option)) != 0;
1043 : }
1044 :
1045 : protected:
1046 : bool _enter() override;
1047 : AP_Mission::Mission_Command cmd_climb;
1048 : AP_Mission::Mission_Command cmd_loiter;
1049 : AP_Mission::Mission_Command cmd_land;
1050 : Location land_start;
1051 : AutoLandStage stage;
1052 : void set_autoland_direction(const float heading);
1053 : };
1054 : #endif
1055 : #if HAL_SOARING_ENABLED
1056 :
1057 : class ModeThermal: public Mode
1058 : {
1059 : public:
1060 :
1061 17580 : Number mode_number() const override { return Number::THERMAL; }
1062 1 : const char *name() const override { return "Thermal"; }
1063 1 : const char *name4() const override { return "THML"; }
1064 :
1065 : // methods that affect movement of the vehicle in this mode
1066 : void update() override;
1067 :
1068 : // Update thermal tracking and exiting logic.
1069 : void update_soaring();
1070 :
1071 : void navigate() override;
1072 :
1073 2724 : bool allows_throttle_nudging() const override { return true; }
1074 :
1075 27237 : bool does_auto_navigation() const override { return true; }
1076 :
1077 : // true if we are in an auto-throttle mode, which means
1078 : // we need to run the speed/height controller
1079 130737 : bool does_auto_throttle() const override { return true; }
1080 :
1081 : protected:
1082 :
1083 : bool exit_heading_aligned() const;
1084 : void restore_mode(const char *reason, ModeReason modereason);
1085 :
1086 : bool _enter() override;
1087 : };
1088 :
1089 : #endif
|
