forked from peterbarker/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmode.h
1916 lines (1462 loc) · 66.3 KB
/
mode.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#pragma once
#include "Copter.h"
#include <AP_Math/chirp.h>
class Parameters;
class ParametersG2;
class GCS_Copter;
class Mode {
public:
// Auto Pilot Modes enumeration
enum class Number : uint8_t {
STABILIZE = 0, // manual airframe angle with manual throttle
ACRO = 1, // manual body-frame angular rate with manual throttle
ALT_HOLD = 2, // manual airframe angle with automatic throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
LOITER = 5, // automatic horizontal acceleration with automatic throttle
RTL = 6, // automatic return to launching point
CIRCLE = 7, // automatic circular flight with automatic throttle
LAND = 9, // automatic landing with horizontal position control
DRIFT = 11, // semi-autonomous position, yaw and throttle control
SPORT = 13, // manual earth-frame angular rate control with manual throttle
FLIP = 14, // automatically flip the vehicle on the roll axis
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
BRAKE = 17, // full-brake using inertial/GPS system, no pilot input
THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude
SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps
FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder
FOLLOW = 23, // follow attempts to follow another vehicle or ground station
ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers
AUTOROTATE = 26, // Autonomous autorotation
AUTO_RTL = 27, // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence
TURTLE = 28, // Flip over after crash
// Mode number 127 reserved for the "drone show mode" in the Skybrush
// fork at https://github.com/skybrush-io/ardupilot
};
// constructor
Mode(void);
// do not allow copying
CLASS_NO_COPY(Mode);
// returns a unique number specific to this mode
virtual Number mode_number() const = 0;
// child classes should override these methods
virtual bool init(bool ignore_checks) {
return true;
}
virtual void exit() {};
virtual void run() = 0;
virtual bool requires_GPS() const = 0;
virtual bool has_manual_throttle() const = 0;
virtual bool allows_arming(AP_Arming::Method method) const = 0;
virtual bool is_autopilot() const { return false; }
virtual bool has_user_takeoff(bool must_navigate) const { return false; }
virtual bool in_guided_mode() const { return false; }
virtual bool logs_attitude() const { return false; }
virtual bool allows_save_trim() const { return false; }
virtual bool allows_autotune() const { return false; }
virtual bool allows_flip() const { return false; }
// return a string for this flightmode
virtual const char *name() const = 0;
virtual const char *name4() const = 0;
bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate);
virtual bool is_taking_off() const;
static void takeoff_stop() { takeoff.stop(); }
virtual bool is_landing() const { return false; }
// mode requires terrain to be present to be functional
virtual bool requires_terrain_failsafe() const { return false; }
// functions for reporting to GCS
virtual bool get_wp(Location &loc) const { return false; };
virtual int32_t wp_bearing() const { return 0; }
virtual uint32_t wp_distance() const { return 0; }
virtual float crosstrack_error() const { return 0.0f;}
// functions to support MAV_CMD_DO_CHANGE_SPEED
virtual bool set_speed_xy(float speed_xy_cms) {return false;}
virtual bool set_speed_up(float speed_xy_cms) {return false;}
virtual bool set_speed_down(float speed_xy_cms) {return false;}
int32_t get_alt_above_ground_cm(void);
// pilot input processing
void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const;
Vector2f get_pilot_desired_velocity(float vel_max) const;
float get_pilot_desired_yaw_rate(float yaw_in);
float get_pilot_desired_throttle() const;
// returns climb target_rate reduced to avoid obstacles and
// altitude fence
float get_avoidance_adjusted_climbrate(float target_rate);
const Vector3f& get_vel_desired_cms() {
// note that position control isn't used in every mode, so
// this may return bogus data:
return pos_control->get_vel_desired_cms();
}
// send output to the motors, can be overridden by subclasses
virtual void output_to_motors();
// returns true if pilot's yaw input should be used to adjust vehicle's heading
virtual bool use_pilot_yaw() const {return true; }
// pause and resume a mode
virtual bool pause() { return false; };
virtual bool resume() { return false; };
// true if weathervaning is allowed in the current mode
#if WEATHERVANE_ENABLED == ENABLED
virtual bool allows_weathervaning() const { return false; }
#endif
protected:
// helper functions
bool is_disarmed_or_landed() const;
void zero_throttle_and_relax_ac(bool spool_up = false);
void zero_throttle_and_hold_attitude();
void make_safe_ground_handling(bool force_throttle_unlimited = false);
// functions to control normal landing. pause_descent is true if vehicle should not descend
void land_run_horizontal_control();
void land_run_vertical_control(bool pause_descent = false);
void land_run_horiz_and_vert_control(bool pause_descent = false) {
land_run_horizontal_control();
land_run_vertical_control(pause_descent);
}
// run normal or precision landing (if enabled)
// pause_descent is true if vehicle should not descend
void land_run_normal_or_precland(bool pause_descent = false);
#if PRECISION_LANDING == ENABLED
// Go towards a position commanded by prec land state machine in order to retry landing
// The passed in location is expected to be NED and in meters
void precland_retry_position(const Vector3f &retry_pos);
// Run precland statemachine. This function should be called from any mode that wants to do precision landing.
// This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
void precland_run();
#endif
// return expected input throttle setting to hover:
virtual float throttle_hover() const;
// Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
enum AltHoldModeState {
AltHold_MotorStopped,
AltHold_Takeoff,
AltHold_Landed_Ground_Idle,
AltHold_Landed_Pre_Takeoff,
AltHold_Flying
};
AltHoldModeState get_alt_hold_state(float target_climb_rate_cms);
// convenience references to avoid code churn in conversion:
Parameters &g;
ParametersG2 &g2;
AC_WPNav *&wp_nav;
AC_Loiter *&loiter_nav;
AC_PosControl *&pos_control;
AP_InertialNav &inertial_nav;
AP_AHRS &ahrs;
AC_AttitudeControl_t *&attitude_control;
MOTOR_CLASS *&motors;
RC_Channel *&channel_roll;
RC_Channel *&channel_pitch;
RC_Channel *&channel_throttle;
RC_Channel *&channel_yaw;
float &G_Dt;
// note that we support two entirely different automatic takeoffs:
// "user-takeoff", which is available in modes such as ALT_HOLD
// (see has_user_takeoff method). "user-takeoff" is a simple
// reach-altitude-based-on-pilot-input-or-parameter routine.
// "auto-takeoff" is used by both Guided and Auto, and is
// basically waypoint navigation with pilot yaw permitted.
// user-takeoff support; takeoff state is shared across all mode instances
class _TakeOff {
public:
void start(float alt_cm);
void stop();
void do_pilot_takeoff(float& pilot_climb_rate);
bool triggered(float target_climb_rate) const;
bool running() const { return _running; }
private:
bool _running;
float take_off_start_alt;
float take_off_complete_alt;
};
static _TakeOff takeoff;
virtual bool do_user_takeoff_start(float takeoff_alt_cm);
// method shared by both Guided and Auto for takeoff.
// position controller controls vehicle but the user can control the yaw.
void auto_takeoff_run();
void auto_takeoff_start(float complete_alt_cm, bool terrain_alt);
bool auto_takeoff_get_position(Vector3p& completion_pos);
// altitude above-ekf-origin below which auto takeoff does not control horizontal position
static bool auto_takeoff_no_nav_active;
static float auto_takeoff_no_nav_alt_cm;
// auto takeoff variables
static float auto_takeoff_complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain
static bool auto_takeoff_complete; // true when takeoff is complete
static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm
public:
// Navigation Yaw control
class AutoYaw {
public:
// Autopilot Yaw Mode enumeration
enum class Mode {
HOLD = 0, // hold zero yaw rate
LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted)
ROI = 2, // point towards a location held in roi (no pilot input accepted)
FIXED = 3, // point towards a particular angle (no pilot input accepted)
LOOK_AHEAD = 4, // point in the direction the copter is moving
RESETTOARMEDYAW = 5, // point towards heading at time motors were armed
ANGLE_RATE = 6, // turn at a specified rate from a starting angle
RATE = 7, // turn at a specified rate (held in auto_yaw_rate)
CIRCLE = 8, // use AC_Circle's provided yaw (used during Loiter-Turns commands)
PILOT_RATE = 9, // target rate from pilot stick
WEATHERVANE = 10, // yaw into wind
};
// mode(): current method of determining desired yaw:
Mode mode() const { return _mode; }
void set_mode_to_default(bool rtl);
void set_mode(Mode new_mode);
Mode default_mode(bool rtl) const;
void set_rate(float new_rate_cds);
// set_roi(...): set a "look at" location:
void set_roi(const Location &roi_location);
void set_fixed_yaw(float angle_deg,
float turn_rate_dps,
int8_t direction,
bool relative_angle);
void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
bool reached_fixed_yaw_target();
#if WEATHERVANE_ENABLED == ENABLED
void update_weathervane(const int16_t pilot_yaw_cds);
#endif
AC_AttitudeControl::HeadingCommand get_heading();
private:
// yaw(): main product of AutoYaw; the heading:
float yaw();
// rate_cds(): desired yaw rate in centidegrees/second:
float rate_cds() const;
float look_ahead_yaw();
float roi_yaw() const;
// auto flight mode's yaw mode
Mode _mode = Mode::LOOK_AT_NEXT_WP;
Mode _last_mode;
// Yaw will point at this location if mode is set to Mode::ROI
Vector3f roi;
// yaw used for YAW_FIXED yaw_mode
float _fixed_yaw_offset_cd;
// Deg/s we should turn
float _fixed_yaw_slewrate_cds;
// time of the last yaw update
uint32_t _last_update_ms;
// heading when in yaw_look_ahead_yaw
float _look_ahead_yaw;
// turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
float _yaw_angle_cd;
float _yaw_rate_cds;
float _pilot_yaw_rate_cds;
};
static AutoYaw auto_yaw;
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle(void);
void update_simple_mode(void);
bool set_mode(Mode::Number mode, ModeReason reason);
void set_land_complete(bool b);
GCS_Copter &gcs();
uint16_t get_pilot_speed_dn(void);
// end pass-through functions
};
#if MODE_ACRO_ENABLED == ENABLED
class ModeAcro : public Mode {
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::ACRO; }
enum class Trainer {
OFF = 0,
LEVELING = 1,
LIMITED = 2,
};
enum class AcroOptions {
AIR_MODE = 1 << 0,
RATE_LOOP_ONLY = 1 << 1,
};
virtual void run() override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return true; }
bool allows_arming(AP_Arming::Method method) const override { return true; };
bool is_autopilot() const override { return false; }
bool init(bool ignore_checks) override;
void exit() override;
// whether an air-mode aux switch has been toggled
void air_mode_aux_changed();
bool allows_save_trim() const override { return true; }
bool allows_flip() const override { return true; }
protected:
const char *name() const override { return "ACRO"; }
const char *name4() const override { return "ACRO"; }
// get_pilot_desired_angle_rates - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates
// inputs are -1 to 1 and the function returns desired angle rates in centi-degrees-per-second
void get_pilot_desired_angle_rates(float roll_in, float pitch_in, float yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
float throttle_hover() const override;
private:
bool disable_air_mode_reset;
};
#endif
#if FRAME_CONFIG == HELI_FRAME
class ModeAcro_Heli : public ModeAcro {
public:
// inherit constructor
using ModeAcro::Mode;
bool init(bool ignore_checks) override;
void run() override;
void virtual_flybar( float &roll_out, float &pitch_out, float &yaw_out, float pitch_leak, float roll_leak);
protected:
private:
};
#endif
class ModeAltHold : public Mode {
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::ALT_HOLD; }
bool init(bool ignore_checks) override;
void run() override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return true; };
bool is_autopilot() const override { return false; }
bool has_user_takeoff(bool must_navigate) const override {
return !must_navigate;
}
bool allows_autotune() const override { return true; }
bool allows_flip() const override { return true; }
protected:
const char *name() const override { return "ALT_HOLD"; }
const char *name4() const override { return "ALTH"; }
private:
};
class ModeAuto : public Mode {
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return auto_RTL? Number::AUTO_RTL : Number::AUTO; }
bool init(bool ignore_checks) override;
void exit() override;
void run() override;
bool requires_GPS() const override;
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override;
bool is_autopilot() const override { return true; }
bool in_guided_mode() const override { return _mode == SubMode::NAVGUIDED || _mode == SubMode::NAV_SCRIPT_TIME; }
// Auto modes
enum class SubMode : uint8_t {
TAKEOFF,
WP,
LAND,
RTL,
CIRCLE_MOVE_TO_EDGE,
CIRCLE,
NAVGUIDED,
LOITER,
LOITER_TO_ALT,
NAV_PAYLOAD_PLACE,
NAV_SCRIPT_TIME,
NAV_ATTITUDE_TIME,
};
// set submode. returns true on success, false on failure
void set_submode(SubMode new_submode);
// pause continue in auto mode
bool pause() override;
bool resume() override;
bool loiter_start();
void rtl_start();
void takeoff_start(const Location& dest_loc);
bool wp_start(const Location& dest_loc);
void land_start();
void circle_movetoedge_start(const Location &circle_center, float radius_m);
void circle_start();
void nav_guided_start();
bool is_landing() const override;
bool is_taking_off() const override;
bool use_pilot_yaw() const override;
bool set_speed_xy(float speed_xy_cms) override;
bool set_speed_up(float speed_up_cms) override;
bool set_speed_down(float speed_down_cms) override;
bool requires_terrain_failsafe() const override { return true; }
// return true if this flight mode supports user takeoff
// must_nagivate is true if mode must also control horizontal position
virtual bool has_user_takeoff(bool must_navigate) const override { return false; }
void payload_place_start();
// for GCS_MAVLink to call:
bool do_guided(const AP_Mission::Mission_Command& cmd);
// Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
bool jump_to_landing_sequence_auto_RTL(ModeReason reason);
// lua accessors for nav script time support
bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4);
void nav_script_time_done(uint16_t id);
AP_Mission mission{
FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)};
// Mission change detector
AP_Mission_ChangeDetector mis_change_detector;
// true if weathervaning is allowed in auto
#if WEATHERVANE_ENABLED == ENABLED
bool allows_weathervaning(void) const override;
#endif
protected:
const char *name() const override { return auto_RTL? "AUTO RTL" : "AUTO"; }
const char *name4() const override { return auto_RTL? "ARTL" : "AUTO"; }
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
float crosstrack_error() const override { return wp_nav->crosstrack_error();}
bool get_wp(Location &loc) const override;
private:
enum class Options : int32_t {
AllowArming = (1 << 0U),
AllowTakeOffWithoutRaisingThrottle = (1 << 1U),
IgnorePilotYaw = (1 << 2U),
AllowWeatherVaning = (1 << 7U),
};
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
void exit_mission();
bool check_for_mission_change(); // detect external changes to mission
void takeoff_run();
void wp_run();
void land_run();
void rtl_run();
void circle_run();
void nav_guided_run();
void loiter_run();
void loiter_to_alt_run();
void nav_attitude_time_run();
Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
void payload_place_run();
bool payload_place_run_should_run();
void payload_place_run_hover();
void payload_place_run_descent();
void payload_place_run_release();
SubMode _mode = SubMode::TAKEOFF; // controls which auto controller is run
bool shift_alt_to_current_alt(Location& target_loc) const;
void do_takeoff(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc);
void do_land(const AP_Mission::Mission_Command& cmd);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_circle(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
void do_spline_wp(const AP_Mission::Mission_Command& cmd);
void get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline);
#if NAV_GUIDED == ENABLED
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
#endif
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);
void do_yaw(const AP_Mission::Mission_Command& cmd);
void do_change_speed(const AP_Mission::Mission_Command& cmd);
void do_set_home(const AP_Mission::Mission_Command& cmd);
void do_roi(const AP_Mission::Mission_Command& cmd);
void do_mount_control(const AP_Mission::Mission_Command& cmd);
#if PARACHUTE == ENABLED
void do_parachute(const AP_Mission::Mission_Command& cmd);
#endif
#if WINCH_ENABLED == ENABLED
void do_winch(const AP_Mission::Mission_Command& cmd);
#endif
void do_payload_place(const AP_Mission::Mission_Command& cmd);
void do_RTL(void);
#if AP_SCRIPTING_ENABLED
void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
#endif
void do_nav_attitude_time(const AP_Mission::Mission_Command& cmd);
bool verify_takeoff();
bool verify_land();
bool verify_payload_place();
bool verify_loiter_unlimited();
bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
bool verify_loiter_to_alt() const;
bool verify_RTL();
bool verify_wait_delay();
bool verify_within_distance();
bool verify_yaw();
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
bool verify_circle(const AP_Mission::Mission_Command& cmd);
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
#if NAV_GUIDED == ENABLED
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
#endif
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
#if AP_SCRIPTING_ENABLED
bool verify_nav_script_time();
#endif
bool verify_nav_attitude_time(const AP_Mission::Mission_Command& cmd);
// Loiter control
uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
uint32_t loiter_time; // How long have we been loitering - The start time in millis
struct {
bool reached_destination_xy : 1;
bool loiter_start_done : 1;
bool reached_alt : 1;
float alt_error_cm;
int32_t alt;
} loiter_to_alt;
// Delay the next navigation command
uint32_t nav_delay_time_max_ms; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t nav_delay_time_start_ms;
// Delay Mission Scripting Command
int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
uint32_t condition_start;
enum class State {
FlyToLocation = 0,
Descending = 1
};
State state = State::FlyToLocation;
struct {
PayloadPlaceStateType state = PayloadPlaceStateType_Descent_Start; // records state of payload place
uint32_t descent_established_time_ms; // milliseconds
uint32_t place_start_time_ms; // milliseconds
float descent_thrust_level;
float descent_start_altitude_cm;
float descent_speed_cms;
float descent_max_cm;
} nav_payload_place;
bool waiting_to_start; // true if waiting for vehicle to be armed or EKF origin before starting mission
// True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode
bool auto_RTL;
#if AP_SCRIPTING_ENABLED
// nav_script_time command variables
struct {
bool done; // true once lua script indicates it has completed
uint16_t id; // unique id to avoid race conditions between commands and lua scripts
uint32_t start_ms; // system time nav_script_time command was received (used for timeout)
uint8_t command; // command number provided by mission command
uint8_t timeout_s; // timeout (in seconds) provided by mission command
float arg1; // 1st argument provided by mission command
float arg2; // 2nd argument provided by mission command
int16_t arg3; // 3rd argument provided by mission command
int16_t arg4; // 4th argument provided by mission command
} nav_scripting;
#endif
// nav attitude time command variables
struct {
int16_t roll_deg; // target roll angle in degrees. provided by mission command
int8_t pitch_deg; // target pitch angle in degrees. provided by mission command
int16_t yaw_deg; // target yaw angle in degrees. provided by mission command
float climb_rate; // climb rate in m/s. provided by mission command
uint32_t start_ms; // system time that nav attitude time command was received (used for timeout)
} nav_attitude_time;
};
#if AUTOTUNE_ENABLED == ENABLED
/*
wrapper class for AC_AutoTune
*/
#if FRAME_CONFIG == HELI_FRAME
class AutoTune : public AC_AutoTune_Heli
#else
class AutoTune : public AC_AutoTune_Multi
#endif
{
public:
bool init() override;
void run() override;
protected:
bool position_ok() override;
float get_pilot_desired_climb_rate_cms(void) const override;
void get_pilot_desired_rp_yrate_cd(float &roll_cd, float &pitch_cd, float &yaw_rate_cds) override;
void init_z_limits() override;
void log_pids() override;
};
class ModeAutoTune : public Mode {
// ParametersG2 sets a pointer within our autotune object:
friend class ParametersG2;
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::AUTOTUNE; }
bool init(bool ignore_checks) override;
void exit() override;
void run() override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return false; }
bool is_autopilot() const override { return false; }
void save_tuning_gains();
void reset();
protected:
const char *name() const override { return "AUTOTUNE"; }
const char *name4() const override { return "ATUN"; }
private:
AutoTune autotune;
};
#endif
class ModeBrake : public Mode {
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::BRAKE; }
bool init(bool ignore_checks) override;
void run() override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return false; };
bool is_autopilot() const override { return false; }
void timeout_to_loiter_ms(uint32_t timeout_ms);
protected:
const char *name() const override { return "BRAKE"; }
const char *name4() const override { return "BRAK"; }
private:
uint32_t _timeout_start;
uint32_t _timeout_ms;
};
class ModeCircle : public Mode {
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::CIRCLE; }
bool init(bool ignore_checks) override;
void run() override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return false; };
bool is_autopilot() const override { return true; }
protected:
const char *name() const override { return "CIRCLE"; }
const char *name4() const override { return "CIRC"; }
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
private:
// Circle
bool speed_changing = false; // true when the roll stick is being held to facilitate stopping at 0 rate
};
class ModeDrift : public Mode {
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::DRIFT; }
bool init(bool ignore_checks) override;
void run() override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return true; };
bool is_autopilot() const override { return false; }
protected:
const char *name() const override { return "DRIFT"; }
const char *name4() const override { return "DRIF"; }
private:
float get_throttle_assist(float velz, float pilot_throttle_scaled);
};
class ModeFlip : public Mode {
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::FLIP; }
bool init(bool ignore_checks) override;
void run() override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return false; };
bool is_autopilot() const override { return false; }
protected:
const char *name() const override { return "FLIP"; }
const char *name4() const override { return "FLIP"; }
private:
// Flip
Vector3f orig_attitude; // original vehicle attitude before flip
enum class FlipState : uint8_t {
Start,
Roll,
Pitch_A,
Pitch_B,
Recover,
Abandon
};
FlipState _state; // current state of flip
Mode::Number orig_control_mode; // flight mode when flip was initated
uint32_t start_time_ms; // time since flip began
int8_t roll_dir; // roll direction (-1 = roll left, 1 = roll right)
int8_t pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back)
};
#if MODE_FLOWHOLD_ENABLED == ENABLED
/*
class to support FLOWHOLD mode, which is a position hold mode using
optical flow directly, avoiding the need for a rangefinder
*/
class ModeFlowHold : public Mode {
public:
// need a constructor for parameters
ModeFlowHold(void);
Number mode_number() const override { return Number::FLOWHOLD; }
bool init(bool ignore_checks) override;
void run(void) override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override { return true; };
bool is_autopilot() const override { return false; }
bool has_user_takeoff(bool must_navigate) const override {
return !must_navigate;
}
bool allows_flip() const override { return true; }
static const struct AP_Param::GroupInfo var_info[];
protected:
const char *name() const override { return "FLOWHOLD"; }
const char *name4() const override { return "FHLD"; }
private:
// FlowHold states
enum FlowHoldModeState {
FlowHold_MotorStopped,
FlowHold_Takeoff,
FlowHold_Flying,
FlowHold_Landed
};
// calculate attitude from flow data
void flow_to_angle(Vector2f &bf_angle);
LowPassFilterVector2f flow_filter;
bool flowhold_init(bool ignore_checks);
void flowhold_run();
void flowhold_flow_to_angle(Vector2f &angle, bool stick_input);
void update_height_estimate(void);
// minimum assumed height
const float height_min = 0.1f;
// maximum scaling height
const float height_max = 3.0f;
AP_Float flow_max;
AC_PI_2D flow_pi_xy{0.2f, 0.3f, 3000, 5, 0.0025f};
AP_Float flow_filter_hz;
AP_Int8 flow_min_quality;
AP_Int8 brake_rate_dps;
float quality_filtered;
uint8_t log_counter;
bool limited;
Vector2f xy_I;
// accumulated INS delta velocity in north-east form since last flow update
Vector2f delta_velocity_ne;
// last flow rate in radians/sec in north-east axis
Vector2f last_flow_rate_rps;
// timestamp of last flow data
uint32_t last_flow_ms;
float last_ins_height;
float height_offset;
// are we braking after pilot input?
bool braking;
// last time there was significant stick input
uint32_t last_stick_input_ms;
};
#endif // MODE_FLOWHOLD_ENABLED
class ModeGuided : public Mode {
public:
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return Number::GUIDED; }
bool init(bool ignore_checks) override;
void run() override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override;
bool is_autopilot() const override { return true; }
bool has_user_takeoff(bool must_navigate) const override { return true; }
bool in_guided_mode() const override { return true; }
bool requires_terrain_failsafe() const override { return true; }
// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
// IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
// ang_vel: angular velocity (rad/s)
// climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless
// use_thrust: IF true: climb_rate_cms_or_thrust represents thrust
// IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust);
bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false);
bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
bool get_wp(Location &loc) const override;
void set_accel(const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
void set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
bool set_destination_posvelaccel(const Vector3f& destination, const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
// get position, velocity and acceleration targets
const Vector3p& get_target_pos() const;
const Vector3f& get_target_vel() const;