forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmode.h
1286 lines (940 loc) · 40.2 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"
class Mode {
public:
// constructor
Mode(void);
// do not allow copying
Mode(const Mode &other) = delete;
Mode &operator=(const Mode&) = delete;
// child classes should override these methods
virtual bool init(bool ignore_checks) {
return true;
}
virtual void run() = 0;
virtual bool requires_GPS() const = 0;
virtual bool has_manual_throttle() const = 0;
virtual bool allows_arming(bool from_gcs) 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; }
// 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 landing_gear_should_be_deployed() const { return false; }
virtual bool is_landing() const { return false; }
virtual bool get_wp(Location &loc) { 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;}
void update_navigation();
int32_t get_alt_above_ground_cm(void);
// pilot input processing
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const;
float get_pilot_desired_yaw_rate(int16_t stick_angle);
float get_pilot_desired_throttle() const;
const Vector3f& get_desired_velocity() {
// note that position control isn't used in every mode, so
// this may return bogus data:
return pos_control->get_desired_velocity();
}
protected:
// navigation support functions
virtual void run_autopilot() {}
// 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_spool_down();
// functions to control landing
// in modes that support landing
void land_run_horizontal_control();
void land_run_vertical_control(bool pause_descent = false);
// 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 get_climb_rates(float& pilot_climb_rate,
float& takeoff_climb_rate);
bool triggered(float target_climb_rate) const;
bool running() const { return _running; }
private:
bool _running;
float max_speed;
float alt_delta;
uint32_t start_ms;
};
static _TakeOff takeoff;
virtual bool do_user_takeoff_start(float takeoff_alt_cm);
// method shared by both Guided and Auto for takeoff. This is
// waypoint navigation but the user can control the yaw.
void auto_takeoff_run();
void auto_takeoff_set_start_alt(void);
void auto_takeoff_attitude_run(float target_yaw_rate);
// altitude below which we do no navigation in auto takeoff
static float auto_takeoff_no_nav_alt_cm;
public:
// Navigation Yaw control
class AutoYaw {
public:
// yaw(): main product of AutoYaw; the heading:
float yaw();
// mode(): current method of determining desired yaw:
autopilot_yaw_mode mode() const { return (autopilot_yaw_mode)_mode; }
void set_mode_to_default(bool rtl);
void set_mode(autopilot_yaw_mode new_mode);
autopilot_yaw_mode default_mode(bool rtl) const;
// rate_cds(): desired yaw rate in centidegrees/second:
float rate_cds() 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);
private:
float look_ahead_yaw();
float roi_yaw();
// auto flight mode's yaw mode
uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;
// Yaw will point at this location if mode is set to AUTO_YAW_ROI
Vector3f roi;
// bearing from current location to the ROI
float _roi_yaw;
// yaw used for YAW_FIXED yaw_mode
int32_t _fixed_yaw;
// Deg/s we should turn
int16_t _fixed_yaw_slewrate;
// 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 _rate_cds;
// used to reduce update rate to 100hz:
uint8_t roi_yaw_counter;
};
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(control_mode_t mode, mode_reason_t reason);
void set_land_complete(bool b);
GCS_Copter &gcs();
void Log_Write_Event(Log_Event id);
void set_throttle_takeoff(void);
float get_avoidance_adjusted_climbrate(float target_rate);
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;
virtual void run() override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return true; }
bool allows_arming(bool from_gcs) const override { return true; };
bool is_autopilot() const override { return false; }
protected:
const char *name() const override { return "ACRO"; }
const char *name4() const override { return "ACRO"; }
void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
float throttle_hover() const override {
if (g2.acro_thr_mid > 0) {
return g2.acro_thr_mid;
}
return Mode::throttle_hover();
}
private:
};
#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;
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(bool from_gcs) const override { return true; };
bool is_autopilot() const override { return false; }
bool has_user_takeoff(bool must_navigate) const override {
return !must_navigate;
}
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;
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(bool from_gcs) const override { return false; };
bool is_autopilot() const override { return true; }
bool in_guided_mode() const override { return mode() == Auto_NavGuided; }
// Auto
AutoMode mode() const { return _mode; }
bool loiter_start();
void rtl_start();
void takeoff_start(const Location& dest_loc);
void wp_start(const Vector3f& destination, bool terrain_alt);
void wp_start(const Location& dest_loc);
void land_start();
void land_start(const Vector3f& destination);
void circle_movetoedge_start(const Location &circle_center, float radius_m);
void circle_start();
void spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination);
void spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination);
void nav_guided_start();
bool is_landing() const override;
bool landing_gear_should_be_deployed() const override;
bool is_taking_off() const override;
// 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);
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)};
protected:
const char *name() const override { return "AUTO"; }
const char *name4() const override { return "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) override;
void run_autopilot() override;
private:
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
void exit_mission();
void takeoff_run();
void wp_run();
void spline_run();
void land_run();
void rtl_run();
void circle_run();
void nav_guided_run();
void loiter_run();
void loiter_to_alt_run();
Location loc_from_cmd(const AP_Mission::Mission_Command& cmd) const;
void payload_place_start(const Vector3f& destination);
void payload_place_run();
bool payload_place_run_should_run();
void payload_place_run_loiter();
void payload_place_run_descend();
void payload_place_run_release();
AutoMode _mode = Auto_TakeOff; // controls which auto controller is run
Location terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const;
void do_takeoff(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
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);
#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);
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();
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);
// 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
int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t nav_delay_time_start;
// Delay Mission Scripting Command
int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
uint32_t condition_start;
LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
struct {
PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...)
uint32_t hover_start_timestamp; // milliseconds
float hover_throttle_level;
uint32_t descend_start_timestamp; // milliseconds
uint32_t place_start_timestamp; // milliseconds
float descend_throttle_level;
float descend_start_altitude;
float descend_max; // centimetres
} nav_payload_place;
};
#if AUTOTUNE_ENABLED == ENABLED
/*
wrapper class for AC_AutoTune
*/
class AutoTune : public AC_AutoTune
{
public:
bool init() override;
void run() override;
protected:
bool start(void) override;
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_Write_Event(enum at_event id) override;
void log_pids() override;
};
class ModeAutoTune : public Mode {
public:
// inherit constructor
using Mode::Mode;
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(bool from_gcs) const override { return false; }
bool is_autopilot() const override { return false; }
void save_tuning_gains();
void stop();
void reset();
protected:
const char *name() const override { return "AUTOTUNE"; }
const char *name4() const override { return "ATUN"; }
};
#endif
class ModeBrake : public Mode {
public:
// inherit constructor
using Mode::Mode;
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(bool from_gcs) 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:
void init_target();
uint32_t _timeout_start;
uint32_t _timeout_ms;
};
class ModeCircle : public Mode {
public:
// inherit constructor
using Mode::Mode;
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(bool from_gcs) 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 pilot_yaw_override = false; // true if pilot is overriding yaw
};
class ModeDrift : public Mode {
public:
// inherit constructor
using Mode::Mode;
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(bool from_gcs) 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;
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(bool from_gcs) 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
control_mode_t 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 !HAL_MINIMIZE_FEATURES && OPTFLOW == 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);
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(bool from_gcs) const override { return true; };
bool is_autopilot() const override { return false; }
bool has_user_takeoff(bool must_navigate) const override {
return !must_navigate;
}
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 // OPTFLOW
class ModeGuided : public Mode {
public:
// inherit constructor
using Mode::Mode;
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(bool from_gcs) const override { return from_gcs; }
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; }
void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
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 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) override;
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);
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);
void limit_clear();
void limit_init_time_and_pos();
void limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
bool limit_check();
bool is_taking_off() const override;
bool do_user_takeoff_start(float final_alt_above_home) override;
GuidedMode mode() const { return guided_mode; }
void angle_control_start();
void angle_control_run();
protected:
const char *name() const override { return "GUIDED"; }
const char *name4() const override { return "GUID"; }
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
float crosstrack_error() const override;
private:
void pos_control_start();
void vel_control_start();
void posvel_control_start();
void takeoff_run();
void pos_control_run();
void vel_control_run();
void posvel_control_run();
void set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des);
void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
// controls which controller is run (pos or vel):
GuidedMode guided_mode = Guided_TakeOff;
};
class ModeGuidedNoGPS : public ModeGuided {
public:
// inherit constructor
using ModeGuided::Mode;
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(bool from_gcs) const override { return from_gcs; }
bool is_autopilot() const override { return true; }
protected:
const char *name() const override { return "GUIDED_NOGPS"; }
const char *name4() const override { return "GNGP"; }
private:
};
class ModeLand : public Mode {
public:
// inherit constructor
using Mode::Mode;
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(bool from_gcs) const override { return false; };
bool is_autopilot() const override { return true; }
bool is_landing() const override { return true; };
bool landing_gear_should_be_deployed() const override { return true; };
void do_not_use_GPS();
protected:
const char *name() const override { return "LAND"; }
const char *name4() const override { return "LAND"; }
private:
void gps_run();
void nogps_run();
};
class ModeLoiter : public Mode {
public:
// inherit constructor
using Mode::Mode;
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(bool from_gcs) const override { return true; };
bool is_autopilot() const override { return false; }
bool has_user_takeoff(bool must_navigate) const override { return true; }
#if PRECISION_LANDING == ENABLED
void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
#endif
protected:
const char *name() const override { return "LOITER"; }
const char *name4() const override { return "LOIT"; }
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
#if PRECISION_LANDING == ENABLED
bool do_precision_loiter();
void precision_loiter_xy();
#endif
private:
#if PRECISION_LANDING == ENABLED
bool _precision_loiter_enabled;
#endif
};
class ModePosHold : public Mode {
public:
// inherit constructor
using Mode::Mode;
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(bool from_gcs) const override { return true; };
bool is_autopilot() const override { return false; }
bool has_user_takeoff(bool must_navigate) const override { return true; }
protected:
const char *name() const override { return "POSHOLD"; }
const char *name4() const override { return "PHLD"; }
private:
void update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw);
float mix_controls(float mix_ratio, float first_control, float second_control);
void update_brake_angle_from_velocity(float &brake_angle, float velocity);
void update_wind_comp_estimate();
void get_wind_comp_lean_angles(float &roll_angle, float &pitch_angle);
void roll_controller_to_pilot_override();
void pitch_controller_to_pilot_override();
enum class RPMode {
PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch)
BRAKE, // this axis is braking towards zero
BRAKE_READY_TO_LOITER, // this axis has completed braking and is ready to enter loiter mode (both modes must be this value before moving to next stage)
BRAKE_TO_LOITER, // both vehicle's axis (roll and pitch) are transitioning from braking to loiter mode (braking and loiter controls are mixed)
LOITER, // both vehicle axis are holding position
CONTROLLER_TO_PILOT_OVERRIDE // pilot has input controls on this axis and this axis is transitioning to pilot override (other axis will transition to brake if no pilot input)
};
RPMode roll_mode;
RPMode pitch_mode;
uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
// pilot input related variables
float pilot_roll; // pilot requested roll angle (filtered to slow returns to zero)
float pilot_pitch; // pilot requested roll angle (filtered to slow returns to zero)
// braking related variables
float brake_gain; // gain used during conversion of vehicle's velocity to lean angle during braking (calculated from brake_rate)
float brake_roll; // target roll angle during braking periods
float brake_pitch; // target pitch angle during braking periods
int16_t brake_timeout_roll; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
int16_t brake_timeout_pitch; // number of cycles allowed for the braking to complete, this timeout will be updated at half-braking
float brake_angle_max_roll; // maximum lean angle achieved during braking. Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
float brake_angle_max_pitch; // maximum lean angle achieved during braking Used to determine when the vehicle has begun to flatten out so that we can re-estimate the braking time
int16_t brake_to_loiter_timer; // cycles to mix brake and loiter controls in POSHOLD_BRAKE_TO_LOITER
// loiter related variables
int16_t controller_to_pilot_timer_roll; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
int16_t controller_to_pilot_timer_pitch; // cycles to mix controller and pilot controls in POSHOLD_CONTROLLER_TO_PILOT
float controller_final_roll; // final roll angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
float controller_final_pitch; // final pitch angle from controller as we exit brake or loiter mode (used for mixing with pilot input)
// wind compensation related variables
Vector2f wind_comp_ef; // wind compensation in earth frame, filtered lean angles from position controller
float wind_comp_roll; // roll angle to compensate for wind
float wind_comp_pitch; // pitch angle to compensate for wind
uint16_t wind_comp_start_timer; // counter to delay start of wind compensation for a short time after loiter is engaged
int8_t wind_comp_timer; // counter to reduce wind comp roll/pitch lean angle calcs to 10hz
// final output
float roll; // final roll angle sent to attitude controller
float pitch; // final pitch angle sent to attitude controller
};
class ModeRTL : public Mode {
public:
// inherit constructor
using Mode::Mode;
bool init(bool ignore_checks) override;
void run() override {
return run(true);
}
void run(bool disarm_on_land);
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return false; };
bool is_autopilot() const override { return true; }
RTLState state() { return _state; }
// this should probably not be exposed
bool state_complete() { return _state_complete; }
bool is_landing() const override;
bool landing_gear_should_be_deployed() const override;
void restart_without_terrain();
protected:
const char *name() const override { return "RTL"; }
const char *name4() const override { return "RTL "; }
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
float crosstrack_error() const override { return wp_nav->crosstrack_error();}
void descent_start();