Line data Source code
1 : /*
2 : Lead developer: Andrew Tridgell & Tom Pittenger
3 :
4 : 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
5 : 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
6 :
7 : Please contribute your ideas! See http://dev.ardupilot.com for details
8 :
9 : This program is free software: you can redistribute it and/or modify
10 : it under the terms of the GNU General Public License as published by
11 : the Free Software Foundation, either version 3 of the License, or
12 : (at your option) any later version.
13 :
14 : This program is distributed in the hope that it will be useful,
15 : but WITHOUT ANY WARRANTY; without even the implied warranty of
16 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 : GNU General Public License for more details.
18 :
19 : You should have received a copy of the GNU General Public License
20 : along with this program. If not, see <http://www.gnu.org/licenses/>.
21 : */
22 : #pragma once
23 :
24 : ////////////////////////////////////////////////////////////////////////////////
25 : // Header includes
26 : ////////////////////////////////////////////////////////////////////////////////
27 :
28 : #include <cmath>
29 : #include <stdarg.h>
30 : #include <stdio.h>
31 :
32 : #include <AP_HAL/AP_HAL.h>
33 : #include <AP_Common/AP_Common.h>
34 : #include <AP_Airspeed/AP_Airspeed.h>
35 : #include <AP_Param/AP_Param.h>
36 : #include <StorageManager/StorageManager.h>
37 : #include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
38 : #include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor Library
39 : #include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
40 : #include <AP_AHRS/AP_AHRS.h> // ArduPilot Mega DCM Library
41 : #include <SRV_Channel/SRV_Channel.h>
42 : #include <AP_RangeFinder/AP_RangeFinder_config.h> // Range finder library
43 : #include <Filter/Filter.h> // Filter library
44 : #include <AP_Camera/AP_Camera.h> // Photo or video camera
45 : #include <AP_Terrain/AP_Terrain.h>
46 : #include <AP_RPM/AP_RPM.h>
47 : #include <AP_Beacon/AP_Beacon.h>
48 :
49 : #include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
50 : #include <APM_Control/APM_Control.h>
51 : #include <APM_Control/AP_AutoTune.h>
52 : #include <GCS_MAVLink/GCS_MAVLink.h> // MAVLink GCS definitions
53 : #include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
54 : #include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
55 : #include <AP_Logger/AP_Logger.h>
56 : #include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
57 : #include <AP_Scheduler/PerfInfo.h> // loop perf monitoring
58 :
59 : #include <AP_Navigation/AP_Navigation.h>
60 : #include <AP_L1_Control/AP_L1_Control.h>
61 : #include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
62 :
63 : #include <AP_Vehicle/AP_Vehicle.h>
64 : #include <AP_TECS/AP_TECS.h>
65 : #include <AP_NavEKF2/AP_NavEKF2.h>
66 : #include <AP_NavEKF3/AP_NavEKF3.h>
67 : #include <AP_Mission/AP_Mission.h> // Mission command library
68 :
69 : #include <AP_Soaring/AP_Soaring.h>
70 : #include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
71 :
72 : #include <AP_Arming/AP_Arming.h>
73 : #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
74 : #include <AP_OSD/AP_OSD.h>
75 :
76 : #include <AP_Rally/AP_Rally.h>
77 :
78 : #include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
79 : #include <AP_Parachute/AP_Parachute.h>
80 : #include <AP_ADSB/AP_ADSB.h>
81 : #include <AP_Avoidance/AP_Avoidance_config.h> // "ADSB" avoidance library
82 : #include <AP_ICEngine/AP_ICEngine.h>
83 : #include <AP_Landing/AP_Landing.h>
84 : #include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
85 : #include <AP_Follow/AP_Follow.h>
86 : #include <AP_ExternalControl/AP_ExternalControl_config.h>
87 : #if AP_EXTERNAL_CONTROL_ENABLED
88 : #include "AP_ExternalControl_Plane.h"
89 : #endif
90 :
91 : #include <AC_PrecLand/AC_PrecLand_config.h>
92 : #if AC_PRECLAND_ENABLED
93 : # include <AC_PrecLand/AC_PrecLand.h>
94 : #endif
95 :
96 : #include "GCS_MAVLink_Plane.h"
97 : #include "GCS_Plane.h"
98 : #include "quadplane.h"
99 : #include <AP_Tuning/AP_Tuning_config.h>
100 : #if AP_TUNING_ENABLED
101 : #include "tuning.h"
102 : #endif
103 :
104 : // Configuration
105 : #include "config.h"
106 :
107 : #if AP_ADVANCEDFAILSAFE_ENABLED
108 : #include "afs_plane.h"
109 : #endif
110 :
111 : // Local modules
112 : #include "defines.h"
113 : #include "mode.h"
114 :
115 : #if AP_SCRIPTING_ENABLED
116 : #include <AP_Scripting/AP_Scripting.h>
117 : #endif
118 :
119 : #include "RC_Channel_Plane.h" // RC Channel Library
120 : #include "Parameters.h"
121 : #if AP_ADSB_AVOIDANCE_ENABLED
122 : #include "avoidance_adsb.h"
123 : #endif // AP_ADSB_AVOIDANCE_ENABLED
124 : #include "AP_Arming_Plane.h"
125 : #include "pullup.h"
126 : #include "systemid.h"
127 :
128 : /*
129 : main APM:Plane class
130 : */
131 : class Plane : public AP_Vehicle {
132 : public:
133 : friend class GCS_MAVLINK_Plane;
134 : friend class Parameters;
135 : friend class ParametersG2;
136 : friend class AP_Arming_Plane;
137 : friend class QuadPlane;
138 : friend class QAutoTune;
139 : friend class AP_Tuning_Plane;
140 : friend class AP_AdvancedFailsafe_Plane;
141 : friend class AP_Avoidance_Plane;
142 : friend class GCS_Plane;
143 : friend class RC_Channel_Plane;
144 : friend class RC_Channels_Plane;
145 : friend class Tailsitter;
146 : friend class Tiltrotor;
147 : friend class SLT_Transition;
148 : friend class Tailsitter_Transition;
149 : friend class VTOL_Assist;
150 :
151 : friend class Mode;
152 : friend class ModeCircle;
153 : friend class ModeStabilize;
154 : friend class ModeTraining;
155 : friend class ModeAcro;
156 : friend class ModeFBWA;
157 : friend class ModeFBWB;
158 : friend class ModeCruise;
159 : friend class ModeAutoTune;
160 : friend class ModeAuto;
161 : friend class ModeRTL;
162 : friend class ModeLoiter;
163 : friend class ModeAvoidADSB;
164 : friend class ModeGuided;
165 : friend class ModeInitializing;
166 : friend class ModeManual;
167 : friend class ModeQStabilize;
168 : friend class ModeQHover;
169 : friend class ModeQLoiter;
170 : friend class ModeQLand;
171 : friend class ModeQRTL;
172 : friend class ModeQAcro;
173 : friend class ModeQAutotune;
174 : friend class ModeTakeoff;
175 : friend class ModeThermal;
176 : friend class ModeLoiterAltQLand;
177 : #if MODE_AUTOLAND_ENABLED
178 : friend class ModeAutoLand;
179 : #endif
180 : #if AP_EXTERNAL_CONTROL_ENABLED
181 : friend class AP_ExternalControl_Plane;
182 : #endif
183 : #if AP_PLANE_GLIDER_PULLUP_ENABLED
184 : friend class GliderPullup;
185 : #endif
186 : #if AP_PLANE_SYSTEMID_ENABLED
187 : friend class AP_SystemID;
188 : #endif
189 :
190 : Plane(void);
191 :
192 : private:
193 :
194 : // key aircraft parameters passed to multiple libraries
195 : AP_FixedWing aparm;
196 :
197 : // Global parameters are all contained within the 'g' and 'g2' classes.
198 : Parameters g;
199 : ParametersG2 g2;
200 :
201 : // mapping between input channels
202 : RCMapper rcmap;
203 :
204 : // primary input channels
205 : RC_Channel *channel_roll;
206 : RC_Channel *channel_pitch;
207 : RC_Channel *channel_throttle;
208 : RC_Channel *channel_rudder;
209 : RC_Channel *channel_flap;
210 : RC_Channel *channel_airbrake;
211 :
212 : // scaled roll limit based on pitch
213 : int32_t roll_limit_cd;
214 : float pitch_limit_min;
215 :
216 : #if AP_RANGEFINDER_ENABLED
217 : AP_FixedWing::Rangefinder_State rangefinder_state;
218 :
219 : /*
220 : orientation of rangefinder to use for landing
221 : */
222 3840993 : Rotation rangefinder_orientation(void) const {
223 3840993 : return Rotation(g2.rangefinder_land_orient.get());
224 : }
225 :
226 : #endif
227 :
228 : #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
229 : struct {
230 : // allow for external height above ground estimate
231 : float hagl;
232 : uint32_t last_update_ms;
233 : uint32_t timeout_ms;
234 : } external_hagl;
235 : bool get_external_HAGL(float &height_agl);
236 : void handle_external_hagl(const mavlink_command_int_t &packet);
237 : #endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
238 :
239 : float get_landing_height(bool &using_rangefinder);
240 :
241 : AP_TECS TECS_controller{ahrs, aparm, landing, MASK_LOG_TECS};
242 : AP_L1_Control L1_controller{ahrs, &TECS_controller};
243 :
244 : // Attitude to servo controllers
245 : AP_RollController rollController{aparm};
246 : AP_PitchController pitchController{aparm};
247 : AP_YawController yawController{aparm};
248 : AP_SteerController steerController{};
249 :
250 : // Training mode
251 : bool training_manual_roll; // user has manual roll control
252 : bool training_manual_pitch; // user has manual pitch control
253 :
254 : // should throttle be pass-thru in guided?
255 : bool guided_throttle_passthru;
256 :
257 : // are we doing calibration? This is used to allow heartbeat to
258 : // external failsafe boards during baro and airspeed calibration
259 : bool in_calibration;
260 :
261 : // are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached
262 : bool long_failsafe_pending;
263 :
264 : // GCS selection
265 : GCS_Plane _gcs; // avoid using this; use gcs()
266 236185 : GCS_Plane &gcs() { return _gcs; }
267 :
268 : // selected navigation controller
269 : AP_Navigation *nav_controller = &L1_controller;
270 :
271 : // Camera
272 : #if AP_CAMERA_ENABLED
273 : AP_Camera camera{MASK_LOG_CAMERA};
274 : #endif
275 :
276 : #if AP_OPTICALFLOW_ENABLED
277 : // Optical flow sensor
278 : AP_OpticalFlow optflow;
279 : #endif
280 :
281 : #if HAL_RALLY_ENABLED
282 : // Rally Points
283 : AP_Rally rally;
284 : #endif
285 :
286 : #if AC_PRECLAND_ENABLED
287 : void precland_update(void);
288 : #endif
289 :
290 : // returns a Location for a rally point or home; if
291 : // HAL_RALLY_ENABLED is false, just home.
292 : Location calc_best_rally_or_home_location(const Location ¤t_loc, float rtl_home_alt_amsl_cm) const;
293 :
294 : #if OSD_ENABLED || OSD_PARAM_ENABLED
295 : AP_OSD osd;
296 : #endif
297 :
298 : ModeCircle mode_circle;
299 : ModeStabilize mode_stabilize;
300 : ModeTraining mode_training;
301 : ModeAcro mode_acro;
302 : ModeFBWA mode_fbwa;
303 : ModeFBWB mode_fbwb;
304 : ModeCruise mode_cruise;
305 : ModeAutoTune mode_autotune;
306 : ModeAuto mode_auto;
307 : ModeRTL mode_rtl;
308 : ModeLoiter mode_loiter;
309 : #if HAL_ADSB_ENABLED
310 : ModeAvoidADSB mode_avoidADSB;
311 : #endif
312 : ModeGuided mode_guided;
313 : ModeInitializing mode_initializing;
314 : ModeManual mode_manual;
315 : #if HAL_QUADPLANE_ENABLED
316 : ModeQStabilize mode_qstabilize;
317 : ModeQHover mode_qhover;
318 : ModeQLoiter mode_qloiter;
319 : ModeQLand mode_qland;
320 : ModeQRTL mode_qrtl;
321 : ModeQAcro mode_qacro;
322 : ModeLoiterAltQLand mode_loiter_qland;
323 : #if QAUTOTUNE_ENABLED
324 : ModeQAutotune mode_qautotune;
325 : #endif // QAUTOTUNE_ENABLED
326 : #endif // HAL_QUADPLANE_ENABLED
327 : ModeTakeoff mode_takeoff;
328 : #if MODE_AUTOLAND_ENABLED
329 : ModeAutoLand mode_autoland;
330 : #endif
331 : #if HAL_SOARING_ENABLED
332 : ModeThermal mode_thermal;
333 : #endif
334 :
335 : #if AP_QUICKTUNE_ENABLED
336 : AP_Quicktune quicktune;
337 : #endif
338 :
339 : // This is the state of the flight control system
340 : // There are multiple states defined such as MANUAL, FBW-A, AUTO
341 : Mode *control_mode = &mode_initializing;
342 : Mode *previous_mode = &mode_initializing;
343 :
344 : // time of last mode change
345 : uint32_t last_mode_change_ms;
346 :
347 : // This is used to enable the inverted flight feature
348 : bool inverted_flight;
349 :
350 : // last time we ran roll/pitch stabilization
351 : uint32_t last_stabilize_ms;
352 :
353 : // Failsafe
354 : struct {
355 : // Used to track if the value on channel 3 (throttle) has fallen below the failsafe threshold
356 : // RC receiver should be set up to output a low throttle value when signal is lost
357 : bool rc_failsafe;
358 :
359 : // true if an adsb related failsafe has occurred
360 : bool adsb;
361 :
362 : // saved flight mode
363 : enum Mode::Number saved_mode_number;
364 :
365 : // A tracking variable for type of failsafe active
366 : // Used for failsafe based on loss of RC signal or GCS signal
367 : int16_t state;
368 :
369 : // number of low throttle values
370 : uint8_t throttle_counter;
371 :
372 : uint32_t last_valid_rc_ms;
373 :
374 : //keeps track of the last valid rc as it relates to the AFS system
375 : //Does not count rc inputs as valid if the standard failsafe is on
376 : uint32_t AFS_last_valid_rc_ms;
377 : } failsafe;
378 :
379 : #if HAL_QUADPLANE_ENABLED
380 : // Landing
381 : class VTOLApproach {
382 : public:
383 : enum class Stage {
384 : RTL,
385 : LOITER_TO_ALT,
386 : ENSURE_RADIUS,
387 : WAIT_FOR_BREAKOUT,
388 : APPROACH_LINE,
389 : VTOL_LANDING,
390 : };
391 :
392 : Stage approach_stage;
393 : float approach_direction_deg;
394 : } vtol_approach_s;
395 : #endif
396 :
397 58267 : bool any_failsafe_triggered() {
398 58267 : return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb;
399 : }
400 :
401 : // A counter used to count down valid gps fixes to allow the gps estimate to settle
402 : // before recording our home position (and executing a ground start if we booted with an air start)
403 : uint8_t ground_start_count = 5;
404 :
405 : // true if we have a position estimate from AHRS
406 : bool have_position;
407 :
408 : // Airspeed
409 : // The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
410 : // Also used for flap deployment criteria. Centimeters per second.
411 : int32_t target_airspeed_cm;
412 : int32_t new_airspeed_cm = -1; //temp variable for AUTO and GUIDED mode speed changes
413 :
414 : // The difference between current and desired airspeed. Used in the pitch controller. Meters per second.
415 : float airspeed_error;
416 :
417 : // An amount that the airspeed should be increased in auto modes based on the user positioning the
418 : // throttle stick in the top half of the range. Centimeters per second.
419 : int16_t airspeed_nudge_cm;
420 :
421 : // Similar to airspeed_nudge, but used when no airspeed sensor.
422 : // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
423 : int16_t throttle_nudge;
424 :
425 : // Ground speed
426 : // The amount current ground speed is below min ground speed. Centimeters per second
427 : int32_t groundspeed_undershoot;
428 : bool groundspeed_undershoot_is_valid;
429 : float last_groundspeed_undershoot_offset;
430 :
431 : // speed scaler for control surfaces, updated at 10Hz
432 : float surface_speed_scaler = 1.0;
433 :
434 : // Battery Sensors
435 : AP_BattMonitor battery{MASK_LOG_CURRENT,
436 : FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t),
437 : _failsafe_priorities};
438 :
439 : struct {
440 : uint32_t last_tkoff_arm_time;
441 : uint32_t last_check_ms;
442 : uint32_t rudder_takeoff_warn_ms;
443 : uint32_t last_report_ms;
444 : bool launchTimerStarted;
445 : uint8_t accel_event_counter;
446 : uint32_t accel_event_ms;
447 : uint32_t start_time_ms;
448 : bool waiting_for_rudder_neutral;
449 : float throttle_lim_max;
450 : float throttle_lim_min;
451 : uint32_t throttle_max_timer_ms;
452 : uint32_t level_off_start_time_ms;
453 : // Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
454 : #if MODE_AUTOLAND_ENABLED
455 : struct {
456 : float heading; // deg
457 : bool initialized;
458 : } initial_direction;
459 : #endif
460 : } takeoff_state;
461 :
462 : // ground steering controller state
463 : struct {
464 : // Direction held during phases of takeoff and landing centidegrees
465 : // A value of -1 indicates the course has not been set/is not in use
466 : // this is a 0..36000 value, or -1 for disabled
467 : int32_t hold_course_cd = -1;
468 :
469 : // locked_course and locked_course_cd are used in stabilize mode
470 : // when ground steering is active, and for steering in auto-takeoff
471 : bool locked_course;
472 : float locked_course_err;
473 : uint32_t last_steer_ms;
474 : } steer_state;
475 :
476 : // flight mode specific
477 : struct {
478 : // Altitude threshold to complete a takeoff command in autonomous
479 : // modes. Centimeters above home
480 : int32_t takeoff_altitude_rel_cm;
481 :
482 : // Begin leveling out the enforced takeoff pitch angle min at this height to reduce/eliminate overshoot
483 : int32_t height_below_takeoff_to_level_off_cm;
484 :
485 : // the highest airspeed we have reached since entering AUTO. Used
486 : // to control ground takeoff
487 : float highest_airspeed;
488 :
489 : // turn angle for next leg of mission
490 : float next_turn_angle {90};
491 :
492 : // filtered sink rate for landing
493 : float sink_rate;
494 :
495 : // distance to next waypoint
496 : float wp_distance;
497 :
498 : // proportion to next waypoint
499 : float wp_proportion;
500 :
501 : // last time is_flying() returned true in milliseconds
502 : uint32_t last_flying_ms;
503 :
504 : // time stamp of when we start flying while in auto mode in milliseconds
505 : uint32_t started_flying_in_auto_ms;
506 :
507 : // barometric altitude at start of takeoff
508 : float baro_takeoff_alt;
509 :
510 : // initial pitch. Used to detect if nose is rising in a tail dragger
511 : int16_t initial_pitch_cd;
512 :
513 : // Minimum pitch to hold during takeoff command execution. Hundredths of a degree
514 : int16_t takeoff_pitch_cd;
515 :
516 : // Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process.
517 : bool takeoff_complete;
518 :
519 : // are we headed to the land approach waypoint? Works for any nav type
520 : bool wp_is_land_approach;
521 :
522 : // should we fly inverted?
523 : bool inverted_flight;
524 :
525 : // should we enable cross-tracking for the next waypoint?
526 : bool next_wp_crosstrack;
527 :
528 : // should we use cross-tracking for this waypoint?
529 : bool crosstrack;
530 :
531 : // in FBWA taildragger takeoff mode
532 : bool fbwa_tdrag_takeoff_mode;
533 :
534 : // have we checked for an auto-land?
535 : bool checked_for_autoland;
536 :
537 : // Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
538 : // are we in idle mode? used for balloon launch to stop servo
539 : // movement until altitude is reached
540 : bool idle_mode;
541 :
542 : // are we in VTOL mode in AUTO?
543 : bool vtol_mode;
544 :
545 : // are we doing loiter mode as a VTOL?
546 : bool vtol_loiter;
547 :
548 : // how much correction have we added for terrain data
549 : float terrain_correction;
550 :
551 : // last home altitude for detecting changes
552 : int32_t last_home_alt_cm;
553 :
554 : // have we finished the takeoff ratation (when it applies)?
555 : bool rotation_complete;
556 : } auto_state;
557 :
558 : #if AP_SCRIPTING_ENABLED
559 : // support for scripting nav commands, with verify
560 : struct {
561 : bool enabled;
562 : uint16_t id;
563 : float roll_rate_dps;
564 : float pitch_rate_dps;
565 : float yaw_rate_dps;
566 : float throttle_pct;
567 : uint32_t start_ms;
568 : uint32_t current_ms;
569 : float rudder_offset_pct;
570 : bool run_yaw_rate_controller;
571 : } nav_scripting;
572 : #endif
573 :
574 : struct GuidedState {
575 : // roll pitch yaw commanded from external controller in centidegrees
576 : Vector3l forced_rpy_cd;
577 : // last time we heard from the external controller
578 : Vector3l last_forced_rpy_ms;
579 :
580 : // throttle commanded from external controller in percent
581 : float forced_throttle;
582 : uint32_t last_forced_throttle_ms;
583 :
584 : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
585 : // airspeed adjustments
586 : float target_airspeed_cm = -1; // don't default to zero here, as zero is a valid speed.
587 : float target_airspeed_accel;
588 : uint32_t target_airspeed_time_ms;
589 :
590 : // altitude adjustments
591 : Location target_location;
592 : // target_location altitude is uses to hold some flag values:
593 : bool target_location_alt_is_minus_one() const;
594 :
595 : float target_alt_rate;
596 : uint32_t target_alt_time_ms = 0;
597 : uint8_t target_mav_frame = -1;
598 :
599 : // heading track
600 : float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians
601 : float target_heading_accel_limit;
602 : uint32_t target_heading_time_ms;
603 : guided_heading_type_t target_heading_type;
604 : bool target_heading_limit;
605 : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
606 : } guided_state;
607 :
608 : #if AP_LANDINGGEAR_ENABLED
609 : // landing gear state
610 : struct {
611 : AP_FixedWing::FlightStage last_flight_stage;
612 : } gear;
613 : #endif
614 :
615 : struct {
616 : // on hard landings, only check once after directly a landing so you
617 : // don't trigger a crash when picking up the aircraft
618 : bool checkedHardLanding;
619 :
620 : // crash detection. True when we are crashed
621 : bool is_crashed;
622 :
623 : // impact detection flag. Expires after a few seconds via impact_timer_ms
624 : bool impact_detected;
625 :
626 : // debounce timer
627 : uint32_t debounce_timer_ms;
628 :
629 : // delay time for debounce to count to
630 : uint32_t debounce_time_total_ms;
631 :
632 : // length of time impact_detected has been true. Times out after a few seconds. Used to clip isFlyingProbability
633 : uint32_t impact_timer_ms;
634 : } crash_state;
635 :
636 : // this controls throttle suppression in auto modes
637 : bool throttle_suppressed;
638 :
639 : #if AP_BATTERY_WATT_MAX_ENABLED
640 : // reduce throttle to eliminate battery over-current
641 : int8_t throttle_watt_limit_max;
642 : int8_t throttle_watt_limit_min; // for reverse thrust
643 : uint32_t throttle_watt_limit_timer_ms;
644 : #endif
645 :
646 : AP_FixedWing::FlightStage flight_stage = AP_FixedWing::FlightStage::NORMAL;
647 :
648 : // probability of aircraft is currently in flight. range from 0 to
649 : // 1 where 1 is 100% sure we're in flight
650 : float isFlyingProbability;
651 :
652 : // previous value of is_flying()
653 : bool previous_is_flying;
654 :
655 : // time since started flying in any mode in milliseconds
656 : uint32_t started_flying_ms;
657 :
658 : // ground mode is true when disarmed and not flying
659 : bool ground_mode;
660 :
661 : // Navigation control variables
662 : // The instantaneous desired bank angle. Hundredths of a degree
663 : int32_t nav_roll_cd;
664 :
665 : // The instantaneous desired pitch angle. Hundredths of a degree
666 : int32_t nav_pitch_cd;
667 :
668 : // the aerodynamic load factor. This is calculated from the demanded
669 : // roll before the roll is clipped, using 1/cos(nav_roll)
670 : float aerodynamic_load_factor = 1.0f;
671 :
672 : // a smoothed airspeed estimate, used for limiting roll angle
673 : float smoothed_airspeed;
674 :
675 : // Mission library
676 : AP_Mission mission{
677 : FUNCTOR_BIND_MEMBER(&Plane::start_command_callback, bool, const AP_Mission::Mission_Command &),
678 : FUNCTOR_BIND_MEMBER(&Plane::verify_command_callback, bool, const AP_Mission::Mission_Command &),
679 : FUNCTOR_BIND_MEMBER(&Plane::exit_mission_callback, void)};
680 :
681 :
682 : #if HAL_PARACHUTE_ENABLED
683 : AP_Parachute parachute;
684 : #endif
685 :
686 : // terrain handling
687 : #if AP_TERRAIN_AVAILABLE
688 : AP_Terrain terrain;
689 : #endif
690 :
691 : AP_Landing landing{mission,ahrs,&TECS_controller,nav_controller,aparm,
692 : FUNCTOR_BIND_MEMBER(&Plane::set_target_altitude_proportion, void, const Location&, float),
693 : FUNCTOR_BIND_MEMBER(&Plane::constrain_target_altitude_location, void, const Location&, const Location&),
694 : FUNCTOR_BIND_MEMBER(&Plane::adjusted_altitude_cm, int32_t),
695 : FUNCTOR_BIND_MEMBER(&Plane::adjusted_relative_altitude_cm, int32_t),
696 : FUNCTOR_BIND_MEMBER(&Plane::disarm_if_autoland_complete, void),
697 : FUNCTOR_BIND_MEMBER(&Plane::update_flight_stage, void)};
698 : #if HAL_ADSB_ENABLED
699 : AP_ADSB adsb;
700 : #endif // HAL_ADSB_ENABLED
701 :
702 : #if AP_ADSB_AVOIDANCE_ENABLED
703 : // avoidance of adsb enabled vehicles (normally manned vehicles)
704 : AP_Avoidance_Plane avoidance_adsb{adsb};
705 : #endif // AP_ADSB_AVOIDANCE_ENABLED
706 :
707 : // Outback Challenge Failsafe Support
708 : #if AP_ADVANCEDFAILSAFE_ENABLED
709 : AP_AdvancedFailsafe_Plane afs;
710 : #endif
711 :
712 : /*
713 : meta data to support counting the number of circles in a loiter
714 : */
715 : struct {
716 : // previous target bearing, used to update sum_cd
717 : int32_t old_target_bearing_cd;
718 :
719 : // Total desired rotation in a loiter. Used for Loiter Turns commands.
720 : int32_t total_cd;
721 :
722 : // total angle completed in the loiter so far
723 : int32_t sum_cd;
724 :
725 : // Direction for loiter. 1 for clockwise, -1 for counter-clockwise
726 : int8_t direction;
727 :
728 : // when loitering and an altitude is involved, this flag is true when it has been reached at least once
729 : bool reached_target_alt;
730 :
731 : // check for scenarios where updrafts can keep you from loitering down indefinitely.
732 : bool unable_to_achieve_target_alt;
733 :
734 : // start time of the loiter. Milliseconds.
735 : uint32_t start_time_ms;
736 :
737 : // altitude at start of loiter loop lap. Used to detect delta alt of each lap.
738 : // only valid when sum_cd > 36000
739 : int32_t start_lap_alt_cm;
740 : int32_t next_sum_lap_cd;
741 :
742 : // The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
743 : uint32_t time_max_ms;
744 :
745 : // current value of loiter radius in metres used by the controller
746 : float radius;
747 : } loiter;
748 :
749 : // Conditional command
750 : // A value used in condition commands (eg delay, change alt, etc.)
751 : // For example in a change altitude command, it is the altitude to change to.
752 : int32_t condition_value;
753 :
754 : // A starting value used to check the status of a conditional command.
755 : // For example in a delay command the condition_start records that start time for the delay
756 : uint32_t condition_start;
757 :
758 : // 3D Location vectors
759 : // Location structure defined in AP_Common
760 : const Location &home = ahrs.get_home();
761 :
762 : // The location of the previous waypoint. Used for track following and altitude ramp calculations
763 : Location prev_WP_loc {};
764 :
765 : // The plane's current location
766 : Location current_loc {};
767 :
768 : // The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
769 : Location next_WP_loc {};
770 :
771 : // Altitude control
772 : struct {
773 : // target altitude above sea level in cm. Used for barometric
774 : // altitude navigation
775 : int32_t amsl_cm;
776 :
777 : // Altitude difference between previous and current waypoint in
778 : // centimeters. Used for altitude slope handling
779 : int32_t offset_cm;
780 :
781 : #if AP_TERRAIN_AVAILABLE
782 : // are we trying to follow terrain?
783 : bool terrain_following;
784 :
785 : // are we waiting to load terrain data to init terrain following
786 : bool terrain_following_pending;
787 :
788 : // target altitude above terrain in cm, valid if terrain_following
789 : // is set
790 : int32_t terrain_alt_cm;
791 :
792 : // lookahead value for height error reporting
793 : float lookahead;
794 : #endif
795 :
796 : // last input for FBWB/CRUISE height control
797 : float last_elevator_input;
798 :
799 : // last time we checked for pilot control of height
800 : uint32_t last_elev_check_us;
801 : } target_altitude {};
802 :
803 : float relative_altitude;
804 :
805 : struct {
806 : uint32_t last_trim_check;
807 : uint32_t last_trim_save;
808 : } auto_trim;
809 :
810 : struct {
811 : bool done_climb;
812 : } rtl;
813 :
814 : // last time home was updated while disarmed
815 : uint32_t last_home_update_ms;
816 :
817 : // Camera/Antenna mount tracking and stabilisation stuff
818 : #if HAL_MOUNT_ENABLED
819 : AP_Mount camera_mount;
820 : #endif
821 :
822 : // Arming/Disarming management class
823 : AP_Arming_Plane arming;
824 :
825 : AP_Param param_loader {var_info};
826 :
827 : // external control library
828 : #if AP_EXTERNAL_CONTROL_ENABLED
829 : AP_ExternalControl_Plane external_control;
830 : #endif
831 :
832 : static const AP_Scheduler::Task scheduler_tasks[];
833 : static const AP_Param::Info var_info[];
834 :
835 : #if HAL_QUADPLANE_ENABLED
836 : // support for quadcopter-plane
837 : QuadPlane quadplane{ahrs};
838 : #endif
839 :
840 : #if AP_TUNING_ENABLED
841 : // support for transmitter tuning
842 : AP_Tuning_Plane tuning;
843 : #endif
844 :
845 : static const struct LogStructure log_structure[];
846 :
847 : // rudder mixing gain for differential thrust (0 - 1)
848 : float rudder_dt;
849 :
850 : // soaring mode-change timer
851 : uint32_t soaring_mode_timer_ms;
852 :
853 : // terrain disable for non AUTO modes, set with an RC Option switch
854 : bool non_auto_terrain_disable;
855 : bool terrain_disabled();
856 : #if AP_TERRAIN_AVAILABLE
857 : bool terrain_enabled_in_current_mode() const;
858 : bool terrain_enabled_in_mode(Mode::Number num) const;
859 : enum class terrain_bitmask {
860 : ALL = 1U << 0,
861 : FLY_BY_WIRE_B = 1U << 1,
862 : CRUISE = 1U << 2,
863 : AUTO = 1U << 3,
864 : RTL = 1U << 4,
865 : AVOID_ADSB = 1U << 5,
866 : GUIDED = 1U << 6,
867 : LOITER = 1U << 7,
868 : CIRCLE = 1U << 8,
869 : QRTL = 1U << 9,
870 : QLAND = 1U << 10,
871 : QLOITER = 1U << 11,
872 : AUTOLAND = 1U << 12,
873 : };
874 : struct TerrainLookupTable{
875 : Mode::Number mode_num;
876 : terrain_bitmask bitmask;
877 : };
878 : static const TerrainLookupTable Terrain_lookup[];
879 : #endif
880 :
881 : #if AP_QUICKTUNE_ENABLED
882 : void update_quicktune(void);
883 : #endif
884 :
885 : // Attitude.cpp
886 : void adjust_nav_pitch_throttle(void);
887 : void update_load_factor(void);
888 : void apply_load_factor_roll_limits(void);
889 : void adjust_altitude_target();
890 : void setup_alt_slope(void);
891 : int32_t get_RTL_altitude_cm() const;
892 : bool rangefinder_use(enum RangeFinderUse rangefinder_use) const;
893 : float relative_ground_altitude(enum RangeFinderUse rangefinder_use);
894 : float relative_ground_altitude(enum RangeFinderUse rangefinder_use, bool use_terrain_if_available);
895 : void set_target_altitude_current(void);
896 : void set_target_altitude_location(const Location &loc);
897 : int32_t relative_target_altitude_cm(void);
898 : void change_target_altitude(int32_t change_cm);
899 : void set_target_altitude_proportion(const Location &loc, float proportion);
900 : #if AP_TERRAIN_AVAILABLE
901 : bool set_target_altitude_proportion_terrain(void);
902 : #endif
903 : void constrain_target_altitude_location(const Location &loc1, const Location &loc2);
904 : int32_t calc_altitude_error_cm(void);
905 : void check_fbwb_altitude(void);
906 : void reset_offset_altitude(void);
907 : void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
908 : bool above_location_current(const Location &loc);
909 : void setup_terrain_target_alt(Location &loc);
910 : int32_t adjusted_altitude_cm(void);
911 : int32_t adjusted_relative_altitude_cm(void);
912 : float mission_alt_offset(void);
913 : void reset_alt_offset(bool force = false);
914 : float height_above_target(void);
915 : float lookahead_adjustment(void);
916 : void fix_terrain_WP(Location &loc, uint32_t linenum);
917 : #if AP_RANGEFINDER_ENABLED
918 : float rangefinder_correction(void);
919 : void rangefinder_height_update(void);
920 : void rangefinder_terrain_correction(float &height);
921 : #endif
922 : void stabilize();
923 : void calc_throttle();
924 : void calc_nav_roll();
925 : void calc_nav_pitch();
926 : float calc_speed_scaler(void);
927 22835409 : float get_speed_scaler(void) const { return surface_speed_scaler; }
928 : bool stick_mixing_enabled(void);
929 : void stabilize_roll();
930 : float stabilize_roll_get_roll_out();
931 : void stabilize_pitch();
932 : float stabilize_pitch_get_pitch_out();
933 : void stabilize_stick_mixing_fbw();
934 : void stabilize_yaw();
935 : int16_t calc_nav_yaw_coordinated();
936 : int16_t calc_nav_yaw_course(void);
937 : int16_t calc_nav_yaw_ground(void);
938 :
939 : // Check if there has been a change in attitude estimate which the attitude controllers should be told about
940 : void check_ahrs_reset();
941 : struct {
942 : uint16_t ahrs_yaw_reset_count;
943 : uint16_t attitude_reset_count;
944 : } ahrs_check;
945 :
946 : #if HAL_LOGGING_ENABLED
947 :
948 : // methods for AP_Vehicle:
949 692 : const AP_Int32 &get_log_bitmask() override { return g.log_bitmask; }
950 692 : const struct LogStructure *get_log_structures() const override {
951 692 : return log_structure;
952 : }
953 : uint8_t get_num_log_structures() const override;
954 :
955 : // Log.cpp
956 : void Log_Write_FullRate(void);
957 : void Log_Write_Attitude(void);
958 : void Log_Write_Control_Tuning();
959 : void Log_Write_OFG_Guided();
960 : void Log_Write_Guided(void);
961 : void Log_Write_Nav_Tuning();
962 : #if AP_RANGEFINDER_ENABLED
963 : void Log_Write_RFNS();
964 : #endif
965 : void Log_Write_Status();
966 : void Log_Write_RC(void);
967 : void Log_Write_Vehicle_Startup_Messages();
968 : void Log_Write_AETR();
969 :
970 : #if AP_PLANE_BLACKBOX_LOGGING
971 : void Log_Write_Blackbox(void);
972 : #endif
973 : #endif
974 :
975 : // Parameters.cpp
976 : void load_parameters(void) override;
977 :
978 : // commands_logic.cpp
979 : void set_next_WP(const Location &loc);
980 : void do_RTL(int32_t alt);
981 : bool verify_takeoff();
982 : bool verify_loiter_unlim(const AP_Mission::Mission_Command &cmd);
983 : bool verify_loiter_time();
984 : bool verify_loiter_turns(const AP_Mission::Mission_Command &cmd);
985 : bool verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd);
986 : bool verify_continue_and_change_alt();
987 : bool verify_wait_delay();
988 : bool verify_within_distance();
989 : bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd);
990 : void do_loiter_at_location();
991 : bool verify_loiter_heading(bool init);
992 : void exit_mission_callback();
993 : bool start_command(const AP_Mission::Mission_Command& cmd);
994 : bool verify_command(const AP_Mission::Mission_Command& cmd);
995 : void do_takeoff(const AP_Mission::Mission_Command& cmd);
996 : void do_nav_wp(const AP_Mission::Mission_Command& cmd);
997 : void do_land(const AP_Mission::Mission_Command& cmd);
998 : #if HAL_QUADPLANE_ENABLED
999 : void do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
1000 : #endif
1001 : void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd);
1002 : void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
1003 : void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
1004 : void do_loiter_time(const AP_Mission::Mission_Command& cmd);
1005 : void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
1006 : void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
1007 : void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
1008 : void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
1009 : void do_vtol_land(const AP_Mission::Mission_Command& cmd);
1010 : bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
1011 : #if HAL_QUADPLANE_ENABLED
1012 : // vtol takeoff from AP_Vehicle for quadplane.
1013 : bool start_takeoff(const float alt_m) override;
1014 : bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
1015 : #endif
1016 : void do_wait_delay(const AP_Mission::Mission_Command& cmd);
1017 : void do_within_distance(const AP_Mission::Mission_Command& cmd);
1018 : bool do_change_speed(const AP_Mission::Mission_Command& cmd);
1019 : void do_set_home(const AP_Mission::Mission_Command& cmd);
1020 : bool start_command_callback(const AP_Mission::Mission_Command &cmd);
1021 : bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
1022 : float get_wp_radius() const;
1023 :
1024 : bool is_land_command(uint16_t cmd) const;
1025 :
1026 : bool do_change_speed(SPEED_TYPE speedtype, float speed_target_ms, float rhtottle_pct);
1027 : /*
1028 : return true if in a specific AUTO mission command
1029 : */
1030 : bool in_auto_mission_id(uint16_t command) const;
1031 :
1032 : #if AP_SCRIPTING_ENABLED
1033 : // nav scripting support
1034 : void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
1035 : bool verify_nav_script_time(const AP_Mission::Mission_Command& cmd);
1036 : #endif
1037 :
1038 : // commands.cpp
1039 : void set_guided_WP(const Location &loc);
1040 :
1041 : // update home position. Return true if update done
1042 : bool update_home();
1043 :
1044 : // update current_loc
1045 : void update_current_loc(void);
1046 :
1047 : // set home location and store it persistently:
1048 : bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
1049 : bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
1050 : bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
1051 :
1052 : // control_modes.cpp
1053 : void autotune_start(void);
1054 : void autotune_restore(void);
1055 : void autotune_enable(bool enable);
1056 : bool fly_inverted(void);
1057 30396 : uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); }
1058 : Mode *mode_from_mode_num(const enum Mode::Number num);
1059 578864 : bool current_mode_requires_mission() const override {
1060 578864 : return control_mode == &mode_auto;
1061 : }
1062 :
1063 : bool autotuning;
1064 :
1065 : // events.cpp
1066 : void rc_failsafe_short_on_event();
1067 : void failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason);
1068 : void rc_failsafe_short_off_event();
1069 : void failsafe_long_off_event(ModeReason reason);
1070 : void handle_battery_failsafe(const char* type_str, const int8_t action);
1071 : bool failsafe_in_landing_sequence() const; // returns true if the vehicle is in landing sequence. Intended only for use in failsafe code.
1072 :
1073 : #if AP_FENCE_ENABLED
1074 : // fence.cpp
1075 : void fence_check();
1076 : void fence_checks_async() override;
1077 : bool fence_stickmixing() const;
1078 : bool in_fence_recovery() const;
1079 : uint8_t orig_breaches;
1080 : #endif
1081 :
1082 : // Plane.cpp
1083 : void disarm_if_autoland_complete();
1084 : bool trigger_land_abort(const float climb_to_alt_m);
1085 : void get_osd_attitude_rad(float &roll, float &pitch, float &yaw) override;
1086 : float tecs_hgt_afe(void);
1087 : void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
1088 : uint8_t &task_count,
1089 : uint32_t &log_bit) override;
1090 : void ahrs_update();
1091 : void update_speed_height(void);
1092 : void update_GPS_50Hz(void);
1093 : void update_GPS_10Hz(void);
1094 : void update_compass(void);
1095 : void update_alt(void);
1096 : #if AP_ADVANCEDFAILSAFE_ENABLED
1097 : void afs_fs_check(void);
1098 : #endif
1099 : void one_second_loop(void);
1100 : void three_hz_loop(void);
1101 : #if AP_AIRSPEED_AUTOCAL_ENABLE
1102 : void airspeed_ratio_update(void);
1103 : #endif
1104 : void update_logging10(void);
1105 : void update_logging25(void);
1106 : void update_control_mode(void);
1107 : void update_fly_forward(void);
1108 : void update_flight_stage();
1109 : void set_flight_stage(AP_FixedWing::FlightStage fs);
1110 : bool flight_option_enabled(FlightOptions flight_option) const;
1111 :
1112 : // navigation.cpp
1113 : void loiter_angle_reset(void);
1114 : void loiter_angle_update(void);
1115 : void navigate();
1116 : void check_home_alt_change(void);
1117 : void calc_airspeed_errors();
1118 : float mode_auto_target_airspeed_cm();
1119 : void calc_gndspeed_undershoot();
1120 : void update_loiter(uint16_t radius);
1121 : void update_loiter_update_nav(uint16_t radius);
1122 : void update_fbwb_speed_height(void);
1123 : void setup_turn_angle(void);
1124 : bool reached_loiter_target(void);
1125 :
1126 : // radio.cpp
1127 : void set_control_channels(void) override;
1128 : void init_rc_in();
1129 : void init_rc_out_main();
1130 : void init_rc_out_aux();
1131 : void read_radio();
1132 : int16_t rudder_input(void);
1133 : void control_failsafe();
1134 : void trim_radio();
1135 : bool rc_throttle_value_ok(void) const;
1136 : bool rc_failsafe_active(void) const;
1137 :
1138 : #if AP_RANGEFINDER_ENABLED
1139 : // sensors.cpp
1140 : void read_rangefinder(void);
1141 : #endif
1142 :
1143 : // system.cpp
1144 : __INITFUNC__ void init_ardupilot() override;
1145 : bool set_mode(Mode& new_mode, const ModeReason reason);
1146 : bool set_mode(const uint8_t mode, const ModeReason reason) override;
1147 : bool set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason);
1148 : void check_long_failsafe();
1149 : void check_short_rc_failsafe();
1150 : void startup_INS(void);
1151 : bool should_log(uint32_t mask);
1152 : int8_t throttle_percentage(void);
1153 : void notify_mode(const Mode& mode);
1154 : bool gcs_mode_enabled(const Mode::Number mode_num) const;
1155 :
1156 : // Return mask of enabled modes, order does not matter, its just for tracking changes
1157 : uint32_t get_available_mode_enabled_mask() const override;
1158 :
1159 : // takeoff.cpp
1160 : bool auto_takeoff_check(void);
1161 : void takeoff_calc_roll(void);
1162 : void takeoff_calc_pitch(void);
1163 : void takeoff_calc_throttle();
1164 : int8_t takeoff_tail_hold(void);
1165 : int16_t get_takeoff_pitch_min_cd(void);
1166 : void landing_gear_update(void);
1167 : bool check_takeoff_timeout(void);
1168 : bool check_takeoff_timeout_level_off(void);
1169 :
1170 : // avoidance_adsb.cpp
1171 : void avoidance_adsb_update(void);
1172 :
1173 : // servos.cpp
1174 : void set_servos();
1175 : float apply_throttle_limits(float throttle_in);
1176 : void set_throttle(void);
1177 : void set_takeoff_expected(void);
1178 : float get_auto_flap_speed() const;
1179 : void set_servos_flaps(void);
1180 : void dspoiler_update(void);
1181 : void airbrake_update(void);
1182 : void landing_neutral_control_surface_servos(void);
1183 : void servos_output(void);
1184 : void servos_auto_trim(void);
1185 : void servos_twin_engine_mix();
1186 : void force_flare();
1187 : void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
1188 : void throttle_slew_limit();
1189 : bool suppress_throttle(void);
1190 : void update_throttle_hover();
1191 : void channel_function_mixer(SRV_Channel::Function func1_in, SRV_Channel::Function func2_in,
1192 : SRV_Channel::Function func1_out, SRV_Channel::Function func2_out) const;
1193 : void flaperon_update();
1194 : void indicate_waiting_for_rud_neutral_to_takeoff(void);
1195 :
1196 : // is_flying.cpp
1197 : void update_is_flying_5Hz(void);
1198 : void crash_detection_update(void);
1199 : bool in_preLaunch_flight_stage(void);
1200 : bool is_flying(void);
1201 :
1202 : // parachute.cpp
1203 : void parachute_check();
1204 : #if HAL_PARACHUTE_ENABLED
1205 : void parachute_release();
1206 : bool parachute_manual_release();
1207 : #endif
1208 :
1209 : // soaring.cpp
1210 : #if HAL_SOARING_ENABLED
1211 : void update_soaring();
1212 : #endif
1213 :
1214 : // RC_Channel.cpp
1215 : bool emergency_landing;
1216 :
1217 : // vehicle specific waypoint info helpers
1218 : bool get_wp_distance_m(float &distance) const override;
1219 : bool get_wp_bearing_deg(float &bearing) const override;
1220 : bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
1221 :
1222 : // reverse_thrust.cpp
1223 : bool reversed_throttle;
1224 : bool have_reverse_throttle_rc_option;
1225 : bool allow_reverse_thrust(void) const;
1226 : bool have_reverse_thrust(void) const;
1227 : float get_throttle_input(bool no_deadzone=false) const;
1228 : float get_adjusted_throttle_input(bool no_deadzone=false) const;
1229 : bool reverse_thrust_enabled(UseReverseThrust use_reverse_thrust_option) const;
1230 :
1231 : #if AP_SCRIPTING_ENABLED
1232 : // support for NAV_SCRIPT_TIME mission command
1233 : bool nav_scripting_active(void);
1234 : bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override;
1235 : void nav_script_time_done(uint16_t id) override;
1236 :
1237 : // command throttle percentage and roll, pitch, yaw target
1238 : // rates. For use with scripting controllers
1239 : void set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override;
1240 : void set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller) override;
1241 : bool nav_scripting_enable(uint8_t mode) override;
1242 : #endif
1243 :
1244 : enum Failsafe_Action {
1245 : Failsafe_Action_None = 0,
1246 : Failsafe_Action_RTL = 1,
1247 : Failsafe_Action_Land = 2,
1248 : Failsafe_Action_Terminate = 3,
1249 : #if HAL_QUADPLANE_ENABLED
1250 : Failsafe_Action_QLand = 4,
1251 : #endif
1252 : Failsafe_Action_Parachute = 5,
1253 : #if HAL_QUADPLANE_ENABLED
1254 : Failsafe_Action_Loiter_alt_QLand = 6,
1255 : #endif
1256 : Failsafe_Action_AUTOLAND_OR_RTL = 7,
1257 : };
1258 :
1259 : // list of priorities, highest priority first
1260 : static constexpr int8_t _failsafe_priorities[] = {
1261 : Failsafe_Action_Terminate,
1262 : Failsafe_Action_Parachute,
1263 : #if HAL_QUADPLANE_ENABLED
1264 : Failsafe_Action_QLand,
1265 : #endif
1266 : Failsafe_Action_Land,
1267 : Failsafe_Action_RTL,
1268 : Failsafe_Action_None,
1269 : -1 // the priority list must end with a sentinel of -1
1270 : };
1271 : static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
1272 : "_failsafe_priorities is missing the sentinel");
1273 :
1274 : // EKF checks for loss of navigation performed in ekf_check.cpp
1275 : // These are specific to VTOL operation
1276 : void ekf_check();
1277 : bool ekf_over_threshold();
1278 : void failsafe_ekf_event();
1279 : void failsafe_ekf_off_event(void);
1280 :
1281 : enum class CrowMode {
1282 : NORMAL,
1283 : PROGRESSIVE,
1284 : CROW_DISABLED,
1285 : };
1286 :
1287 : using ThrFailsafe = Parameters::ThrFailsafe;
1288 :
1289 : CrowMode crow_mode = CrowMode::NORMAL;
1290 :
1291 : enum class FlareMode {
1292 : FLARE_DISABLED = 0,
1293 : ENABLED_NO_PITCH_TARGET,
1294 : ENABLED_PITCH_TARGET
1295 : };
1296 :
1297 : enum class AutoTuneAxis {
1298 : ROLL = 1U <<0,
1299 : PITCH = 1U <<1,
1300 : YAW = 1U <<2,
1301 : };
1302 :
1303 : FlareMode flare_mode;
1304 : bool throttle_at_zero(void) const;
1305 :
1306 : // expo handling
1307 : float roll_in_expo(bool use_dz) const;
1308 : float pitch_in_expo(bool use_dz) const;
1309 : float rudder_in_expo(bool use_dz) const;
1310 :
1311 : // mode reason for entering previous mode
1312 : ModeReason previous_mode_reason = ModeReason::UNKNOWN;
1313 :
1314 : // last target alt we passed to tecs
1315 : int32_t tecs_target_alt_cm;
1316 :
1317 : public:
1318 : void failsafe_check(void);
1319 : bool is_landing() const override;
1320 : bool is_taking_off() const override;
1321 : #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
1322 : bool set_target_location(const Location& target_loc) override;
1323 : #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
1324 : #if AP_SCRIPTING_ENABLED
1325 : bool get_target_location(Location& target_loc) override;
1326 : bool update_target_location(const Location &old_loc, const Location &new_loc) override;
1327 : bool set_velocity_match(const Vector2f &velocity_ne_ms) override;
1328 :
1329 : // allow for landing descent rate to be overridden by a script, may be -ve to climb
1330 : bool set_land_descent_rate(float descent_rate_ms) override;
1331 :
1332 : // allow scripts to override mission/guided crosstrack behaviour
1333 : // It's up to the Lua script to ensure the provided location makes sense
1334 : bool set_crosstrack_start(const Location &new_start_location) override;
1335 :
1336 : #endif // AP_SCRIPTING_ENABLED
1337 :
1338 71460 : bool tkoff_option_is_set(AP_FixedWing::TakeoffOption option) const {
1339 71460 : return (aparm.takeoff_options & int32_t(option)) != 0;
1340 : }
1341 :
1342 :
1343 : };
1344 :
1345 : extern Plane plane;
1346 :
1347 : using AP_HAL::millis;
1348 : using AP_HAL::micros;
|
