-
Notifications
You must be signed in to change notification settings - Fork 16.6k
/
Plane.h
1282 lines (1056 loc) · 42.6 KB
/
Plane.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
/*
Lead developer: Andrew Tridgell & Tom Pittenger
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon
Please contribute your ideas! See http://dev.ardupilot.com for details
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
#include <cmath>
#include <stdarg.h>
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Param/AP_Param.h>
#include <StorageManager/StorageManager.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor Library
#include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
#include <AP_AHRS/AP_AHRS.h> // ArduPilot Mega DCM Library
#include <SRV_Channel/SRV_Channel.h>
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
#include <Filter/Filter.h> // Filter library
#include <AP_Camera/AP_Camera.h> // Photo or video camera
#include <AP_Terrain/AP_Terrain.h>
#include <AP_RPM/AP_RPM.h>
#include <AP_Beacon/AP_Beacon.h>
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
#include <APM_Control/APM_Control.h>
#include <APM_Control/AP_AutoTune.h>
#include <GCS_MAVLink/GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_Logger/AP_Logger.h>
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
#include <AP_Scheduler/PerfInfo.h> // loop perf monitoring
#include <AP_Navigation/AP_Navigation.h>
#include <AP_L1_Control/AP_L1_Control.h>
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_TECS/AP_TECS.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_Mission/AP_Mission.h> // Mission command library
#include <AP_Soaring/AP_Soaring.h>
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
#include <AP_Arming/AP_Arming.h>
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
#include <AP_OSD/AP_OSD.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
#include <AP_Parachute/AP_Parachute.h>
#include <AP_ADSB/AP_ADSB.h>
#include <AP_ICEngine/AP_ICEngine.h>
#include <AP_Landing/AP_Landing.h>
#include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
#include <AP_Follow/AP_Follow.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h>
#if AP_EXTERNAL_CONTROL_ENABLED
#include "AP_ExternalControl_Plane.h"
#endif
#include <AC_PrecLand/AC_PrecLand_config.h>
#if AC_PRECLAND_ENABLED
# include <AC_PrecLand/AC_PrecLand.h>
#endif
#include "GCS_Mavlink.h"
#include "GCS_Plane.h"
#include "quadplane.h"
#include <AP_Tuning/AP_Tuning_config.h>
#if AP_TUNING_ENABLED
#include "tuning.h"
#endif
// Configuration
#include "config.h"
#if AP_ADVANCEDFAILSAFE_ENABLED
#include "afs_plane.h"
#endif
// Local modules
#include "defines.h"
#include "mode.h"
#if AP_SCRIPTING_ENABLED
#include <AP_Scripting/AP_Scripting.h>
#endif
#include "RC_Channel.h" // RC Channel Library
#include "Parameters.h"
#if HAL_ADSB_ENABLED
#include "avoidance_adsb.h"
#endif
#include "AP_Arming.h"
/*
main APM:Plane class
*/
class Plane : public AP_Vehicle {
public:
friend class GCS_MAVLINK_Plane;
friend class Parameters;
friend class ParametersG2;
friend class AP_Arming_Plane;
friend class QuadPlane;
friend class QAutoTune;
friend class AP_Tuning_Plane;
friend class AP_AdvancedFailsafe_Plane;
friend class AP_Avoidance_Plane;
friend class GCS_Plane;
friend class RC_Channel_Plane;
friend class RC_Channels_Plane;
friend class Tailsitter;
friend class Tiltrotor;
friend class SLT_Transition;
friend class Tailsitter_Transition;
friend class VTOL_Assist;
friend class Mode;
friend class ModeCircle;
friend class ModeStabilize;
friend class ModeTraining;
friend class ModeAcro;
friend class ModeFBWA;
friend class ModeFBWB;
friend class ModeCruise;
friend class ModeAutoTune;
friend class ModeAuto;
friend class ModeRTL;
friend class ModeLoiter;
friend class ModeAvoidADSB;
friend class ModeGuided;
friend class ModeInitializing;
friend class ModeManual;
friend class ModeQStabilize;
friend class ModeQHover;
friend class ModeQLoiter;
friend class ModeQLand;
friend class ModeQRTL;
friend class ModeQAcro;
friend class ModeQAutotune;
friend class ModeTakeoff;
friend class ModeThermal;
friend class ModeLoiterAltQLand;
#if AP_EXTERNAL_CONTROL_ENABLED
friend class AP_ExternalControl_Plane;
#endif
Plane(void);
private:
// key aircraft parameters passed to multiple libraries
AP_FixedWing aparm;
// Global parameters are all contained within the 'g' and 'g2' classes.
Parameters g;
ParametersG2 g2;
// mapping between input channels
RCMapper rcmap;
// primary input channels
RC_Channel *channel_roll;
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
RC_Channel *channel_rudder;
RC_Channel *channel_flap;
RC_Channel *channel_airbrake;
// scaled roll limit based on pitch
int32_t roll_limit_cd;
float pitch_limit_min;
// flight modes convenience array
AP_Int8 *flight_modes = &g.flight_mode1;
const uint8_t num_flight_modes = 6;
AP_FixedWing::Rangefinder_State rangefinder_state;
#if AP_RPM_ENABLED
AP_RPM rpm_sensor;
#endif
AP_TECS TECS_controller{ahrs, aparm, landing, MASK_LOG_TECS};
AP_L1_Control L1_controller{ahrs, &TECS_controller};
// Attitude to servo controllers
AP_RollController rollController{aparm};
AP_PitchController pitchController{aparm};
AP_YawController yawController{aparm};
AP_SteerController steerController{};
// Training mode
bool training_manual_roll; // user has manual roll control
bool training_manual_pitch; // user has manual pitch control
// should throttle be pass-thru in guided?
bool guided_throttle_passthru;
// are we doing calibration? This is used to allow heartbeat to
// external failsafe boards during baro and airspeed calibration
bool in_calibration;
// are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached
bool long_failsafe_pending;
// GCS selection
GCS_Plane _gcs; // avoid using this; use gcs()
GCS_Plane &gcs() { return _gcs; }
// selected navigation controller
AP_Navigation *nav_controller = &L1_controller;
// Camera
#if AP_CAMERA_ENABLED
AP_Camera camera{MASK_LOG_CAMERA};
#endif
#if AP_OPTICALFLOW_ENABLED
// Optical flow sensor
AP_OpticalFlow optflow;
#endif
#if HAL_RALLY_ENABLED
// Rally Points
AP_Rally rally;
#endif
#if AC_PRECLAND_ENABLED
void precland_update(void);
#endif
// returns a Location for a rally point or home; if
// HAL_RALLY_ENABLED is false, just home.
Location calc_best_rally_or_home_location(const Location ¤t_loc, float rtl_home_alt_amsl_cm) const;
#if OSD_ENABLED || OSD_PARAM_ENABLED
AP_OSD osd;
#endif
ModeCircle mode_circle;
ModeStabilize mode_stabilize;
ModeTraining mode_training;
ModeAcro mode_acro;
ModeFBWA mode_fbwa;
ModeFBWB mode_fbwb;
ModeCruise mode_cruise;
ModeAutoTune mode_autotune;
ModeAuto mode_auto;
ModeRTL mode_rtl;
ModeLoiter mode_loiter;
#if HAL_ADSB_ENABLED
ModeAvoidADSB mode_avoidADSB;
#endif
ModeGuided mode_guided;
ModeInitializing mode_initializing;
ModeManual mode_manual;
#if HAL_QUADPLANE_ENABLED
ModeQStabilize mode_qstabilize;
ModeQHover mode_qhover;
ModeQLoiter mode_qloiter;
ModeQLand mode_qland;
ModeQRTL mode_qrtl;
ModeQAcro mode_qacro;
ModeLoiterAltQLand mode_loiter_qland;
#if QAUTOTUNE_ENABLED
ModeQAutotune mode_qautotune;
#endif // QAUTOTUNE_ENABLED
#endif // HAL_QUADPLANE_ENABLED
ModeTakeoff mode_takeoff;
#if HAL_SOARING_ENABLED
ModeThermal mode_thermal;
#endif
// This is the state of the flight control system
// There are multiple states defined such as MANUAL, FBW-A, AUTO
Mode *control_mode = &mode_initializing;
Mode *previous_mode = &mode_initializing;
// time of last mode change
uint32_t last_mode_change_ms;
// Used to maintain the state of the previous control switch position
// This is set to 254 when we need to re-read the switch
uint8_t oldSwitchPosition = 254;
// This is used to enable the inverted flight feature
bool inverted_flight;
// last time we ran roll/pitch stabilization
uint32_t last_stabilize_ms;
// Failsafe
struct {
// Used to track if the value on channel 3 (throttle) has fallen below the failsafe threshold
// RC receiver should be set up to output a low throttle value when signal is lost
bool rc_failsafe;
// true if an adsb related failsafe has occurred
bool adsb;
// saved flight mode
enum Mode::Number saved_mode_number;
// A tracking variable for type of failsafe active
// Used for failsafe based on loss of RC signal or GCS signal
int16_t state;
// number of low throttle values
uint8_t throttle_counter;
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
uint32_t short_timer_ms;
uint32_t last_valid_rc_ms;
//keeps track of the last valid rc as it relates to the AFS system
//Does not count rc inputs as valid if the standard failsafe is on
uint32_t AFS_last_valid_rc_ms;
} failsafe;
enum Landing_ApproachStage {
RTL,
LOITER_TO_ALT,
ENSURE_RADIUS,
WAIT_FOR_BREAKOUT,
APPROACH_LINE,
VTOL_LANDING,
};
#if HAL_QUADPLANE_ENABLED
// Landing
struct {
enum Landing_ApproachStage approach_stage;
float approach_direction_deg;
} vtol_approach_s;
#endif
bool any_failsafe_triggered() {
return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb;
}
// A counter used to count down valid gps fixes to allow the gps estimate to settle
// before recording our home position (and executing a ground start if we booted with an air start)
uint8_t ground_start_count = 5;
// true if we have a position estimate from AHRS
bool have_position;
// Airspeed
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
// Also used for flap deployment criteria. Centimeters per second.
int32_t target_airspeed_cm;
int32_t new_airspeed_cm = -1; //temp variable for AUTO and GUIDED mode speed changes
// The difference between current and desired airspeed. Used in the pitch controller. Meters per second.
float airspeed_error;
// An amount that the airspeed should be increased in auto modes based on the user positioning the
// throttle stick in the top half of the range. Centimeters per second.
int16_t airspeed_nudge_cm;
// Similar to airspeed_nudge, but used when no airspeed sensor.
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
int16_t throttle_nudge;
// Ground speed
// The amount current ground speed is below min ground speed. Centimeters per second
int32_t groundspeed_undershoot;
bool groundspeed_undershoot_is_valid;
// Difference between current altitude and desired altitude. Centimeters
int32_t altitude_error_cm;
// speed scaler for control surfaces, updated at 10Hz
float surface_speed_scaler = 1.0;
// Battery Sensors
AP_BattMonitor battery{MASK_LOG_CURRENT,
FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t),
_failsafe_priorities};
struct {
uint32_t last_tkoff_arm_time;
uint32_t last_check_ms;
uint32_t rudder_takeoff_warn_ms;
uint32_t last_report_ms;
bool launchTimerStarted;
uint8_t accel_event_counter;
uint32_t accel_event_ms;
uint32_t start_time_ms;
bool waiting_for_rudder_neutral;
} takeoff_state;
// ground steering controller state
struct {
// Direction held during phases of takeoff and landing centidegrees
// A value of -1 indicates the course has not been set/is not in use
// this is a 0..36000 value, or -1 for disabled
int32_t hold_course_cd = -1;
// locked_course and locked_course_cd are used in stabilize mode
// when ground steering is active, and for steering in auto-takeoff
bool locked_course;
float locked_course_err;
uint32_t last_steer_ms;
} steer_state;
// flight mode specific
struct {
// Altitude threshold to complete a takeoff command in autonomous
// modes. Centimeters above home
int32_t takeoff_altitude_rel_cm;
// Begin leveling out the enforced takeoff pitch angle min at this height to reduce/eliminate overshoot
int32_t height_below_takeoff_to_level_off_cm;
// the highest airspeed we have reached since entering AUTO. Used
// to control ground takeoff
float highest_airspeed;
// turn angle for next leg of mission
float next_turn_angle {90};
// filtered sink rate for landing
float sink_rate;
// time when we first pass min GPS speed on takeoff
uint32_t takeoff_speed_time_ms;
// distance to next waypoint
float wp_distance;
// proportion to next waypoint
float wp_proportion;
// last time is_flying() returned true in milliseconds
uint32_t last_flying_ms;
// time stamp of when we start flying while in auto mode in milliseconds
uint32_t started_flying_in_auto_ms;
// barometric altitude at start of takeoff
float baro_takeoff_alt;
// initial pitch. Used to detect if nose is rising in a tail dragger
int16_t initial_pitch_cd;
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
int16_t takeoff_pitch_cd;
// used to 'wiggle' servos in idle mode to prevent them freezing
// at high altitudes
uint8_t idle_wiggle_stage;
// Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process.
bool takeoff_complete;
// are we headed to the land approach waypoint? Works for any nav type
bool wp_is_land_approach;
// should we fly inverted?
bool inverted_flight;
// should we enable cross-tracking for the next waypoint?
bool next_wp_crosstrack;
// should we use cross-tracking for this waypoint?
bool crosstrack;
// in FBWA taildragger takeoff mode
bool fbwa_tdrag_takeoff_mode;
// have we checked for an auto-land?
bool checked_for_autoland;
// are we in VTOL mode in AUTO?
bool vtol_mode;
// are we doing loiter mode as a VTOL?
bool vtol_loiter;
// how much correction have we added for terrain data
float terrain_correction;
// last home altitude for detecting changes
int32_t last_home_alt_cm;
} auto_state;
#if AP_SCRIPTING_ENABLED
// support for scripting nav commands, with verify
struct {
bool enabled;
uint16_t id;
float roll_rate_dps;
float pitch_rate_dps;
float yaw_rate_dps;
float throttle_pct;
uint32_t start_ms;
uint32_t current_ms;
float rudder_offset_pct;
bool run_yaw_rate_controller;
} nav_scripting;
#endif
struct {
// roll pitch yaw commanded from external controller in centidegrees
Vector3l forced_rpy_cd;
// last time we heard from the external controller
Vector3l last_forced_rpy_ms;
// throttle commanded from external controller in percent
float forced_throttle;
uint32_t last_forced_throttle_ms;
#if OFFBOARD_GUIDED == ENABLED
// airspeed adjustments
float target_airspeed_cm = -1; // don't default to zero here, as zero is a valid speed.
float target_airspeed_accel;
uint32_t target_airspeed_time_ms;
// altitude adjustments
float target_alt = -1; // don't default to zero here, as zero is a valid alt.
uint32_t last_target_alt = 0;
float target_alt_accel;
uint32_t target_alt_time_ms = 0;
uint8_t target_alt_frame = 0;
// heading track
float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians
float target_heading_accel_limit;
uint32_t target_heading_time_ms;
guided_heading_type_t target_heading_type;
bool target_heading_limit;
#endif // OFFBOARD_GUIDED == ENABLED
} guided_state;
#if AP_LANDINGGEAR_ENABLED
// landing gear state
struct {
AP_FixedWing::FlightStage last_flight_stage;
} gear;
#endif
struct {
// on hard landings, only check once after directly a landing so you
// don't trigger a crash when picking up the aircraft
bool checkedHardLanding;
// crash detection. True when we are crashed
bool is_crashed;
// impact detection flag. Expires after a few seconds via impact_timer_ms
bool impact_detected;
// debounce timer
uint32_t debounce_timer_ms;
// delay time for debounce to count to
uint32_t debounce_time_total_ms;
// length of time impact_detected has been true. Times out after a few seconds. Used to clip isFlyingProbability
uint32_t impact_timer_ms;
} crash_state;
// this controls throttle suppression in auto modes
bool throttle_suppressed;
#if AP_BATTERY_WATT_MAX_ENABLED
// reduce throttle to eliminate battery over-current
int8_t throttle_watt_limit_max;
int8_t throttle_watt_limit_min; // for reverse thrust
uint32_t throttle_watt_limit_timer_ms;
#endif
AP_FixedWing::FlightStage flight_stage = AP_FixedWing::FlightStage::NORMAL;
// probability of aircraft is currently in flight. range from 0 to
// 1 where 1 is 100% sure we're in flight
float isFlyingProbability;
// previous value of is_flying()
bool previous_is_flying;
// time since started flying in any mode in milliseconds
uint32_t started_flying_ms;
// ground mode is true when disarmed and not flying
bool ground_mode;
// Navigation control variables
// The instantaneous desired bank angle. Hundredths of a degree
int32_t nav_roll_cd;
// The instantaneous desired pitch angle. Hundredths of a degree
int32_t nav_pitch_cd;
// the aerodynamic load factor. This is calculated from the demanded
// roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
float aerodynamic_load_factor = 1.0f;
// a smoothed airspeed estimate, used for limiting roll angle
float smoothed_airspeed;
// Mission library
AP_Mission mission{
FUNCTOR_BIND_MEMBER(&Plane::start_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Plane::verify_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Plane::exit_mission_callback, void)};
#if PARACHUTE == ENABLED
AP_Parachute parachute;
#endif
// terrain handling
#if AP_TERRAIN_AVAILABLE
AP_Terrain terrain;
#endif
AP_Landing landing{mission,ahrs,&TECS_controller,nav_controller,aparm,
FUNCTOR_BIND_MEMBER(&Plane::set_target_altitude_proportion, void, const Location&, float),
FUNCTOR_BIND_MEMBER(&Plane::constrain_target_altitude_location, void, const Location&, const Location&),
FUNCTOR_BIND_MEMBER(&Plane::adjusted_altitude_cm, int32_t),
FUNCTOR_BIND_MEMBER(&Plane::adjusted_relative_altitude_cm, int32_t),
FUNCTOR_BIND_MEMBER(&Plane::disarm_if_autoland_complete, void),
FUNCTOR_BIND_MEMBER(&Plane::update_flight_stage, void)};
#if HAL_ADSB_ENABLED
AP_ADSB adsb;
// avoidance of adsb enabled vehicles (normally manned vehicles)
AP_Avoidance_Plane avoidance_adsb{adsb};
#endif
// Outback Challenge Failsafe Support
#if AP_ADVANCEDFAILSAFE_ENABLED
AP_AdvancedFailsafe_Plane afs;
#endif
/*
meta data to support counting the number of circles in a loiter
*/
struct {
// previous target bearing, used to update sum_cd
int32_t old_target_bearing_cd;
// Total desired rotation in a loiter. Used for Loiter Turns commands.
int32_t total_cd;
// total angle completed in the loiter so far
int32_t sum_cd;
// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
int8_t direction;
// when loitering and an altitude is involved, this flag is true when it has been reached at least once
bool reached_target_alt;
// check for scenarios where updrafts can keep you from loitering down indefinitely.
bool unable_to_acheive_target_alt;
// start time of the loiter. Milliseconds.
uint32_t start_time_ms;
// altitude at start of loiter loop lap. Used to detect delta alt of each lap.
// only valid when sum_cd > 36000
int32_t start_lap_alt_cm;
int32_t next_sum_lap_cd;
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
uint32_t time_max_ms;
// current value of loiter radius in metres used by the controller
float radius;
} loiter;
// Conditional command
// A value used in condition commands (eg delay, change alt, etc.)
// For example in a change altitude command, it is the altitude to change to.
int32_t condition_value;
// A starting value used to check the status of a conditional command.
// For example in a delay command the condition_start records that start time for the delay
uint32_t condition_start;
// A value used in condition commands. For example the rate at which to change altitude.
int16_t condition_rate;
// 3D Location vectors
// Location structure defined in AP_Common
const Location &home = ahrs.get_home();
// The location of the previous waypoint. Used for track following and altitude ramp calculations
Location prev_WP_loc {};
// The plane's current location
Location current_loc {};
// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
Location next_WP_loc {};
// Altitude control
struct {
// target altitude above sea level in cm. Used for barometric
// altitude navigation
int32_t amsl_cm;
// Altitude difference between previous and current waypoint in
// centimeters. Used for glide slope handling
int32_t offset_cm;
#if AP_TERRAIN_AVAILABLE
// are we trying to follow terrain?
bool terrain_following;
// are we waiting to load terrain data to init terrain following
bool terrain_following_pending;
// target altitude above terrain in cm, valid if terrain_following
// is set
int32_t terrain_alt_cm;
// lookahead value for height error reporting
float lookahead;
#endif
// last input for FBWB/CRUISE height control
float last_elevator_input;
// last time we checked for pilot control of height
uint32_t last_elev_check_us;
} target_altitude {};
float relative_altitude;
// loop performance monitoring:
AP::PerfInfo perf_info;
struct {
uint32_t last_trim_check;
uint32_t last_trim_save;
} auto_trim;
struct {
bool done_climb;
} rtl;
// last time home was updated while disarmed
uint32_t last_home_update_ms;
// Camera/Antenna mount tracking and stabilisation stuff
#if HAL_MOUNT_ENABLED
AP_Mount camera_mount;
#endif
// Arming/Disarming management class
AP_Arming_Plane arming;
AP_Param param_loader {var_info};
// external control library
#if AP_EXTERNAL_CONTROL_ENABLED
AP_ExternalControl_Plane external_control;
#endif
static const AP_Scheduler::Task scheduler_tasks[];
static const AP_Param::Info var_info[];
// time that rudder arming has been running
uint32_t rudder_arm_timer;
// have we seen neutral rudder since arming with rudder?
bool seen_neutral_rudder;
#if HAL_QUADPLANE_ENABLED
// support for quadcopter-plane
QuadPlane quadplane{ahrs};
#endif
#if AP_TUNING_ENABLED
// support for transmitter tuning
AP_Tuning_Plane tuning;
#endif
static const struct LogStructure log_structure[];
// rudder mixing gain for differential thrust (0 - 1)
float rudder_dt;
// soaring mode-change timer
uint32_t soaring_mode_timer_ms;
// terrain disable for non AUTO modes, set with an RC Option switch
bool non_auto_terrain_disable;
bool terrain_disabled();
#if AP_TERRAIN_AVAILABLE
bool terrain_enabled_in_current_mode() const;
bool terrain_enabled_in_mode(Mode::Number num) const;
enum class terrain_bitmask {
ALL = 1U << 0,
FLY_BY_WIRE_B = 1U << 1,
CRUISE = 1U << 2,
AUTO = 1U << 3,
RTL = 1U << 4,
AVOID_ADSB = 1U << 5,
GUIDED = 1U << 6,
LOITER = 1U << 7,
CIRCLE = 1U << 8,
QRTL = 1U << 9,
QLAND = 1U << 10,
QLOITER = 1U << 11,
};
struct TerrainLookupTable{
Mode::Number mode_num;
terrain_bitmask bitmask;
};
static const TerrainLookupTable Terrain_lookup[];
#endif
// Attitude.cpp
void adjust_nav_pitch_throttle(void);
void update_load_factor(void);
void adjust_altitude_target();
void setup_glide_slope(void);
int32_t get_RTL_altitude_cm() const;
float relative_ground_altitude(bool use_rangefinder_if_available);
float relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available);
void set_target_altitude_current(void);
void set_target_altitude_current_adjusted(void);
void set_target_altitude_location(const Location &loc);
int32_t relative_target_altitude_cm(void);
void change_target_altitude(int32_t change_cm);
void set_target_altitude_proportion(const Location &loc, float proportion);
void constrain_target_altitude_location(const Location &loc1, const Location &loc2);
int32_t calc_altitude_error_cm(void);
void check_fbwb_altitude(void);
void reset_offset_altitude(void);
void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
bool above_location_current(const Location &loc);
void setup_terrain_target_alt(Location &loc);
int32_t adjusted_altitude_cm(void);
int32_t adjusted_relative_altitude_cm(void);
float mission_alt_offset(void);
float height_above_target(void);
float lookahead_adjustment(void);
float rangefinder_correction(void);
void rangefinder_height_update(void);
void rangefinder_terrain_correction(float &height);
void stabilize();
void calc_throttle();
void calc_nav_roll();
void calc_nav_pitch();
float calc_speed_scaler(void);
float get_speed_scaler(void) const { return surface_speed_scaler; }
bool stick_mixing_enabled(void);
void stabilize_roll();
float stabilize_roll_get_roll_out();
void stabilize_pitch();
float stabilize_pitch_get_pitch_out();
void stabilize_stick_mixing_fbw();
void stabilize_yaw();
int16_t calc_nav_yaw_coordinated();
int16_t calc_nav_yaw_course(void);
int16_t calc_nav_yaw_ground(void);
#if HAL_LOGGING_ENABLED
// methods for AP_Vehicle:
const AP_Int32 &get_log_bitmask() override { return g.log_bitmask; }
const struct LogStructure *get_log_structures() const override {
return log_structure;
}
uint8_t get_num_log_structures() const override;
// Log.cpp
void Log_Write_FullRate(void);
void Log_Write_Attitude(void);
void Log_Write_Control_Tuning();
void Log_Write_OFG_Guided();
void Log_Write_Guided(void);
void Log_Write_Nav_Tuning();
void Log_Write_Status();
void Log_Write_RC(void);
void Log_Write_Vehicle_Startup_Messages();
void Log_Write_AETR();
void log_init();
#endif
// Parameters.cpp
void load_parameters(void) override;
// commands_logic.cpp
void set_next_WP(const Location &loc);
void do_RTL(int32_t alt);
bool verify_takeoff();
bool verify_loiter_unlim(const AP_Mission::Mission_Command &cmd);
bool verify_loiter_time();
bool verify_loiter_turns(const AP_Mission::Mission_Command &cmd);
bool verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd);
bool verify_RTL();
bool verify_continue_and_change_alt();
bool verify_wait_delay();
bool verify_within_distance();
bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd);
void do_loiter_at_location();
bool verify_loiter_heading(bool init);
void exit_mission_callback();
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
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);
#if HAL_QUADPLANE_ENABLED
void do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
#endif
void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
void do_vtol_land(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
#if HAL_QUADPLANE_ENABLED
bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
#endif
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);
bool do_change_speed(const AP_Mission::Mission_Command& cmd);
void do_set_home(const AP_Mission::Mission_Command& cmd);
bool start_command_callback(const AP_Mission::Mission_Command &cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
float get_wp_radius() const;
bool is_land_command(uint16_t cmd) const;
bool do_change_speed(uint8_t speedtype, float speed_target_ms, float rhtottle_pct);
/*
return true if in a specific AUTO mission command
*/
bool in_auto_mission_id(uint16_t command) const;
#if AP_SCRIPTING_ENABLED
// nav scripting support
void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
bool verify_nav_script_time(const AP_Mission::Mission_Command& cmd);
#endif
// commands.cpp
void set_guided_WP(const Location &loc);
// update home position. Return true if update done
bool update_home();
// update current_loc
void update_current_loc(void);
// set home location and store it persistently:
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
// control_modes.cpp
void read_control_switch();
uint8_t readSwitch(void) const;
void autotune_start(void);
void autotune_restore(void);
void autotune_enable(bool enable);
bool fly_inverted(void);
bool mode_allows_autotuning(void);