Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : #pragma once
16 : /*
17 : This is the main Copter class
18 : */
19 :
20 : ////////////////////////////////////////////////////////////////////////////////
21 : // Header includes
22 : ////////////////////////////////////////////////////////////////////////////////
23 :
24 : #include <cmath>
25 : #include <stdio.h>
26 : #include <stdarg.h>
27 :
28 : #include <AP_HAL/AP_HAL.h>
29 :
30 : // Common dependencies
31 : #include <AP_Common/AP_Common.h> // Common definitions and utility routines for the ArduPilot libraries
32 : #include <AP_Common/Location.h> // Library having the implementation of location class
33 : #include <AP_Param/AP_Param.h> // A system for managing and storing variables that are of general interest to the system.
34 : #include <StorageManager/StorageManager.h> // library for Management for hal.storage to allow for backwards compatible mapping of storage offsets to available storage
35 :
36 : // Application dependencies
37 : #include <AP_Logger/AP_Logger.h> // ArduPilot Mega Flash Memory Library
38 : #include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
39 : #include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
40 : #include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
41 : #include <AP_AHRS/AP_AHRS.h> // AHRS (Attitude Heading Reference System) interface library for ArduPilot
42 : #include <AP_Mission/AP_Mission.h> // Mission command library
43 : #include <AP_Mission/AP_Mission_ChangeDetector.h> // Mission command change detection library
44 : #include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
45 : #include <AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h> // 6DoF Attitude control library
46 : #include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
47 : #include <AC_AttitudeControl/AC_PosControl.h> // Position control library
48 : #include <AC_AttitudeControl/AC_CommandModel.h> // Command model library
49 : #include <AP_Motors/AP_Motors.h> // AP Motors library
50 : #include <Filter/Filter.h> // Filter library
51 : #include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
52 : #include <AC_WPNav/AC_WPNav.h> // ArduCopter waypoint navigation library
53 : #include <AC_WPNav/AC_Loiter.h> // ArduCopter Loiter Mode Library
54 : #include <AC_WPNav/AC_Circle.h> // circle navigation library
55 : #include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
56 : #include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
57 : #include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
58 : #include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
59 : #include <AC_InputManager/AC_InputManager.h> // Pilot input handling library
60 : #include <AC_InputManager/AC_InputManager_Heli.h> // Heli specific pilot input handling library
61 : #include <AP_Arming/AP_Arming.h> // ArduPilot motor arming library
62 : #include <AP_SmartRTL/AP_SmartRTL.h> // ArduPilot Smart Return To Launch Mode (SRTL) library
63 : #include <AP_TempCalibration/AP_TempCalibration.h> // temperature calibration library
64 : #include <AC_AutoTune/AC_AutoTune_Multi.h> // ArduCopter autotune library. support for autotune of multirotors.
65 : #include <AC_AutoTune/AC_AutoTune_Heli.h> // ArduCopter autotune library. support for autotune of helicopters.
66 : #include <AP_Parachute/AP_Parachute.h> // ArduPilot parachute release library
67 : #include <AC_Sprayer/AC_Sprayer.h> // Crop sprayer library
68 : #include <AP_Avoidance/AP_Avoidance.h> // "ADSB" avoidance library
69 : #include <AP_ADSB/AP_ADSB.h> // ADS-B RF based collision avoidance module library
70 : #include <AP_Proximity/AP_Proximity.h> // ArduPilot proximity sensor library
71 : #include <AC_PrecLand/AC_PrecLand_config.h>
72 : #include <AP_OpticalFlow/AP_OpticalFlow.h>
73 : #include <AP_Winch/AP_Winch_config.h>
74 : #include <AP_SurfaceDistance/AP_SurfaceDistance.h>
75 :
76 : // Configuration
77 : #include "defines.h"
78 : #include "config.h"
79 :
80 : #if FRAME_CONFIG == HELI_FRAME
81 : #define MOTOR_CLASS AP_MotorsHeli
82 : #else
83 : #define MOTOR_CLASS AP_MotorsMulticopter
84 : #endif
85 :
86 : #if MODE_AUTOROTATE_ENABLED
87 : #include <AC_Autorotation/AC_Autorotation.h> // Autorotation controllers
88 : #endif
89 :
90 : #include "RC_Channel_Copter.h" // RC Channel Library
91 :
92 : #include "GCS_MAVLink_Copter.h"
93 : #include "GCS_Copter.h"
94 : #include "AP_Rally.h" // Rally point library
95 : #include "AP_Arming_Copter.h"
96 :
97 : #include <AP_ExternalControl/AP_ExternalControl_config.h>
98 : #if AP_EXTERNAL_CONTROL_ENABLED
99 : #include "AP_ExternalControl_Copter.h"
100 : #endif
101 :
102 : #if AP_AVOIDANCE_ENABLED
103 : #include <AC_Avoidance/AC_Avoid.h>
104 : #endif
105 : #if AP_OAPATHPLANNER_ENABLED
106 : #include <AC_WPNav/AC_WPNav_OA.h>
107 : #include <AC_Avoidance/AP_OAPathPlanner.h>
108 : #endif
109 : #if AC_PRECLAND_ENABLED
110 : # include <AC_PrecLand/AC_PrecLand.h>
111 : # include <AC_PrecLand/AC_PrecLand_StateMachine.h>
112 : #endif
113 : #if MODE_FOLLOW_ENABLED
114 : # include <AP_Follow/AP_Follow.h>
115 : #endif
116 : #if AP_TERRAIN_AVAILABLE
117 : # include <AP_Terrain/AP_Terrain.h>
118 : #endif
119 : #if AP_RANGEFINDER_ENABLED
120 : # include <AP_RangeFinder/AP_RangeFinder.h>
121 : #endif
122 :
123 : #include <AP_Mount/AP_Mount.h>
124 :
125 : #include <AP_Camera/AP_Camera.h>
126 :
127 : #if HAL_BUTTON_ENABLED
128 : # include <AP_Button/AP_Button.h>
129 : #endif
130 :
131 : #if OSD_ENABLED || OSD_PARAM_ENABLED
132 : #include <AP_OSD/AP_OSD.h>
133 : #endif
134 :
135 : #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
136 : # include "afs_copter.h"
137 : #endif
138 : #if TOY_MODE_ENABLED
139 : # include "toy_mode.h"
140 : #endif
141 : #if AP_WINCH_ENABLED
142 : # include <AP_Winch/AP_Winch.h>
143 : #endif
144 : #include <AP_RPM/AP_RPM.h>
145 :
146 : #if AP_SCRIPTING_ENABLED
147 : #include <AP_Scripting/AP_Scripting.h>
148 : #endif
149 :
150 : #if AC_CUSTOMCONTROL_MULTI_ENABLED
151 : #include <AC_CustomControl/AC_CustomControl.h> // Custom control library
152 : #endif
153 :
154 : #if MODE_BRAKE_ENABLED && !MODE_ALTHOLD_ENABLED
155 : #error Brake mode requires AltHold; disable MODE_BRAKE_ENABLED or enable MODE_ALTHOLD_ENABLED
156 : #endif
157 :
158 : #if AP_AVOIDANCE_ENABLED && !AP_FENCE_ENABLED
159 : #error AC_Avoidance relies on AP_FENCE_ENABLED which is disabled
160 : #endif
161 :
162 : #if AP_OAPATHPLANNER_ENABLED && !AP_FENCE_ENABLED
163 : #error AP_OAPathPlanner relies on AP_FENCE_ENABLED which is disabled
164 : #endif
165 :
166 : #if MODE_AUTOROTATE_ENABLED && !AP_RPM_ENABLED
167 : #error AC_Autorotation relies on AP_RPM_ENABLED which is disabled
168 : #endif
169 :
170 : #if HAL_ADSB_ENABLED
171 : #include "avoidance_adsb.h"
172 : #endif
173 : // Local modules
174 : #include "Parameters.h"
175 : #if USER_PARAMS_ENABLED
176 : #include "UserParameters.h"
177 : #endif
178 : #include "mode.h"
179 :
180 : class Copter : public AP_Vehicle {
181 : public:
182 : friend class GCS_MAVLINK_Copter;
183 : friend class GCS_Copter;
184 : friend class AP_Rally_Copter;
185 : friend class Parameters;
186 : friend class ParametersG2;
187 : friend class AP_Avoidance_Copter;
188 :
189 : #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
190 : friend class AP_AdvancedFailsafe_Copter;
191 : #endif
192 : friend class AP_Arming_Copter;
193 : #if AP_EXTERNAL_CONTROL_ENABLED
194 : friend class AP_ExternalControl_Copter;
195 : #endif
196 : friend class ToyMode;
197 : friend class RC_Channel_Copter;
198 : friend class RC_Channels_Copter;
199 :
200 : friend class AutoTune;
201 :
202 : friend class Mode;
203 : friend class ModeAcro;
204 : friend class ModeAcro_Heli;
205 : friend class ModeAltHold;
206 : friend class ModeAuto;
207 : friend class ModeAutoTune;
208 : friend class ModeAvoidADSB;
209 : friend class ModeBrake;
210 : friend class ModeCircle;
211 : friend class ModeDrift;
212 : friend class ModeFlip;
213 : friend class ModeFlowHold;
214 : friend class ModeFollow;
215 : friend class ModeGuided;
216 : friend class ModeLand;
217 : friend class ModeLoiter;
218 : friend class ModePosHold;
219 : friend class ModeRTL;
220 : friend class ModeSmartRTL;
221 : friend class ModeSport;
222 : friend class ModeStabilize;
223 : friend class ModeStabilize_Heli;
224 : friend class ModeSystemId;
225 : friend class ModeThrow;
226 : friend class ModeZigZag;
227 : friend class ModeAutorotate;
228 : friend class ModeTurtle;
229 :
230 : friend class _AutoTakeoff;
231 :
232 : friend class PayloadPlace;
233 :
234 : Copter(void);
235 :
236 : private:
237 :
238 : // Global parameters are all contained within the 'g' class.
239 : Parameters g;
240 : ParametersG2 g2;
241 :
242 : // used to detect MAVLink acks from GCS to stop compassmot
243 : uint8_t command_ack_counter;
244 :
245 : // primary input control channels
246 : RC_Channel *channel_roll;
247 : RC_Channel *channel_pitch;
248 : RC_Channel *channel_throttle;
249 : RC_Channel *channel_yaw;
250 :
251 : #if AP_RC_TRANSMITTER_TUNING_ENABLED
252 : // channel which is being used to tune a parameter value:
253 : RC_Channel *rc_tuning;
254 : RC_Channel *rc_tuning2;
255 : #endif // AP_RC_TRANSMITTER_TUNING_ENABLED
256 :
257 : AP_SurfaceDistance rangefinder_state {ROTATION_PITCH_270, 0U, &g2.surf_dist_parameters};
258 : AP_SurfaceDistance rangefinder_up_state {ROTATION_PITCH_90, 1U, &g2.surf_dist_parameters};
259 :
260 : // helper function to get inertially interpolated rangefinder height.
261 : bool get_rangefinder_height_interpolated_m(float& ret) const;
262 :
263 : #if AP_RANGEFINDER_ENABLED
264 : class SurfaceTracking {
265 : public:
266 :
267 : // update_surface_offset - manages the vertical offset of the position controller to follow the
268 : // measured ground or ceiling level measured using the range finder.
269 : void update_surface_offset();
270 :
271 : // target has already been set by terrain following so do not initialise again
272 : // this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag)
273 : void external_init();
274 :
275 : // get target and actual distances (in m) for logging purposes
276 : bool get_target_dist_for_logging(float &target_dist_m) const;
277 : float get_dist_for_logging() const;
278 30412448 : void invalidate_for_logging() { valid_for_logging = false; }
279 :
280 : // surface tracking surface
281 : enum class Surface {
282 : NONE = 0,
283 : GROUND = 1,
284 : CEILING = 2
285 : };
286 : // set surface to track
287 : void set_surface(Surface new_surface);
288 : // initialise surface tracking
289 1001 : void init(Surface surf) { surface = surf; }
290 :
291 : private:
292 : Surface surface;
293 : uint32_t last_update_ms; // system time of last update to target_alt_m
294 : uint32_t last_glitch_cleared_ms; // system time of last handle glitch recovery
295 : bool valid_for_logging; // true if we have a desired target altitude
296 : bool reset_target; // true if target should be reset because of change in surface being tracked
297 : } surface_tracking;
298 : #endif
299 :
300 : // Inertial Navigation EKF - different viewpoint
301 : AP_AHRS_View *ahrs_view;
302 :
303 : // Arming/Disarming management class
304 : AP_Arming_Copter arming;
305 :
306 : // Optical flow sensor
307 : #if AP_OPTICALFLOW_ENABLED
308 : AP_OpticalFlow optflow;
309 : #endif
310 :
311 : // external control library
312 : #if AP_EXTERNAL_CONTROL_ENABLED
313 : AP_ExternalControl_Copter external_control;
314 : #endif
315 :
316 :
317 : // old value of counter which increments when our yaw estimate is reset
318 : uint16_t ahrs_yaw_reset_count;
319 : // old value of counter which increments when our attitude estimate is reset
320 : uint16_t attitude_reset_count;
321 :
322 : // vibration check
323 : struct {
324 : bool high_vibes; // true while high vibration are detected
325 : uint32_t start_ms; // system time high vibration were last detected
326 : uint32_t clear_ms; // system time high vibrations stopped
327 : } vibration_check;
328 :
329 : // EKF variances are unfiltered and are designed to recover very quickly when possible
330 : // thus failsafes should be triggered on filtered values in order to avoid transient errors
331 : LowPassFilterFloat pos_variance_filt;
332 : LowPassFilterFloat vel_variance_filt;
333 : bool variances_valid;
334 : uint32_t last_ekf_check_us;
335 :
336 : // takeoff check
337 : uint32_t takeoff_check_warning_ms; // system time user was last warned of takeoff check failure
338 :
339 : // GCS selection
340 : GCS_Copter _gcs; // avoid using this; use gcs()
341 7486 : GCS_Copter &gcs() { return _gcs; }
342 :
343 : // User variables
344 : #ifdef USERHOOK_VARIABLES
345 : # include USERHOOK_VARIABLES
346 : #endif
347 :
348 : // ap_value calculates a 32-bit bitmask representing various pieces of
349 : // state about the Copter. It replaces a global variable which was
350 : // used to track this state.
351 : uint32_t ap_value() const;
352 :
353 : // These variables are essentially global variables. These should
354 : // be removed over time. It is critical that the offsets of these
355 : // variables remain unchanged - the logging is dependent on this
356 : // ordering!
357 : struct PACKED {
358 : bool unused1; // 0
359 : bool unused_was_simple_mode_byte1; // 1
360 : bool unused_was_simple_mode_byte2; // 2
361 : bool pre_arm_rc_check; // 3 true if rc input pre-arm checks have been completed successfully
362 : bool pre_arm_check; // 4 true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
363 : bool auto_armed; // 5 stops auto missions from beginning until throttle is raised
364 : bool unused_log_started; // 6
365 : bool land_complete; // 7 true if we have detected a landing
366 : bool unused_new_radio_frame; // 8
367 : bool unused_usb_connected; // 9
368 : bool unused_receiver_present; // 10
369 : bool compass_mot; // 11 true if we are currently performing compassmot calibration
370 : bool motor_test; // 12 true if we are currently performing the motors test
371 : bool initialised; // 13 true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
372 : bool land_complete_maybe; // 14 true if we may have landed (less strict version of land_complete)
373 : bool throttle_zero; // 15 true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
374 : bool system_time_set_unused; // 16 true if the system time has been set from the GPS
375 : bool gps_glitching; // 17 true if GPS glitching is affecting navigation accuracy
376 : bool using_interlock; // 18 aux switch motor interlock function is in use
377 : bool land_repo_active; // 19 true if the pilot is overriding the landing position
378 : bool motor_interlock_switch; // 20 true if pilot is requesting motor interlock enable
379 : bool in_arming_delay; // 21 true while we are armed but waiting to spin motors
380 : bool initialised_params; // 22 true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
381 : bool unused_compass_init_location; // 23
382 : bool unused2_aux_switch_rc_override_allowed; // 24
383 : bool armed_with_airmode_switch; // 25 we armed using a arming switch
384 : bool prec_land_active; // 26 true if precland is active
385 : } ap;
386 :
387 : AirMode air_mode; // air mode is 0 = not-configured ; 1 = disabled; 2 = enabled;
388 : bool force_flying; // force flying is enabled when true;
389 :
390 : // true if air-mode should be honoured; either it was turned on
391 : // explicitly, or we armed with an arming switch which implies it:
392 2660735 : bool air_mode_active() const {
393 2660735 : return air_mode == AirMode::AIRMODE_ENABLED || ap.armed_with_airmode_switch;
394 : }
395 :
396 : // This is the state of the flight control system
397 : // There are multiple states defined such as STABILIZE, ACRO,
398 : Mode *flightmode;
399 :
400 : RCMapper rcmap;
401 :
402 : // inertial nav alt when we armed
403 : float arming_altitude_m;
404 :
405 : // Failsafe
406 : struct {
407 : uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure
408 : uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed
409 :
410 : int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
411 :
412 : uint8_t radio : 1; // A status flag for the radio failsafe
413 : uint8_t gcs : 1; // A status flag for the ground station failsafe
414 : uint8_t ekf : 1; // true if ekf failsafe has occurred
415 : uint8_t terrain : 1; // true if the missing terrain data failsafe has occurred
416 : uint8_t adsb : 1; // true if an adsb related failsafe has occurred
417 : uint8_t deadreckon : 1; // true if a dead reckoning failsafe has triggered
418 : } failsafe;
419 :
420 86797 : bool any_failsafe_triggered() const {
421 86797 : return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb || failsafe.deadreckon;
422 : }
423 :
424 : using FS_GCS_Action = Parameters::FS_GCS_Action;
425 : using FS_THR_Action = Parameters::FS_THR_Action;
426 : using FS_EKF_Action = Parameters::FS_EKF_Action;
427 : using WPYawBehavior = Parameters::WPYawBehavior;
428 :
429 : // dead reckoning state
430 : struct {
431 : bool active; // true if dead reckoning (position estimate using estimated airspeed, no position or velocity source)
432 : bool timeout; // true if dead reckoning has timedout and EKF's position and velocity estimate should no longer be trusted
433 : uint32_t start_ms; // system time that EKF began deadreckoning
434 : } dead_reckoning;
435 :
436 : // Motor Output
437 : MOTOR_CLASS *motors;
438 : const struct AP_Param::GroupInfo *motors_var_info;
439 :
440 : float _home_bearing_rad;
441 : float _home_distance_m;
442 :
443 : // SIMPLE Mode
444 : // Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming
445 : // or in SuperSimple mode when the vehicle leaves a 20m radius from home.
446 : enum class SimpleMode {
447 : NONE = 0,
448 : SIMPLE = 1,
449 : SUPERSIMPLE = 2,
450 : } simple_mode;
451 :
452 : float simple_cos_yaw;
453 : float simple_sin_yaw;
454 : float super_simple_last_bearing_rad;
455 : float super_simple_cos_yaw;
456 : float super_simple_sin_yaw;
457 :
458 : // Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable
459 : float initial_armed_bearing_rad;
460 :
461 : // Battery Sensors
462 : AP_BattMonitor battery{MASK_LOG_CURRENT,
463 : FUNCTOR_BIND_MEMBER(&Copter::handle_battery_failsafe, void, const char*, const int8_t),
464 : _failsafe_priorities};
465 :
466 : #if OSD_ENABLED || OSD_PARAM_ENABLED
467 : AP_OSD osd;
468 : #endif
469 :
470 : // Altitude
471 : float baro_alt_m; // barometer altitude in meters above home
472 : LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
473 :
474 : // filtered pilot's throttle input used to cancel landing if throttle held high
475 : LowPassFilterFloat rc_throttle_control_in_filter;
476 :
477 : // 3D Location vectors
478 : // Current location of the vehicle (altitude is relative to home)
479 : Location current_loc;
480 :
481 : // Attitude, Position and Waypoint navigation objects
482 : // To-Do: move inertial nav up or other navigation variables down here
483 : AC_AttitudeControl *attitude_control;
484 : const struct AP_Param::GroupInfo *attitude_control_var_info;
485 : AC_PosControl *pos_control;
486 : AC_WPNav *wp_nav;
487 : AC_Loiter *loiter_nav;
488 :
489 : #if AC_CUSTOMCONTROL_MULTI_ENABLED
490 : AC_CustomControl custom_control{ahrs_view, attitude_control, motors};
491 : #endif
492 :
493 : #if MODE_CIRCLE_ENABLED
494 : AC_Circle *circle_nav;
495 : #endif
496 :
497 : // System Timers
498 : // --------------
499 : // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.
500 : uint32_t arm_time_ms;
501 :
502 : // Camera
503 : #if AP_CAMERA_ENABLED
504 : AP_Camera camera{MASK_LOG_CAMERA};
505 : #endif
506 :
507 : // Camera/Antenna mount tracking and stabilisation stuff
508 : #if HAL_MOUNT_ENABLED
509 : AP_Mount camera_mount;
510 : #endif
511 :
512 : #if AP_AVOIDANCE_ENABLED
513 : AC_Avoid avoid;
514 : #endif
515 :
516 : // Rally library
517 : #if HAL_RALLY_ENABLED
518 : AP_Rally_Copter rally;
519 : #endif
520 :
521 : // Crop Sprayer
522 : #if HAL_SPRAYER_ENABLED
523 : AC_Sprayer sprayer;
524 : #endif
525 :
526 : // Parachute release
527 : #if HAL_PARACHUTE_ENABLED
528 : AP_Parachute parachute;
529 : #endif
530 :
531 : // Landing Gear Controller
532 : #if AP_LANDINGGEAR_ENABLED
533 : AP_LandingGear landinggear;
534 : #endif
535 :
536 : // terrain handling
537 : #if AP_TERRAIN_AVAILABLE
538 : AP_Terrain terrain;
539 : #endif
540 :
541 : // Precision Landing
542 : #if AC_PRECLAND_ENABLED
543 : AC_PrecLand precland;
544 : AC_PrecLand_StateMachine precland_statemachine;
545 : #endif
546 :
547 : // Pilot Input Management Library
548 : // Only used for Helicopter for now
549 : #if FRAME_CONFIG == HELI_FRAME
550 : AC_InputManager_Heli input_manager;
551 : #endif
552 :
553 : #if HAL_ADSB_ENABLED
554 : AP_ADSB adsb;
555 : #endif // HAL_ADSB_ENABLED
556 :
557 : #if AP_ADSB_AVOIDANCE_ENABLED
558 : // avoidance of adsb enabled vehicles (normally manned vehicles)
559 : AP_Avoidance_Copter avoidance_adsb{adsb};
560 : #endif
561 :
562 : // last valid RC input time
563 : uint32_t last_radio_update_ms;
564 :
565 : // last esc calibration notification update
566 : uint32_t esc_calibration_notify_update_ms;
567 :
568 : // Top-level logic
569 : // setup the var_info table
570 : AP_Param param_loader;
571 :
572 : #if FRAME_CONFIG == HELI_FRAME
573 : // Tradheli flags
574 : typedef struct {
575 : bool dynamic_flight ; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
576 : bool coll_stk_low ; // 1 // true when collective stick is on lower limit
577 : } heli_flags_t;
578 : heli_flags_t heli_flags;
579 :
580 : int16_t hover_roll_trim_scalar_slew;
581 : #endif
582 :
583 : bool standby_active;
584 :
585 : static const AP_Scheduler::Task scheduler_tasks[];
586 : static const AP_Param::Info var_info[];
587 : static const struct LogStructure log_structure[];
588 :
589 : // enum for ESC CALIBRATION
590 : enum ESCCalibrationModes : uint8_t {
591 : ESCCAL_NONE = 0,
592 : ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1,
593 : ESCCAL_PASSTHROUGH_ALWAYS = 2,
594 : ESCCAL_AUTO = 3,
595 : ESCCAL_DISABLED = 9,
596 : };
597 :
598 : enum class FailsafeAction : uint8_t {
599 : NONE = 0,
600 : LAND = 1,
601 : RTL = 2,
602 : SMARTRTL = 3,
603 : SMARTRTL_LAND = 4,
604 : TERMINATE = 5,
605 : AUTO_DO_LAND_START = 6,
606 : BRAKE_LAND = 7
607 : };
608 :
609 : enum class FailsafeOption {
610 : RC_CONTINUE_IF_AUTO = (1<<0), // 1
611 : GCS_CONTINUE_IF_AUTO = (1<<1), // 2
612 : RC_CONTINUE_IF_GUIDED = (1<<2), // 4
613 : CONTINUE_IF_LANDING = (1<<3), // 8
614 : GCS_CONTINUE_IF_PILOT_CONTROL = (1<<4), // 16
615 : RELEASE_GRIPPER = (1<<5), // 32
616 : };
617 :
618 :
619 : enum class FlightOption : uint32_t {
620 : DISABLE_THRUST_LOSS_CHECK = (1<<0), // 1
621 : DISABLE_YAW_IMBALANCE_WARNING = (1<<1), // 2
622 : RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2), // 4
623 : };
624 :
625 : // type of fast rate attitude controller in operation
626 : enum class FastRateType : uint8_t {
627 : FAST_RATE_DISABLED = 0,
628 : FAST_RATE_DYNAMIC = 1,
629 : FAST_RATE_FIXED_ARMED = 2,
630 : FAST_RATE_FIXED = 3,
631 : };
632 :
633 891015 : FastRateType get_fast_rate_type() const { return FastRateType(g2.att_enable.get()); }
634 :
635 : // returns true if option is enabled for this vehicle
636 60824900 : bool option_is_enabled(FlightOption option) const {
637 60824900 : return (g2.flight_options & uint32_t(option)) != 0;
638 : }
639 :
640 : static constexpr int8_t _failsafe_priorities[] = {
641 : (int8_t)FailsafeAction::TERMINATE,
642 : (int8_t)FailsafeAction::LAND,
643 : (int8_t)FailsafeAction::RTL,
644 : (int8_t)FailsafeAction::SMARTRTL_LAND,
645 : (int8_t)FailsafeAction::SMARTRTL,
646 : (int8_t)FailsafeAction::NONE,
647 : -1 // the priority list must end with a sentinel of -1
648 : };
649 :
650 : #define FAILSAFE_LAND_PRIORITY 1
651 : static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == (int8_t)FailsafeAction::LAND,
652 : "FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities");
653 : static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
654 : "_failsafe_priorities is missing the sentinel");
655 :
656 :
657 :
658 : // AP_State.cpp
659 : void set_auto_armed(bool b);
660 : void set_simple_mode(SimpleMode b);
661 : void set_failsafe_radio(bool b);
662 : void set_failsafe_gcs(bool b);
663 : void update_using_interlock();
664 :
665 : // Copter.cpp
666 : void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
667 : uint8_t &task_count,
668 : uint32_t &log_bit) override;
669 : #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
670 : #if MODE_GUIDED_ENABLED
671 : bool set_target_location(const Location& target_loc) override;
672 : bool start_takeoff(const float alt_m) override;
673 : #endif // MODE_GUIDED_ENABLED
674 : #endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
675 :
676 : #if AP_SCRIPTING_ENABLED
677 : #if MODE_GUIDED_ENABLED
678 : bool get_target_location(Location& target_loc) override;
679 : bool update_target_location(const Location &old_loc, const Location &new_loc) override;
680 : bool set_target_pos_NED(const Vector3f& target_pos_ned_m, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool is_terrain_alt) override;
681 : bool set_target_posvel_NED(const Vector3f& target_pos_ned_m, const Vector3f& target_vel_ned_ms) override;
682 : bool set_target_posvelaccel_NED(const Vector3f& target_pos_ned_m, const Vector3f& target_vel_ned_ms, const Vector3f& target_accel_ned_mss, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override;
683 : bool set_target_velocity_NED(const Vector3f& vel_ned_ms, bool align_yaw_to_target) override;
684 : bool set_target_velaccel_NED(const Vector3f& target_vel_ned_ms, const Vector3f& target_accel_ned_mss, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override;
685 : bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
686 : bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override;
687 : bool set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_degs, float pitch_rate_degs, float yaw_rate_degs, float throttle) override;
688 :
689 : // Register a custom mode with given number and names
690 : AP_Vehicle::custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;
691 : #endif
692 : #if MODE_CIRCLE_ENABLED
693 : bool get_circle_radius(float &radius_m) override;
694 : bool set_circle_rate(float rate_dps) override;
695 : #endif
696 : bool set_desired_speed(float speed_ms) override;
697 : #if MODE_AUTO_ENABLED
698 : bool nav_scripting_enable(uint8_t mode) override;
699 : bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override;
700 : void nav_script_time_done(uint16_t id) override;
701 : #endif
702 : // lua scripts use this to retrieve EKF failsafe state
703 : // returns true if the EKF failsafe has triggered
704 : bool has_ekf_failsafed() const override;
705 : #endif // AP_SCRIPTING_ENABLED
706 : bool is_landing() const override;
707 : bool is_taking_off() const override;
708 : void rc_loop();
709 : void throttle_loop();
710 : void update_batt_compass(void);
711 : void loop_rate_logging();
712 : void ten_hz_logging_loop();
713 : void twentyfive_hz_logging();
714 : void three_hz_loop();
715 : void one_hz_loop();
716 : void init_simple_bearing();
717 : void update_super_simple_bearing(bool force_update);
718 : void read_AHRS(void);
719 : void update_altitude();
720 : bool get_wp_distance_m(float &distance) const override;
721 : #if AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
722 : bool get_wp_location(Location &loc) const override;
723 : #endif // AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
724 : bool get_wp_bearing_deg(float &bearing) const override;
725 : bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
726 : bool get_rate_ef_targets(Vector3f& rate_ef_targets_rads) const override;
727 :
728 : // Attitude.cpp
729 : void update_throttle_hover();
730 : float get_pilot_desired_climb_rate_ms();
731 : float get_non_takeoff_throttle();
732 : void set_accel_throttle_I_from_pilot_throttle();
733 : float get_pilot_speed_dn_ms() const;
734 : float get_pilot_speed_up_adjusted_ms() const;
735 : float get_pilot_speed_dn_adjusted_ms() const;
736 : void run_rate_controller_main();
737 :
738 : // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
739 : struct RateControllerRates {
740 : uint8_t fast_logging_rate;
741 : uint8_t medium_logging_rate;
742 : uint8_t filter_rate;
743 : uint8_t main_loop_rate;
744 : };
745 :
746 : uint8_t calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz);
747 : void rate_controller_thread();
748 : void rate_controller_filter_update();
749 : void rate_controller_log_update();
750 : void rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high);
751 : void enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates);
752 : void disable_fast_rate_loop(RateControllerRates& rates);
753 : void update_dynamic_notch_at_specified_rate_main();
754 : // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
755 :
756 : #if AC_CUSTOMCONTROL_MULTI_ENABLED
757 27178469 : void run_custom_controller() { custom_control.update(); }
758 : #endif
759 :
760 : // avoidance.cpp
761 : void low_alt_avoidance();
762 :
763 : #if HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
764 : // avoidance_adsb.cpp
765 : void avoidance_adsb_update(void);
766 : #endif // HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
767 :
768 : // baro_ground_effect.cpp
769 : #if AP_GROUNDEFFECT_ENABLED
770 : void update_ground_effect_detector(void);
771 : #endif
772 : void update_ekf_terrain_height_stable();
773 :
774 : // commands.cpp
775 : void update_home_from_EKF();
776 : void set_home_to_current_location_inflight();
777 : bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
778 : bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
779 :
780 : // compassmot.cpp
781 : MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan);
782 :
783 : // crash_check.cpp
784 : void crash_check();
785 : void thrust_loss_check();
786 : void yaw_imbalance_check();
787 1001 : LowPassFilterFloat yaw_I_filt{0.05f};
788 : uint32_t last_yaw_warn_ms;
789 : void parachute_check();
790 : void parachute_release();
791 : void parachute_manual_release();
792 :
793 : // ekf_check.cpp
794 : void ekf_check();
795 : bool ekf_over_threshold();
796 : void failsafe_ekf_event();
797 : void failsafe_ekf_off_event(void);
798 : void failsafe_ekf_recheck();
799 : void check_ekf_reset();
800 : void check_vibration();
801 :
802 : // esc_calibration.cpp
803 : void esc_calibration_startup_check();
804 : void esc_calibration_passthrough();
805 : void esc_calibration_auto();
806 : void esc_calibration_notify();
807 : void esc_calibration_setup();
808 :
809 : // events.cpp
810 : bool failsafe_option(FailsafeOption opt) const;
811 : void failsafe_radio_on_event();
812 : void failsafe_radio_off_event();
813 : void handle_battery_failsafe(const char* type_str, const int8_t action);
814 : void failsafe_gcs_check();
815 : void failsafe_gcs_on_event(void);
816 : void failsafe_gcs_off_event(void);
817 : void failsafe_terrain_check();
818 : void failsafe_terrain_set_status(bool data_ok);
819 : void failsafe_terrain_on_event();
820 : void gpsglitch_check();
821 : void failsafe_deadreckon_check();
822 : void set_mode_RTL_or_land_with_pause(ModeReason reason);
823 : void set_mode_SmartRTL_or_RTL(ModeReason reason);
824 : void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
825 : void set_mode_auto_do_land_start_or_RTL(ModeReason reason);
826 : void set_mode_brake_or_land_with_pause(ModeReason reason);
827 : bool should_disarm_on_failsafe();
828 : void do_failsafe_action(FailsafeAction action, ModeReason reason);
829 : void announce_failsafe(const char *type, const char *action_undertaken=nullptr);
830 :
831 : // failsafe.cpp
832 : void failsafe_enable();
833 : void failsafe_disable();
834 : #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
835 : void afs_fs_check(void);
836 : #endif
837 :
838 : // fence.cpp
839 : #if AP_FENCE_ENABLED
840 : void fence_check();
841 : void fence_checks_async() override;
842 : #endif
843 :
844 : // heli.cpp
845 : void heli_init();
846 : void check_dynamic_flight(void);
847 : bool should_use_landing_swash() const;
848 : void update_heli_control_dynamics(void);
849 : void heli_update_landing_swash();
850 : float get_pilot_desired_rotor_speed() const;
851 : void heli_update_rotor_speed_targets();
852 : void heli_update_autorotation();
853 : void update_collective_low_flag(int16_t throttle_control);
854 :
855 : // inertia.cpp
856 : void read_inertia();
857 :
858 : // landing_detector.cpp
859 : void update_land_and_crash_detectors();
860 : void update_land_detector();
861 : void set_land_complete(bool b);
862 : void set_land_complete_maybe(bool b);
863 : void update_throttle_mix();
864 : bool get_force_flying() const;
865 : #if HAL_LOGGING_ENABLED
866 : enum class LandDetectorLoggingFlag : uint16_t {
867 : LANDED = 1U << 0,
868 : LANDED_MAYBE = 1U << 1,
869 : LANDING = 1U << 2,
870 : STANDBY_ACTIVE = 1U << 3,
871 : WOW = 1U << 4,
872 : RANGEFINDER_BELOW_2M = 1U << 5,
873 : DESCENT_RATE_LOW = 1U << 6,
874 : ACCEL_STATIONARY = 1U << 7,
875 : LARGE_ANGLE_ERROR = 1U << 8,
876 : LARGE_ANGLE_REQUEST = 1U << 8,
877 : MOTOR_AT_LOWER_LIMIT = 1U << 9,
878 : THROTTLE_MIX_AT_MIN = 1U << 10,
879 : };
880 : struct {
881 : uint32_t last_logged_ms;
882 : uint32_t last_logged_count;
883 : uint16_t last_logged_flags;
884 : } land_detector;
885 : void Log_LDET(uint16_t logging_flags, uint32_t land_detector_count);
886 : #endif
887 :
888 : #if AP_LANDINGGEAR_ENABLED
889 : // landing_gear.cpp
890 : void landinggear_update();
891 : #endif
892 :
893 : // standby.cpp
894 : void standby_update();
895 :
896 : #if HAL_LOGGING_ENABLED
897 : // methods for AP_Vehicle:
898 1001 : const AP_Int32 &get_log_bitmask() override { return g.log_bitmask; }
899 1001 : const struct LogStructure *get_log_structures() const override {
900 1001 : return log_structure;
901 : }
902 : uint8_t get_num_log_structures() const override;
903 :
904 : // Log.cpp
905 : void Log_Write_Control_Tuning();
906 : void Log_Write_Attitude();
907 : void Log_Write_Rate();
908 : void Log_Write_EKF_POS();
909 : void Log_Write_PIDS();
910 : void Log_Write_Data(LogDataID id, int32_t value);
911 : void Log_Write_Data(LogDataID id, uint32_t value);
912 : void Log_Write_Data(LogDataID id, int16_t value);
913 : void Log_Write_Data(LogDataID id, uint16_t value);
914 : void Log_Write_Data(LogDataID id, float value);
915 : void Log_Write_PTUN(uint8_t param, float tuning_val, float tune_min, float tune_max, float norm_in);
916 : void Log_Video_Stabilisation();
917 : void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3p& pos_target_ned_m, bool is_terrain_alt, const Vector3f& vel_target_ms, const Vector3f& accel_target_mss);
918 : void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode submode, float roll_rad, float pitch_rad, float yaw_rad, const Vector3f &ang_vel_rads, float thrust, float climb_rate_ms);
919 : void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
920 : void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq_hz, float angle_x_degs, float angle_y_degs, float angle_z_degs, float accel_x_mss, float accel_y_mss, float accel_z_mss);
921 : void Log_Write_Vehicle_Startup_Messages();
922 : void Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin);
923 : #endif // HAL_LOGGING_ENABLED
924 :
925 : // mode.cpp
926 : bool set_mode(Mode::Number mode, ModeReason reason);
927 : bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
928 : ModeReason _last_reason;
929 : // called when an attempt to change into a mode is unsuccessful:
930 : void mode_change_failed(const Mode *mode, const char *reason);
931 10014 : uint8_t get_mode() const override { return (uint8_t)flightmode->mode_number(); }
932 : bool current_mode_requires_mission() const override;
933 : void update_flight_mode();
934 : void notify_flight_mode();
935 :
936 : // Check if this mode can be entered from the GCS
937 : bool gcs_mode_enabled(const Mode::Number mode_num);
938 :
939 : // Return mask of enabled modes, order does not matter, its just for tracking changes
940 : uint32_t get_available_mode_enabled_mask() const override;
941 :
942 : // mode_land.cpp
943 : void set_mode_land_with_pause(ModeReason reason);
944 : bool landing_with_GPS();
945 :
946 : // motor_test.cpp
947 : void motor_test_output();
948 : bool mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode);
949 : MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, float throttle_value, float timeout_sec, uint8_t motor_count);
950 : void motor_test_stop();
951 :
952 : // motors.cpp
953 : void auto_disarm_check();
954 : void motors_output(bool full_push = true);
955 : void motors_output_main();
956 : void lost_vehicle_check();
957 :
958 : // navigation.cpp
959 : void run_nav_updates(void);
960 : float home_bearing_rad();
961 : float home_distance_m();
962 :
963 : // Parameters.cpp
964 : void load_parameters(void) override;
965 :
966 : // precision_landing.cpp
967 : void init_precland();
968 : void update_precland();
969 :
970 : // radio.cpp
971 : void default_dead_zones();
972 : void init_rc_in();
973 : void init_rc_out();
974 : void read_radio();
975 : void set_throttle_and_failsafe(uint16_t throttle_pwm);
976 : void set_throttle_zero_flag(int16_t throttle_control);
977 : void radio_passthrough_to_motors();
978 : int16_t get_throttle_mid(void);
979 :
980 : // sensors.cpp
981 : void read_barometer(void);
982 : void init_rangefinder(void);
983 : void read_rangefinder(void);
984 : bool rangefinder_alt_ok() const;
985 : bool rangefinder_up_ok() const;
986 : void update_rangefinder_terrain_offset();
987 :
988 : // takeoff_check.cpp
989 : void takeoff_check();
990 :
991 : // system.cpp
992 : void init_ardupilot() override;
993 : void startup_INS_ground();
994 : bool position_ok() const;
995 : bool ekf_has_absolute_position() const;
996 : bool ekf_has_relative_position() const;
997 : bool ekf_alt_ok() const;
998 : void update_auto_armed();
999 : bool should_log(uint32_t mask);
1000 : const char* get_frame_string() const;
1001 : void allocate_motors(void);
1002 : bool is_tradheli() const;
1003 :
1004 : // terrain.cpp
1005 : void terrain_update();
1006 : void terrain_logging();
1007 :
1008 : #if AP_RC_TRANSMITTER_TUNING_ENABLED
1009 : // tuning.cpp
1010 : void tuning();
1011 : void tuning(const class RC_Channel *tuning_ch, int8_t tuning_param, float tuning_min, float tuning_max);
1012 : bool being_tuned(int8_t tuning_param) const;
1013 : #endif // AP_RC_TRANSMITTER_TUNING_ENABLED
1014 :
1015 : // UserCode.cpp
1016 : void userhook_init();
1017 : void userhook_FastLoop();
1018 : void userhook_50Hz();
1019 : void userhook_MediumLoop();
1020 : void userhook_SlowLoop();
1021 : void userhook_SuperSlowLoop();
1022 : void userhook_auxSwitch1(const RC_Channel::AuxSwitchPos ch_flag);
1023 : void userhook_auxSwitch2(const RC_Channel::AuxSwitchPos ch_flag);
1024 : void userhook_auxSwitch3(const RC_Channel::AuxSwitchPos ch_flag);
1025 :
1026 : #if MODE_ACRO_ENABLED
1027 : #if FRAME_CONFIG == HELI_FRAME
1028 : ModeAcro_Heli mode_acro;
1029 : #else
1030 : ModeAcro mode_acro;
1031 : #endif
1032 : #endif
1033 : #if MODE_ALTHOLD_ENABLED
1034 : ModeAltHold mode_althold;
1035 : #endif
1036 : #if MODE_AUTO_ENABLED
1037 : ModeAuto mode_auto;
1038 : #endif
1039 : #if AUTOTUNE_ENABLED
1040 : ModeAutoTune mode_autotune;
1041 : #endif
1042 : #if MODE_BRAKE_ENABLED
1043 : ModeBrake mode_brake;
1044 : #endif
1045 : #if MODE_CIRCLE_ENABLED
1046 : ModeCircle mode_circle;
1047 : #endif
1048 : #if MODE_DRIFT_ENABLED
1049 : ModeDrift mode_drift;
1050 : #endif
1051 : #if MODE_FLIP_ENABLED
1052 : ModeFlip mode_flip;
1053 : #endif
1054 : #if MODE_FOLLOW_ENABLED
1055 : ModeFollow mode_follow;
1056 : #endif
1057 : #if MODE_GUIDED_ENABLED
1058 : ModeGuided mode_guided;
1059 : #if AP_SCRIPTING_ENABLED
1060 : // Custom modes registered at runtime
1061 : ModeGuidedCustom *mode_guided_custom[5];
1062 : #endif
1063 : #endif
1064 : ModeLand mode_land;
1065 : #if MODE_LOITER_ENABLED
1066 : ModeLoiter mode_loiter;
1067 : #endif
1068 : #if MODE_POSHOLD_ENABLED
1069 : ModePosHold mode_poshold;
1070 : #endif
1071 : #if MODE_RTL_ENABLED
1072 : ModeRTL mode_rtl;
1073 : #endif
1074 : #if FRAME_CONFIG == HELI_FRAME
1075 : ModeStabilize_Heli mode_stabilize;
1076 : #else
1077 : ModeStabilize mode_stabilize;
1078 : #endif
1079 : #if MODE_SPORT_ENABLED
1080 : ModeSport mode_sport;
1081 : #endif
1082 : #if MODE_SYSTEMID_ENABLED
1083 : ModeSystemId mode_systemid;
1084 : #endif
1085 : #if AP_ADSB_AVOIDANCE_ENABLED
1086 : ModeAvoidADSB mode_avoid_adsb;
1087 : #endif // AP_ADSB_AVOIDANCE_ENABLED
1088 : #if MODE_THROW_ENABLED
1089 : ModeThrow mode_throw;
1090 : #endif
1091 : #if MODE_GUIDED_NOGPS_ENABLED
1092 : ModeGuidedNoGPS mode_guided_nogps;
1093 : #endif
1094 : #if MODE_SMARTRTL_ENABLED
1095 : ModeSmartRTL mode_smartrtl;
1096 : #endif
1097 : #if MODE_FLOWHOLD_ENABLED
1098 : ModeFlowHold mode_flowhold;
1099 : #endif
1100 : #if MODE_ZIGZAG_ENABLED
1101 : ModeZigZag mode_zigzag;
1102 : #endif
1103 : #if MODE_AUTOROTATE_ENABLED
1104 : ModeAutorotate mode_autorotate;
1105 : #endif
1106 : #if MODE_TURTLE_ENABLED
1107 : ModeTurtle mode_turtle;
1108 : #endif
1109 :
1110 : // mode.cpp
1111 : Mode *mode_from_mode_num(const Mode::Number mode);
1112 : void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
1113 :
1114 : bool started_rate_thread;
1115 : bool using_rate_thread;
1116 :
1117 : public:
1118 : void failsafe_check(); // failsafe.cpp
1119 : };
1120 :
1121 : extern Copter copter;
1122 :
1123 : using AP_HAL::millis;
1124 : using AP_HAL::micros;
|
