/* 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 . */ #pragma once /* This is the main Copter class */ //////////////////////////////////////////////////////////////////////////////// // Header includes //////////////////////////////////////////////////////////////////////////////// #include #include #include #include // Common dependencies #include // Common definitions and utility routines for the ArduPilot libraries #include // Library having the implementation of location class #include // A system for managing and storing variables that are of general interest to the system. #include // library for Management for hal.storage to allow for backwards compatible mapping of storage offsets to available storage // Application dependencies #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Vector/Matrix math Library #include // interface and maths for accelerometer calibration #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library #include // AHRS (Attitude Heading Reference System) interface library for ArduPilot #include // Mission command library #include // Mission command change detection library #include // Attitude control library #include // 6DoF Attitude control library #include // Attitude control library for traditional helicopter #include // Position control library #include // Command model library #include // AP Motors library #include // Filter library #include // needed for AHRS build #include // inertial navigation library #include // ArduCopter waypoint navigation library #include // ArduCopter Loiter Mode Library #include // circle navigation library #include // ArduPilot Mega Declination Helper Library #include // RC input mapping library #include // Battery monitor library #include // Landing Gear library #include // Pilot input handling library #include // Heli specific pilot input handling library #include // ArduPilot motor arming library #include // ArduPilot Smart Return To Launch Mode (SRTL) library #include // temperature calibration library #include // ArduCopter autotune library. support for autotune of multirotors. #include // ArduCopter autotune library. support for autotune of helicopters. #include // ArduPilot parachute release library #include // Crop sprayer library #include // ADS-B RF based collision avoidance module library #include // ArduPilot proximity sensor library #include #include #include #include // Configuration #include "defines.h" #include "config.h" #if FRAME_CONFIG == HELI_FRAME #define MOTOR_CLASS AP_MotorsHeli #else #define MOTOR_CLASS AP_MotorsMulticopter #endif #if MODE_AUTOROTATE_ENABLED #include // Autorotation controllers #endif #include "RC_Channel.h" // RC Channel Library #include "GCS_Mavlink.h" #include "GCS_Copter.h" #include "AP_Rally.h" // Rally point library #include "AP_Arming.h" #include #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_ExternalControl_Copter.h" #endif #include #if AP_BEACON_ENABLED #include #endif #if AP_AVOIDANCE_ENABLED #include #endif #if AP_OAPATHPLANNER_ENABLED #include #include #endif #if AC_PRECLAND_ENABLED # include # include #endif #if MODE_FOLLOW_ENABLED # include #endif #if AP_TERRAIN_AVAILABLE # include #endif #if AP_RANGEFINDER_ENABLED # include #endif #include #include #if HAL_BUTTON_ENABLED # include #endif #if OSD_ENABLED || OSD_PARAM_ENABLED #include #endif #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED # include "afs_copter.h" #endif #if TOY_MODE_ENABLED # include "toy_mode.h" #endif #if AP_WINCH_ENABLED # include #endif #include #if AP_SCRIPTING_ENABLED #include #endif #if AC_CUSTOMCONTROL_MULTI_ENABLED #include // Custom control library #endif #if AP_AVOIDANCE_ENABLED && !AP_FENCE_ENABLED #error AC_Avoidance relies on AP_FENCE_ENABLED which is disabled #endif #if AP_OAPATHPLANNER_ENABLED && !AP_FENCE_ENABLED #error AP_OAPathPlanner relies on AP_FENCE_ENABLED which is disabled #endif #if HAL_ADSB_ENABLED #include "avoidance_adsb.h" #endif // Local modules #include "Parameters.h" #if USER_PARAMS_ENABLED #include "UserParameters.h" #endif #include "mode.h" class Copter : public AP_Vehicle { public: friend class GCS_MAVLINK_Copter; friend class GCS_Copter; friend class AP_Rally_Copter; friend class Parameters; friend class ParametersG2; friend class AP_Avoidance_Copter; #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED friend class AP_AdvancedFailsafe_Copter; #endif friend class AP_Arming_Copter; #if AP_EXTERNAL_CONTROL_ENABLED friend class AP_ExternalControl_Copter; #endif friend class ToyMode; friend class RC_Channel_Copter; friend class RC_Channels_Copter; friend class AutoTune; friend class Mode; friend class ModeAcro; friend class ModeAcro_Heli; friend class ModeAltHold; friend class ModeAuto; friend class ModeAutoTune; friend class ModeAvoidADSB; friend class ModeBrake; friend class ModeCircle; friend class ModeDrift; friend class ModeFlip; friend class ModeFlowHold; friend class ModeFollow; friend class ModeGuided; friend class ModeLand; friend class ModeLoiter; friend class ModePosHold; friend class ModeRTL; friend class ModeSmartRTL; friend class ModeSport; friend class ModeStabilize; friend class ModeStabilize_Heli; friend class ModeSystemId; friend class ModeThrow; friend class ModeZigZag; friend class ModeAutorotate; friend class ModeTurtle; friend class _AutoTakeoff; friend class PayloadPlace; Copter(void); private: // key aircraft parameters passed to multiple libraries AP_MultiCopter aparm; // Global parameters are all contained within the 'g' class. Parameters g; ParametersG2 g2; // used to detect MAVLink acks from GCS to stop compassmot uint8_t command_ack_counter; // primary input control channels RC_Channel *channel_roll; RC_Channel *channel_pitch; RC_Channel *channel_throttle; RC_Channel *channel_yaw; // flight modes convenience array AP_Int8 *flight_modes; const uint8_t num_flight_modes = 6; AP_SurfaceDistance rangefinder_state {ROTATION_PITCH_270, inertial_nav, 0U}; AP_SurfaceDistance rangefinder_up_state {ROTATION_PITCH_90, inertial_nav, 1U}; // helper function to get inertially interpolated rangefinder height. bool get_rangefinder_height_interpolated_cm(int32_t& ret) const; #if AP_RANGEFINDER_ENABLED class SurfaceTracking { public: // update_surface_offset - manages the vertical offset of the position controller to follow the // measured ground or ceiling level measured using the range finder. void update_surface_offset(); // target has already been set by terrain following so do not initalise again // this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag) void external_init(); // get target and actual distances (in m) for logging purposes bool get_target_dist_for_logging(float &target_dist) const; float get_dist_for_logging() const; void invalidate_for_logging() { valid_for_logging = false; } // surface tracking surface enum class Surface { NONE = 0, GROUND = 1, CEILING = 2 }; // set surface to track void set_surface(Surface new_surface); // initialise surface tracking void init(Surface surf) { surface = surf; } private: Surface surface; uint32_t last_update_ms; // system time of last update to target_alt_cm uint32_t last_glitch_cleared_ms; // system time of last handle glitch recovery bool valid_for_logging; // true if we have a desired target altitude bool reset_target; // true if target should be reset because of change in surface being tracked } surface_tracking; #endif #if AP_RPM_ENABLED AP_RPM rpm_sensor; #endif // Inertial Navigation EKF - different viewpoint AP_AHRS_View *ahrs_view; // Arming/Disarming management class AP_Arming_Copter arming; // Optical flow sensor #if AP_OPTICALFLOW_ENABLED AP_OpticalFlow optflow; #endif // external control library #if AP_EXTERNAL_CONTROL_ENABLED AP_ExternalControl_Copter external_control; #endif // system time in milliseconds of last recorded yaw reset from ekf uint32_t ekfYawReset_ms; int8_t ekf_primary_core; // vibration check struct { bool high_vibes; // true while high vibration are detected uint32_t start_ms; // system time high vibration were last detected uint32_t clear_ms; // system time high vibrations stopped } vibration_check; // EKF variances are unfiltered and are designed to recover very quickly when possible // thus failsafes should be triggered on filtered values in order to avoid transient errors LowPassFilterFloat pos_variance_filt; LowPassFilterFloat vel_variance_filt; LowPassFilterFloat hgt_variance_filt; bool variances_valid; uint32_t last_ekf_check_us; // takeoff check uint32_t takeoff_check_warning_ms; // system time user was last warned of takeoff check failure // GCS selection GCS_Copter _gcs; // avoid using this; use gcs() GCS_Copter &gcs() { return _gcs; } // User variables #ifdef USERHOOK_VARIABLES # include USERHOOK_VARIABLES #endif // ap_value calculates a 32-bit bitmask representing various pieces of // state about the Copter. It replaces a global variable which was // used to track this state. uint32_t ap_value() const; // These variables are essentially global variables. These should // be removed over time. It is critical that the offsets of these // variables remain unchanged - the logging is dependent on this // ordering! struct PACKED { bool unused1; // 0 bool unused_was_simple_mode_byte1; // 1 bool unused_was_simple_mode_byte2; // 2 bool pre_arm_rc_check; // 3 true if rc input pre-arm checks have been completed successfully bool pre_arm_check; // 4 true if all pre-arm checks (rc, accel calibration, gps lock) have been performed bool auto_armed; // 5 stops auto missions from beginning until throttle is raised bool unused_log_started; // 6 bool land_complete; // 7 true if we have detected a landing bool new_radio_frame; // 8 Set true if we have new PWM data to act on from the Radio bool unused_usb_connected; // 9 bool unused_receiver_present; // 10 bool compass_mot; // 11 true if we are currently performing compassmot calibration bool motor_test; // 12 true if we are currently performing the motors test bool initialised; // 13 true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes bool land_complete_maybe; // 14 true if we may have landed (less strict version of land_complete) bool throttle_zero; // 15 true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock bool system_time_set_unused; // 16 true if the system time has been set from the GPS bool gps_glitching; // 17 true if GPS glitching is affecting navigation accuracy bool using_interlock; // 18 aux switch motor interlock function is in use bool land_repo_active; // 19 true if the pilot is overriding the landing position bool motor_interlock_switch; // 20 true if pilot is requesting motor interlock enable bool in_arming_delay; // 21 true while we are armed but waiting to spin motors bool initialised_params; // 22 true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done bool unused_compass_init_location; // 23 bool unused2_aux_switch_rc_override_allowed; // 24 bool armed_with_airmode_switch; // 25 we armed using a arming switch bool prec_land_active; // 26 true if precland is active } ap; AirMode air_mode; // air mode is 0 = not-configured ; 1 = disabled; 2 = enabled; bool force_flying; // force flying is enabled when true; // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, Mode *flightmode; RCMapper rcmap; // inertial nav alt when we armed float arming_altitude_m; // Failsafe struct { uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed int8_t radio_counter; // number of iterations with throttle below throttle_fs_value uint8_t radio : 1; // A status flag for the radio failsafe uint8_t gcs : 1; // A status flag for the ground station failsafe uint8_t ekf : 1; // true if ekf failsafe has occurred uint8_t terrain : 1; // true if the missing terrain data failsafe has occurred uint8_t adsb : 1; // true if an adsb related failsafe has occurred uint8_t deadreckon : 1; // true if a dead reckoning failsafe has triggered } failsafe; bool any_failsafe_triggered() const { return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb || failsafe.deadreckon; } // dead reckoning state struct { bool active; // true if dead reckoning (position estimate using estimated airspeed, no position or velocity source) bool timeout; // true if dead reckoning has timedout and EKF's position and velocity estimate should no longer be trusted uint32_t start_ms; // system time that EKF began deadreckoning } dead_reckoning; // Motor Output MOTOR_CLASS *motors; const struct AP_Param::GroupInfo *motors_var_info; int32_t _home_bearing; uint32_t _home_distance; // SIMPLE Mode // Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming // or in SuperSimple mode when the vehicle leaves a 20m radius from home. enum class SimpleMode { NONE = 0, SIMPLE = 1, SUPERSIMPLE = 2, } simple_mode; float simple_cos_yaw; float simple_sin_yaw; int32_t super_simple_last_bearing; float super_simple_cos_yaw; float super_simple_sin_yaw; // Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable int32_t initial_armed_bearing; // Battery Sensors AP_BattMonitor battery{MASK_LOG_CURRENT, FUNCTOR_BIND_MEMBER(&Copter::handle_battery_failsafe, void, const char*, const int8_t), _failsafe_priorities}; #if OSD_ENABLED || OSD_PARAM_ENABLED AP_OSD osd; #endif // Altitude int32_t baro_alt; // barometer altitude in cm above home LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests // filtered pilot's throttle input used to cancel landing if throttle held high LowPassFilterFloat rc_throttle_control_in_filter; // 3D Location vectors // Current location of the vehicle (altitude is relative to home) Location current_loc; // Inertial Navigation AP_InertialNav inertial_nav; // Attitude, Position and Waypoint navigation objects // To-Do: move inertial nav up or other navigation variables down here AC_AttitudeControl *attitude_control; const struct AP_Param::GroupInfo *attitude_control_var_info; AC_PosControl *pos_control; AC_WPNav *wp_nav; AC_Loiter *loiter_nav; #if AC_CUSTOMCONTROL_MULTI_ENABLED AC_CustomControl custom_control{ahrs_view, attitude_control, motors, scheduler.get_loop_period_s()}; #endif #if MODE_CIRCLE_ENABLED AC_Circle *circle_nav; #endif // System Timers // -------------- // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. uint32_t arm_time_ms; // Used to exit the roll and pitch auto trim function uint8_t auto_trim_counter; bool auto_trim_started = false; // Camera #if AP_CAMERA_ENABLED AP_Camera camera{MASK_LOG_CAMERA}; #endif // Camera/Antenna mount tracking and stabilisation stuff #if HAL_MOUNT_ENABLED AP_Mount camera_mount; #endif #if AP_AVOIDANCE_ENABLED AC_Avoid avoid; #endif // Rally library #if HAL_RALLY_ENABLED AP_Rally_Copter rally; #endif // Crop Sprayer #if HAL_SPRAYER_ENABLED AC_Sprayer sprayer; #endif // Parachute release #if HAL_PARACHUTE_ENABLED AP_Parachute parachute; #endif // Landing Gear Controller #if AP_LANDINGGEAR_ENABLED AP_LandingGear landinggear; #endif // terrain handling #if AP_TERRAIN_AVAILABLE AP_Terrain terrain; #endif // Precision Landing #if AC_PRECLAND_ENABLED AC_PrecLand precland; AC_PrecLand_StateMachine precland_statemachine; #endif // Pilot Input Management Library // Only used for Helicopter for now #if FRAME_CONFIG == HELI_FRAME AC_InputManager_Heli input_manager; #endif #if HAL_ADSB_ENABLED AP_ADSB adsb; // avoidance of adsb enabled vehicles (normally manned vehicles) AP_Avoidance_Copter avoidance_adsb{adsb}; #endif // last valid RC input time uint32_t last_radio_update_ms; // last esc calibration notification update uint32_t esc_calibration_notify_update_ms; // Top-level logic // setup the var_info table AP_Param param_loader; #if FRAME_CONFIG == HELI_FRAME // Mode filter to reject RC Input glitches. Filter size is 5, and it draws the 4th element, so it can reject 3 low glitches, // and 1 high glitch. This is because any "off" glitches can be highly problematic for a helicopter running an ESC // governor. Even a single "off" frame can cause the rotor to slow dramatically and take a long time to restart. ModeFilterInt16_Size5 rotor_speed_deglitch_filter {4}; // Tradheli flags typedef struct { uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms) bool coll_stk_low ; // 1 // true when collective stick is on lower limit } heli_flags_t; heli_flags_t heli_flags; int16_t hover_roll_trim_scalar_slew; #endif // ground effect detector struct { bool takeoff_expected; bool touchdown_expected; uint32_t takeoff_time_ms; float takeoff_alt_cm; } gndeffect_state; bool standby_active; static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; static const struct LogStructure log_structure[]; // enum for ESC CALIBRATION enum ESCCalibrationModes : uint8_t { ESCCAL_NONE = 0, ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1, ESCCAL_PASSTHROUGH_ALWAYS = 2, ESCCAL_AUTO = 3, ESCCAL_DISABLED = 9, }; enum class FailsafeAction : uint8_t { NONE = 0, LAND = 1, RTL = 2, SMARTRTL = 3, SMARTRTL_LAND = 4, TERMINATE = 5, AUTO_DO_LAND_START = 6, BRAKE_LAND = 7 }; enum class FailsafeOption { RC_CONTINUE_IF_AUTO = (1<<0), // 1 GCS_CONTINUE_IF_AUTO = (1<<1), // 2 RC_CONTINUE_IF_GUIDED = (1<<2), // 4 CONTINUE_IF_LANDING = (1<<3), // 8 GCS_CONTINUE_IF_PILOT_CONTROL = (1<<4), // 16 RELEASE_GRIPPER = (1<<5), // 32 }; enum class FlightOption : uint32_t { DISABLE_THRUST_LOSS_CHECK = (1<<0), // 1 DISABLE_YAW_IMBALANCE_WARNING = (1<<1), // 2 RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2), // 4 REQUIRE_POSITION_FOR_ARMING = (1<<3), // 8 }; // returns true if option is enabled for this vehicle bool option_is_enabled(FlightOption option) const { return (g2.flight_options & uint32_t(option)) != 0; } static constexpr int8_t _failsafe_priorities[] = { (int8_t)FailsafeAction::TERMINATE, (int8_t)FailsafeAction::LAND, (int8_t)FailsafeAction::RTL, (int8_t)FailsafeAction::SMARTRTL_LAND, (int8_t)FailsafeAction::SMARTRTL, (int8_t)FailsafeAction::NONE, -1 // the priority list must end with a sentinel of -1 }; #define FAILSAFE_LAND_PRIORITY 1 static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == (int8_t)FailsafeAction::LAND, "FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities"); static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); // AP_State.cpp void set_auto_armed(bool b); void set_simple_mode(SimpleMode b); void set_failsafe_radio(bool b); void set_failsafe_gcs(bool b); void update_using_interlock(); // Copter.cpp void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) override; #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if MODE_GUIDED_ENABLED bool set_target_location(const Location& target_loc) override; #endif // MODE_GUIDED_ENABLED #endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if AP_SCRIPTING_ENABLED #if MODE_GUIDED_ENABLED bool start_takeoff(float alt) override; bool get_target_location(Location& target_loc) override; bool update_target_location(const Location &old_loc, const Location &new_loc) override; bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) override; bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override; bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override; bool set_target_velocity_NED(const Vector3f& vel_ned) override; bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override; 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; bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override; #endif #if MODE_CIRCLE_ENABLED bool get_circle_radius(float &radius_m) override; bool set_circle_rate(float rate_dps) override; #endif bool set_desired_speed(float speed) override; #if MODE_AUTO_ENABLED bool nav_scripting_enable(uint8_t mode) override; bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override; void nav_script_time_done(uint16_t id) override; #endif // lua scripts use this to retrieve EKF failsafe state // returns true if the EKF failsafe has triggered bool has_ekf_failsafed() const override; #endif // AP_SCRIPTING_ENABLED bool is_landing() const override; bool is_taking_off() const override; void rc_loop(); void throttle_loop(); void update_batt_compass(void); void loop_rate_logging(); void ten_hz_logging_loop(); void twentyfive_hz_logging(); void three_hz_loop(); void one_hz_loop(); void init_simple_bearing(); void update_simple_mode(void); void update_super_simple_bearing(bool force_update); void read_AHRS(void); void update_altitude(); bool get_wp_distance_m(float &distance) const override; bool get_wp_bearing_deg(float &bearing) const override; bool get_wp_crosstrack_error_m(float &xtrack_error) const override; bool get_rate_ef_targets(Vector3f& rate_ef_targets) const override; // Attitude.cpp void update_throttle_hover(); float get_pilot_desired_climb_rate(float throttle_control); float get_non_takeoff_throttle(); void set_accel_throttle_I_from_pilot_throttle(); void rotate_body_frame_to_NE(float &x, float &y); uint16_t get_pilot_speed_dn() const; void run_rate_controller(); #if AC_CUSTOMCONTROL_MULTI_ENABLED void run_custom_controller() { custom_control.update(); } #endif // avoidance.cpp void low_alt_avoidance(); #if HAL_ADSB_ENABLED // avoidance_adsb.cpp void avoidance_adsb_update(void); #endif // baro_ground_effect.cpp void update_ground_effect_detector(void); void update_ekf_terrain_height_stable(); // commands.cpp void update_home_from_EKF(); void set_home_to_current_location_inflight(); bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; // compassmot.cpp MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan); // crash_check.cpp void crash_check(); void thrust_loss_check(); void yaw_imbalance_check(); LowPassFilterFloat yaw_I_filt{0.05f}; uint32_t last_yaw_warn_ms; void parachute_check(); void parachute_release(); void parachute_manual_release(); // ekf_check.cpp void ekf_check(); bool ekf_over_threshold(); void failsafe_ekf_event(); void failsafe_ekf_off_event(void); void failsafe_ekf_recheck(); void check_ekf_reset(); void check_vibration(); // esc_calibration.cpp void esc_calibration_startup_check(); void esc_calibration_passthrough(); void esc_calibration_auto(); void esc_calibration_notify(); void esc_calibration_setup(); // events.cpp bool failsafe_option(FailsafeOption opt) const; void failsafe_radio_on_event(); void failsafe_radio_off_event(); void handle_battery_failsafe(const char* type_str, const int8_t action); void failsafe_gcs_check(); void failsafe_gcs_on_event(void); void failsafe_gcs_off_event(void); void failsafe_terrain_check(); void failsafe_terrain_set_status(bool data_ok); void failsafe_terrain_on_event(); void gpsglitch_check(); void failsafe_deadreckon_check(); void set_mode_RTL_or_land_with_pause(ModeReason reason); void set_mode_SmartRTL_or_RTL(ModeReason reason); void set_mode_SmartRTL_or_land_with_pause(ModeReason reason); void set_mode_auto_do_land_start_or_RTL(ModeReason reason); void set_mode_brake_or_land_with_pause(ModeReason reason); bool should_disarm_on_failsafe(); void do_failsafe_action(FailsafeAction action, ModeReason reason); void announce_failsafe(const char *type, const char *action_undertaken=nullptr); // failsafe.cpp void failsafe_enable(); void failsafe_disable(); #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED void afs_fs_check(void); #endif // fence.cpp #if AP_FENCE_ENABLED void fence_check(); #endif // heli.cpp void heli_init(); void check_dynamic_flight(void); bool should_use_landing_swash() const; void update_heli_control_dynamics(void); void heli_update_landing_swash(); float get_pilot_desired_rotor_speed() const; void heli_update_rotor_speed_targets(); void heli_update_autorotation(); void update_collective_low_flag(int16_t throttle_control); // inertia.cpp void read_inertia(); // landing_detector.cpp void update_land_and_crash_detectors(); void update_land_detector(); void set_land_complete(bool b); void set_land_complete_maybe(bool b); void update_throttle_mix(); bool get_force_flying() const; #if HAL_LOGGING_ENABLED enum class LandDetectorLoggingFlag : uint16_t { LANDED = 1U << 0, LANDED_MAYBE = 1U << 1, LANDING = 1U << 2, STANDBY_ACTIVE = 1U << 3, WOW = 1U << 4, RANGEFINDER_BELOW_2M = 1U << 5, DESCENT_RATE_LOW = 1U << 6, ACCEL_STATIONARY = 1U << 7, LARGE_ANGLE_ERROR = 1U << 8, LARGE_ANGLE_REQUEST = 1U << 8, MOTOR_AT_LOWER_LIMIT = 1U << 9, THROTTLE_MIX_AT_MIN = 1U << 10, }; struct { uint32_t last_logged_ms; uint32_t last_logged_count; uint16_t last_logged_flags; } land_detector; void Log_LDET(uint16_t logging_flags, uint32_t land_detector_count); #endif #if AP_LANDINGGEAR_ENABLED // landing_gear.cpp void landinggear_update(); #endif // standby.cpp void standby_update(); #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_Control_Tuning(); void Log_Write_Attitude(); void Log_Write_EKF_POS(); void Log_Write_PIDS(); void Log_Write_Data(LogDataID id, int32_t value); void Log_Write_Data(LogDataID id, uint32_t value); void Log_Write_Data(LogDataID id, int16_t value); void Log_Write_Data(LogDataID id, uint16_t value); void Log_Write_Data(LogDataID id, float value); void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max); void Log_Video_Stabilisation(); void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target); void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate); 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); void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_Vehicle_Startup_Messages(); #endif // HAL_LOGGING_ENABLED // mode.cpp bool set_mode(Mode::Number mode, ModeReason reason); bool set_mode(const uint8_t new_mode, const ModeReason reason) override; ModeReason _last_reason; // called when an attempt to change into a mode is unsuccessful: void mode_change_failed(const Mode *mode, const char *reason); uint8_t get_mode() const override { return (uint8_t)flightmode->mode_number(); } bool current_mode_requires_mission() const override; void update_flight_mode(); void notify_flight_mode(); // Check if this mode can be entered from the GCS bool gcs_mode_enabled(const Mode::Number mode_num); // mode_land.cpp void set_mode_land_with_pause(ModeReason reason); bool landing_with_GPS(); // motor_test.cpp void motor_test_output(); bool mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode); 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); void motor_test_stop(); // motors.cpp void arm_motors_check(); void auto_disarm_check(); void motors_output(); void lost_vehicle_check(); // navigation.cpp void run_nav_updates(void); int32_t home_bearing(); uint32_t home_distance(); // Parameters.cpp void load_parameters(void) override; void convert_pid_parameters(void); #if HAL_PROXIMITY_ENABLED void convert_prx_parameters(); #endif void convert_lgr_parameters(void); void convert_tradheli_parameters(void) const; // precision_landing.cpp void init_precland(); void update_precland(); // radio.cpp void default_dead_zones(); void init_rc_in(); void init_rc_out(); void read_radio(); void set_throttle_and_failsafe(uint16_t throttle_pwm); void set_throttle_zero_flag(int16_t throttle_control); void radio_passthrough_to_motors(); int16_t get_throttle_mid(void); // sensors.cpp void read_barometer(void); void init_rangefinder(void); void read_rangefinder(void); bool rangefinder_alt_ok() const; bool rangefinder_up_ok() const; void update_rangefinder_terrain_offset(); void update_optical_flow(void); // takeoff_check.cpp void takeoff_check(); // RC_Channel.cpp void save_trim(); void auto_trim(); void auto_trim_cancel(); // system.cpp void init_ardupilot() override; void startup_INS_ground(); bool position_ok() const; bool ekf_has_absolute_position() const; bool ekf_has_relative_position() const; bool ekf_alt_ok() const; void update_auto_armed(); bool should_log(uint32_t mask); const char* get_frame_string() const; void allocate_motors(void); bool is_tradheli() const; // terrain.cpp void terrain_update(); void terrain_logging(); // tuning.cpp void tuning(); // UserCode.cpp void userhook_init(); void userhook_FastLoop(); void userhook_50Hz(); void userhook_MediumLoop(); void userhook_SlowLoop(); void userhook_SuperSlowLoop(); void userhook_auxSwitch1(const RC_Channel::AuxSwitchPos ch_flag); void userhook_auxSwitch2(const RC_Channel::AuxSwitchPos ch_flag); void userhook_auxSwitch3(const RC_Channel::AuxSwitchPos ch_flag); #if MODE_ACRO_ENABLED #if FRAME_CONFIG == HELI_FRAME ModeAcro_Heli mode_acro; #else ModeAcro mode_acro; #endif #endif ModeAltHold mode_althold; #if MODE_AUTO_ENABLED ModeAuto mode_auto; #endif #if AUTOTUNE_ENABLED ModeAutoTune mode_autotune; #endif #if MODE_BRAKE_ENABLED ModeBrake mode_brake; #endif #if MODE_CIRCLE_ENABLED ModeCircle mode_circle; #endif #if MODE_DRIFT_ENABLED ModeDrift mode_drift; #endif #if MODE_FLIP_ENABLED ModeFlip mode_flip; #endif #if MODE_FOLLOW_ENABLED ModeFollow mode_follow; #endif #if MODE_GUIDED_ENABLED ModeGuided mode_guided; #endif ModeLand mode_land; #if MODE_LOITER_ENABLED ModeLoiter mode_loiter; #endif #if MODE_POSHOLD_ENABLED ModePosHold mode_poshold; #endif #if MODE_RTL_ENABLED ModeRTL mode_rtl; #endif #if FRAME_CONFIG == HELI_FRAME ModeStabilize_Heli mode_stabilize; #else ModeStabilize mode_stabilize; #endif #if MODE_SPORT_ENABLED ModeSport mode_sport; #endif #if MODE_SYSTEMID_ENABLED ModeSystemId mode_systemid; #endif #if HAL_ADSB_ENABLED ModeAvoidADSB mode_avoid_adsb; #endif #if MODE_THROW_ENABLED ModeThrow mode_throw; #endif #if MODE_GUIDED_NOGPS_ENABLED ModeGuidedNoGPS mode_guided_nogps; #endif #if MODE_SMARTRTL_ENABLED ModeSmartRTL mode_smartrtl; #endif #if MODE_FLOWHOLD_ENABLED ModeFlowHold mode_flowhold; #endif #if MODE_ZIGZAG_ENABLED ModeZigZag mode_zigzag; #endif #if MODE_AUTOROTATE_ENABLED ModeAutorotate mode_autorotate; #endif #if MODE_TURTLE_ENABLED ModeTurtle mode_turtle; #endif // mode.cpp Mode *mode_from_mode_num(const Mode::Number mode); void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); public: void failsafe_check(); // failsafe.cpp }; extern Copter copter; using AP_HAL::millis; using AP_HAL::micros;