door Roberto Alejandro Morin Romero 7 jaren geleden
1206
Meer zoals dit
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
/*
This is the main Copter class
*/
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
#include <cmath>
#include <stdio.h>
#include <stdarg.h>
#include <AP_HAL/AP_HAL.h>
// Common dependencies
#include <AP_Common/AP_Common.h>
#include <AP_Common/Location.h>
#include <AP_Menu/AP_Menu.h>
#include <AP_Param/AP_Param.h>
#include <StorageManager/StorageManager.h>
// Application dependencies
#include <GCS_MAVLink/GCS.h>
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
#include <AP_GPS/AP_GPS.h> // ArduPilot GPS library
#include <DataFlash/DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC/AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
#include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS/AP_AHRS.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_Mission/AP_Mission.h> // Mission command library
#include <AC_PID/AC_PID.h> // PID library
#include <AC_PID/AC_PI_2D.h> // PID library (2-axis)
#include <AC_PID/AC_HELI_PID.h> // Heli specific Rate PID library
#include <AC_PID/AC_P.h> // P library
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
#include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
#include <AC_AttitudeControl/AC_PosControl.h> // Position control library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include <AP_Motors/AP_Motors.h> // AP Motors library
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
#include <AP_Proximity/AP_Proximity.h>
#include <AP_Stats/AP_Stats.h> // statistics library
#include <AP_Beacon/AP_Beacon.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
#include <AP_RSSI/AP_RSSI.h> // RSSI Library
#include <Filter/Filter.h> // Filter library
#include <AP_Buffer/AP_Buffer.h> // APM FIFO Buffer
#include <AP_Relay/AP_Relay.h> // APM relay
#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
#include <AP_Camera/AP_Camera.h> // Photo or video camera
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
#include <AP_Airspeed/AP_Airspeed.h> // needed for AHRS build
#include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
#include <AP_InertialNav/AP_InertialNav.h> // ArduPilot Mega inertial navigation library
#include <AC_WPNav/AC_WPNav.h> // ArduCopter waypoint navigation library
#include <AC_WPNav/AC_Circle.h> // circle navigation library
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AC_Fence/AC_Fence.h> // Arducopter Fence library
#include <AC_Avoidance/AC_Avoid.h> // Arducopter stop at fence library
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
#include <AP_Notify/AP_Notify.h> // Notify library
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
#include <AP_BoardConfig/AP_BoardConfig.h> // board configuration library
#include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
#include <AP_Terrain/AP_Terrain.h>
#include <AP_ADSB/AP_ADSB.h>
#include <AP_RPM/AP_RPM.h>
#include <AC_InputManager/AC_InputManager.h> // Pilot input handling library
#include <AC_InputManager/AC_InputManager_Heli.h> // Heli specific pilot input handling library
#include <AP_Button/AP_Button.h>
#include <AP_Arming/AP_Arming.h>
// Configuration
#include "defines.h"
#include "config.h"
#include "GCS_Mavlink.h"
#include "AP_Rally.h" // Rally point library
#include "AP_Arming.h"
// libraries which are dependent on #defines in defines.h and/or config.h
#if SPRAYER == ENABLED
#include <AC_Sprayer/AC_Sprayer.h> // crop sprayer library
#endif
#if GRIPPER_ENABLED == ENABLED
#include <AP_Gripper/AP_Gripper.h> // gripper stuff
#endif
#if PARACHUTE == ENABLED
#include <AP_Parachute/AP_Parachute.h> // Parachute release library
#endif
#if PRECISION_LANDING == ENABLED
#include <AC_PrecLand/AC_PrecLand.h>
#include <AP_IRLock/AP_IRLock.h>
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
#endif
#if ADVANCED_FAILSAFE == ENABLED
#include "afs_copter.h"
#endif
// Local modules
#include "Parameters.h"
#include "avoidance_adsb.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#endif
class Copter : public AP_HAL::HAL::Callbacks {
public:
friend class GCS_MAVLINK_Copter;
friend class AP_Rally_Copter;
friend class Parameters;
friend class ParametersG2;
friend class AP_Avoidance_Copter;
#if ADVANCED_FAILSAFE == ENABLED
friend class AP_AdvancedFailsafe_Copter;
#endif
friend class AP_Arming_Copter;
Copter(void);
// HAL::Callbacks implementation.
void setup() override;
void loop() override;
private:
// key aircraft parameters passed to multiple libraries
AP_Vehicle::MultiCopter aparm;
// cliSerial isn't strictly necessary - it is an alias for hal.console. It may
// be deprecated in favor of hal.console in later releases.
AP_HAL::BetterStream* cliSerial;
// Global parameters are all contained within the 'g' class.
Parameters g;
ParametersG2 g2;
// main loop scheduler
AP_Scheduler scheduler;
// AP_Notify instance
AP_Notify notify;
// used to detect MAVLink acks from GCS to stop compassmot
uint8_t command_ack_counter;
// has a log download started?
bool in_log_download;
// primary input control channels
RC_Channel *channel_roll;
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
RC_Channel *channel_yaw;
// Dataflash
DataFlash_Class DataFlash;
AP_GPS gps;
// flight modes convenience array
AP_Int8 *flight_modes;
AP_Baro barometer;
Compass compass;
AP_InertialSensor ins;
RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270};
struct {
bool enabled:1;
bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder
uint32_t last_healthy_ms;
LowPassFilterFloat alt_cm_filt; // altitude filter
int8_t glitch_count;
} rangefinder_state = { false, false, 0, 0 };
AP_RPM rpm_sensor;
// Inertial Navigation EKF
NavEKF2 EKF2{&ahrs, barometer, rangefinder};
NavEKF3 EKF3{&ahrs, barometer, rangefinder};
AP_AHRS_NavEKF ahrs{ins, barometer, gps, rangefinder, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL sitl;
#endif
// Mission library
AP_Mission mission;
// Arming/Disarming mangement class
AP_Arming_Copter arming {ahrs, barometer, compass, battery, inertial_nav, ins};
// Optical flow sensor
#if OPTFLOW == ENABLED
OpticalFlow optflow{ahrs};
#endif
// gnd speed limit required to observe optical flow sensor limits
float ekfGndSpdLimit;
// scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
float ekfNavVelGainScaler;
// system time in milliseconds of last recorded yaw reset from ekf
uint32_t ekfYawReset_ms = 0;
int8_t ekf_primary_core;
// GCS selection
AP_SerialManager serial_manager;
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
GCS_MAVLINK_Copter gcs_chan[MAVLINK_COMM_NUM_BUFFERS];
GCS _gcs; // avoid using this; use gcs()
GCS &gcs() { return _gcs; }
// User variables
#ifdef USERHOOK_VARIABLES
# include USERHOOK_VARIABLES
#endif
// Documentation of GLobals:
union {
struct {
uint8_t unused1 : 1; // 0
uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE
uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully
uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // 6 // true if dataflash logging has started
uint8_t land_complete : 1; // 7 // true if we have detected a landing
uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio
uint8_t usb_connected : 1; // 9 // true if APM is powered from USB connection
uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update
uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration
uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test
uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete)
uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS
uint8_t gps_base_pos_set : 1; // 17 // true when the gps base position has been set (used for RTK gps only)
enum HomeState home_state : 2; // 18,19 // home status (unset, set, locked)
uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use
uint8_t motor_emergency_stop : 1; // 21 // motor estop switch, shuts off motors when enabled
uint8_t land_repo_active : 1; // 22 // true if the pilot is overriding the landing position
uint8_t motor_interlock_switch : 1; // 23 // true if pilot is requesting motor interlock enable
uint8_t in_arming_delay : 1; // 24 // true while we are armed but waiting to spin motors
};
uint32_t value;
} ap;
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
control_mode_t control_mode;
mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
control_mode_t prev_control_mode;
mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN;
// Structure used to detect changes in the flight mode control switch
struct {
int8_t debounced_switch_position; // currently used switch position
int8_t last_switch_position; // switch position in previous iteration
uint32_t last_edge_time_ms; // system time that switch position was last changed
} control_switch_state;
struct {
bool running;
float max_speed;
float alt_delta;
uint32_t start_ms;
} takeoff_state;
// altitude below which we do no navigation in auto takeoff
float auto_takeoff_no_nav_alt_cm;
RCMapper rcmap;
// board specific config
AP_BoardConfig BoardConfig;
// receiver RSSI
uint8_t receiver_rssi;
// Failsafe
struct {
uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station
uint8_t radio : 1; // 1 // A status flag for the radio failsafe
uint8_t battery : 1; // 2 // A status flag for the battery failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred
uint8_t terrain : 1; // 6 // true if the missing terrain data failsafe has occurred
uint8_t adsb : 1; // 7 // true if an adsb related failsafe has occurred
int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
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
} failsafe;
// sensor health for logging
struct {
uint8_t baro : 1; // true if baro is healthy
uint8_t compass : 1; // true if compass is healthy
uint8_t primary_gps; // primary gps index
} sensor_health;
// Motor Output
#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli_Single
#else
#define MOTOR_CLASS AP_MotorsMulticopter
#endif
MOTOR_CLASS *motors;
// GPS variables
// Sometimes we need to remove the scaling for distance calcs
float scaleLongDown;
// Location & Navigation
int32_t wp_bearing;
// The location of home in relation to the copter in centi-degrees
int32_t home_bearing;
// distance between plane and home in cm
int32_t home_distance;
// distance between plane and next waypoint in cm.
uint32_t wp_distance;
LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
struct {
PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...)
uint32_t hover_start_timestamp; // milliseconds
float hover_throttle_level;
uint32_t descend_start_timestamp; // milliseconds
uint32_t place_start_timestamp; // milliseconds
float descend_throttle_level;
float descend_start_altitude;
float descend_max; // centimetres
} nav_payload_place;
// Auto
AutoMode auto_mode; // controls which auto controller is run
// Guided
GuidedMode guided_mode; // controls which controller is run (pos or vel)
// RTL
RTLState rtl_state; // records state of rtl (initial climb, returning home, etc)
bool rtl_state_complete; // set to true if the current state is completed
struct {
// NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
Location_Class origin_point;
Location_Class climb_target;
Location_Class return_target;
Location_Class descent_target;
bool land;
bool terrain_used;
} rtl_path;
// Circle
bool circle_pilot_yaw_override; // true if pilot is overriding yaw
// SIMPLE Mode
// Used to track the orientation of the copter for Simple mode. This value is reset at each arming
// or in SuperSimple mode when the copter leaves a 20m radius from home.
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;
// Loiter control
uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
uint32_t loiter_time; // How long have we been loitering - The start time in millis
// Brake
uint32_t brake_timeout_start;
uint32_t brake_timeout_ms;
// Delay the next navigation command
int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t nav_delay_time_start;
// Flip
Vector3f flip_orig_attitude; // original copter attitude before flip
// throw mode state
struct {
ThrowModeStage stage;
ThrowModeStage prev_stage;
uint32_t last_log_ms;
bool nextmode_attempted;
uint32_t free_fall_start_ms; // system time free fall was detected
float free_fall_start_velz; // vertical velocity when free fall was detected
} throw_state = {Throw_Disarmed, Throw_Disarmed, 0, false, 0, 0.0f};
// Battery Sensors
AP_BattMonitor battery;
// FrSky telemetry support
#if FRSKY_TELEM_ENABLED == ENABLED
AP_Frsky_Telem frsky_telemetry;
#endif
// Variables for extended status MAVLink messages
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// Altitude
// The cm/s we are moving up or down based on filtered data - Positive = UP
int16_t climb_rate;
float target_rangefinder_alt; // desired altitude in cm above the ground
int32_t baro_alt; // barometer altitude in cm above home
float baro_climbrate; // barometer climbrate in cm/s
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 copter (altitude is relative to home)
Location_Class current_loc;
// Navigation Yaw control
// auto flight mode's yaw mode
uint8_t auto_yaw_mode;
// Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI
Vector3f roi_WP;
// bearing from current location to the yaw_look_at_WP
float yaw_look_at_WP_bearing;
// yaw used for YAW_LOOK_AT_HEADING yaw_mode
int32_t yaw_look_at_heading;
// Deg/s we should turn
int16_t yaw_look_at_heading_slew;
// heading when in yaw_look_ahead_bearing
float yaw_look_ahead_bearing;
// Delay Mission Scripting Command
int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
uint32_t condition_start;
// IMU variables
// Integration time (in seconds) for the gyros (DCM algorithm)
// Updated with the fast loop
float G_Dt;
// Inertial Navigation
AP_InertialNav_NavEKF inertial_nav;
// Attitude, Position and Waypoint navigation objects
// To-Do: move inertial nav up or other navigation variables down here
AC_AttitudeControl *attitude_control;
AC_PosControl *pos_control;
AC_WPNav *wp_nav;
AC_Circle *circle_nav;
// Performance monitoring
int16_t pmTest1;
// System Timers
// --------------
// Time in microseconds of main control loop
uint32_t fast_loopTimer;
// Counter of main loop executions. Used for performance monitoring and failsafe processing
uint16_t mainLoop_count;
// Loiter timer - Records how long we have been in loiter
uint32_t rtl_loiter_start_time;
// 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;
// Reference to the relay object
AP_Relay relay;
// handle repeated servo and relay events
AP_ServoRelayEvents ServoRelayEvents;
// Reference to the camera object (it uses the relay object inside it)
#if CAMERA == ENABLED
AP_Camera camera;
#endif
// Camera/Antenna mount tracking and stabilisation stuff
#if MOUNT == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
AP_Mount camera_mount;
#endif
// AC_Fence library to reduce fly-aways
#if AC_FENCE == ENABLED
AC_Fence fence;
#endif
#if AC_AVOID_ENABLED == ENABLED
AC_Avoid avoid;
#endif
// Rally library
#if AC_RALLY == ENABLED
AP_Rally_Copter rally;
#endif
// RSSI
AP_RSSI rssi;
// Crop Sprayer
#if SPRAYER == ENABLED
AC_Sprayer sprayer;
#endif
// Parachute release
#if PARACHUTE == ENABLED
AP_Parachute parachute;
#endif
// Landing Gear Controller
AP_LandingGear landinggear;
// terrain handling
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
AP_Terrain terrain;
#endif
// Precision Landing
#if PRECISION_LANDING == ENABLED
AC_PrecLand precland;
#endif
// Pilot Input Management Library
// Only used for Helicopter for AC3.3, to be expanded to include Multirotor
// child class for AC3.4
#if FRAME_CONFIG == HELI_FRAME
AC_InputManager_Heli input_manager;
#endif
AP_ADSB adsb {ahrs};
// avoidance of adsb enabled vehicles (normally manned vheicles)
AP_Avoidance_Copter avoidance_adsb{ahrs, adsb};
// use this to prevent recursion during sensor init
bool in_mavlink_delay;
// true if we are out of time in our event timeslice
bool gcs_out_of_time;
// last valid RC input time
uint32_t last_radio_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
struct {
uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
uint8_t init_targets_on_arming : 1; // 1 // true if we have been disarmed, and need to reset rate controller targets when we arm
} 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;
// set when we are upgrading parameters from 3.4
bool upgrading_frame_params;
static const AP_Scheduler::Task scheduler_tasks[];
static const AP_Param::Info var_info[];
static const struct LogStructure log_structure[];
void compass_accumulate(void);
void compass_cal_update(void);
void barometer_accumulate(void);
void perf_update(void);
void fast_loop();
void rc_loop();
void throttle_loop();
void update_mount();
void update_trigger(void);
void update_batt_compass(void);
void ten_hz_logging_loop();
void twentyfive_hz_logging();
void three_hz_loop();
void one_hz_loop();
void update_GPS(void);
void init_simple_bearing();
void update_simple_mode(void);
void update_super_simple_bearing(bool force_update);
void read_AHRS(void);
void update_altitude();
void set_home_state(enum HomeState new_home_state);
bool home_is_set();
void set_auto_armed(bool b);
void set_simple_mode(uint8_t b);
void set_failsafe_radio(bool b);
void set_failsafe_battery(bool b);
void set_failsafe_gcs(bool b);
void set_land_complete(bool b);
void set_land_complete_maybe(bool b);
void update_using_interlock();
void set_motor_emergency_stop(bool b);
float get_smoothing_gain();
void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
float get_pilot_desired_yaw_rate(int16_t stick_angle);
void check_ekf_reset();
float get_roi_yaw();
float get_look_ahead_yaw();
void update_throttle_hover();
void set_throttle_takeoff();
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
float get_pilot_desired_climb_rate(float throttle_control);
float get_non_takeoff_throttle();
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
float get_avoidance_adjusted_climbrate(float target_rate);
void auto_takeoff_set_start_alt(void);
void auto_takeoff_attitude_run(float target_yaw_rate);
void set_accel_throttle_I_from_pilot_throttle();
void rotate_body_frame_to_NE(float &x, float &y);
void gcs_send_heartbeat(void);
void gcs_send_deferred(void);
void send_heartbeat(mavlink_channel_t chan);
void send_attitude(mavlink_channel_t chan);
void send_fence_status(mavlink_channel_t chan);
void send_extended_status1(mavlink_channel_t chan);
void send_location(mavlink_channel_t chan);
void send_nav_controller_output(mavlink_channel_t chan);
void send_simstate(mavlink_channel_t chan);
void send_hwstatus(mavlink_channel_t chan);
void send_vfr_hud(mavlink_channel_t chan);
void send_current_waypoint(mavlink_channel_t chan);
void send_rangefinder(mavlink_channel_t chan);
void send_proximity(mavlink_channel_t chan, uint16_t count_max);
void send_rpm(mavlink_channel_t chan);
void rpm_update();
void button_update();
void init_proximity();
void update_proximity();
void stats_update();
void init_beacon();
void update_beacon();
void send_pid_tuning(mavlink_channel_t chan);
void gcs_send_message(enum ap_message id);
void gcs_send_mission_item_reached_message(uint16_t mission_index);
void gcs_data_stream_send(void);
void gcs_check_input(void);
void gcs_send_text(MAV_SEVERITY severity, const char *str);
void do_erase_logs(void);
void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt);
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds);
void Log_Write_Current();
void Log_Write_Optflow();
void Log_Write_Nav_Tuning();
void Log_Write_Control_Tuning();
void Log_Write_Performance();
void Log_Write_Attitude();
void Log_Write_MotBatt();
void Log_Write_Event(uint8_t id);
void Log_Write_Data(uint8_t id, int32_t value);
void Log_Write_Data(uint8_t id, uint32_t value);
void Log_Write_Data(uint8_t id, int16_t value);
void Log_Write_Data(uint8_t id, uint16_t value);
void Log_Write_Data(uint8_t id, float value);
void Log_Write_Error(uint8_t sub_system, uint8_t error_code);
void Log_Write_Baro(void);
void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high);
void Log_Write_Home_And_Origin();
void Log_Sensor_Health();
#if FRAME_CONFIG == HELI_FRAME
void Log_Write_Heli(void);
#endif
void Log_Write_Precland();
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool position_ok);
void Log_Write_Proximity();
void Log_Write_Beacon();
void Log_Write_Vehicle_Startup_Messages();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void start_logging() ;
void load_parameters(void);
void convert_pid_parameters(void);
void userhook_init();
void userhook_FastLoop();
void userhook_50Hz();
void userhook_MediumLoop();
void userhook_SlowLoop();
void userhook_SuperSlowLoop();
void update_home_from_EKF();
void set_home_to_current_location_inflight();
bool set_home_to_current_location();
bool set_home_to_current_location_and_lock();
bool set_home_and_lock(const Location& loc);
bool set_home(const Location& loc);
bool far_from_EKF_origin(const Location& loc);
void set_system_time_from_GPS();
void exit_mission();
void do_RTL(void);
bool verify_takeoff();
bool verify_land();
bool verify_payload_place();
bool verify_loiter_unlimited();
bool verify_loiter_time();
bool verify_RTL();
bool verify_wait_delay();
bool verify_within_distance();
bool verify_yaw();
void do_take_picture();
void log_picture();
MAV_RESULT mavlink_compassmot(mavlink_channel_t chan);
void delay(uint32_t ms);
bool acro_init(bool ignore_checks);
void acro_run();
void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
bool althold_init(bool ignore_checks);
void althold_run();
bool auto_init(bool ignore_checks);
void auto_run();
void auto_takeoff_start(const Location& dest_loc);
void auto_takeoff_run();
void auto_wp_start(const Vector3f& destination);
void auto_wp_start(const Location_Class& dest_loc);
void auto_wp_run();
void auto_spline_run();
void auto_land_start();
void auto_land_start(const Vector3f& destination);
void auto_land_run();
void do_payload_place(const AP_Mission::Mission_Command& cmd);
void auto_payload_place_start();
void auto_payload_place_start(const Vector3f& destination);
void auto_payload_place_run();
bool auto_payload_place_run_should_run();
void auto_payload_place_run_loiter();
void auto_payload_place_run_descend();
void auto_payload_place_run_release();
void auto_rtl_start();
void auto_rtl_run();
void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
void auto_circle_start();
void auto_circle_run();
void auto_nav_guided_start();
void auto_nav_guided_run();
bool auto_loiter_start();
void auto_loiter_run();
uint8_t get_default_auto_yaw_mode(bool rtl);
void set_auto_yaw_mode(uint8_t yaw_mode);
void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle);
void set_auto_yaw_roi(const Location &roi_location);
float get_auto_heading(void);
bool autotune_init(bool ignore_checks);
void autotune_stop();
bool autotune_start(bool ignore_checks);
void autotune_run();
void autotune_attitude_control();
void autotune_backup_gains_and_initialise();
void autotune_load_orig_gains();
void autotune_load_tuned_gains();
void autotune_load_intra_test_gains();
void autotune_load_twitch_gains();
void autotune_save_tuning_gains();
void autotune_update_gcs(uint8_t message_id);
bool autotune_roll_enabled();
bool autotune_pitch_enabled();
bool autotune_yaw_enabled();
void autotune_twitching_test(float measurement, float target, float &measurement_min, float &measurement_max);
void autotune_updating_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
void autotune_updating_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
void autotune_updating_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float target, float measurement_max);
void autotune_updating_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float target, float measurement_max);
void autotune_updating_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float target, float measurement_min, float measurement_max);
void autotune_twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max);
void autotune_get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd);
void avoidance_adsb_update(void);
#if ADVANCED_FAILSAFE == ENABLED
void afs_fs_check(void);
#endif
bool brake_init(bool ignore_checks);
void brake_run();
void brake_timeout_to_loiter_ms(uint32_t timeout_ms);
bool circle_init(bool ignore_checks);
void circle_run();
bool drift_init(bool ignore_checks);
void drift_run();
float get_throttle_assist(float velz, float pilot_throttle_scaled);
bool flip_init(bool ignore_checks);
void flip_run();
bool guided_init(bool ignore_checks);
bool guided_takeoff_start(float final_alt_above_home);
void guided_pos_control_start();
void guided_vel_control_start();
void guided_posvel_control_start();
void guided_angle_control_start();
bool guided_set_destination(const Vector3f& destination);
bool guided_set_destination(const Location_Class& dest_loc);
void guided_set_velocity(const Vector3f& velocity);
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
void guided_run();
void guided_takeoff_run();
void guided_pos_control_run();
void guided_vel_control_run();
void guided_posvel_control_run();
void guided_angle_control_run();
void guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des);
void guided_limit_clear();
void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
void guided_limit_init_time_and_pos();
bool guided_limit_check();
bool guided_nogps_init(bool ignore_checks);
void guided_nogps_run();
bool land_init(bool ignore_checks);
void land_run();
void land_gps_run();
void land_nogps_run();
int32_t land_get_alt_above_ground(void);
void land_run_vertical_control(bool pause_descent = false);
void land_run_horizontal_control();
void land_do_not_use_GPS();
void set_mode_land_with_pause(mode_reason_t reason);
bool landing_with_GPS();
bool loiter_init(bool ignore_checks);
void loiter_run();
#if PRECISION_LANDING == ENABLED
bool do_precision_loiter() const;
void precision_loiter_xy();
void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
bool _precision_loiter_enabled;
#endif
bool poshold_init(bool ignore_checks);
void poshold_run();
void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw);
int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control);
void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity);
void poshold_update_wind_comp_estimate();
void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle);
void poshold_roll_controller_to_pilot_override();
void poshold_pitch_controller_to_pilot_override();
// Throw to launch functionality
bool throw_init(bool ignore_checks);
void throw_run();
bool throw_detected();
bool throw_attitude_good();
bool throw_height_good();
bool throw_position_good();
bool rtl_init(bool ignore_checks);
void rtl_restart_without_terrain();
void rtl_run();
void rtl_climb_start();
void rtl_return_start();
void rtl_climb_return_run();
void rtl_loiterathome_start();
void rtl_loiterathome_run();
void rtl_descent_start();
void rtl_descent_run();
void rtl_land_start();
void rtl_land_run();
void rtl_build_path(bool terrain_following_allowed);
void rtl_compute_return_target(bool terrain_following_allowed);
bool sport_init(bool ignore_checks);
void sport_run();
bool stabilize_init(bool ignore_checks);
void stabilize_run();
void crash_check();
void parachute_check();
void parachute_release();
void parachute_manual_release();
// support for AP_Avoidance custom flight mode, AVOID_ADSB
bool avoid_adsb_init(bool ignore_checks);
void avoid_adsb_run();
bool avoid_adsb_set_velocity(const Vector3f& velocity_neu);
void ekf_check();
bool ekf_over_threshold();
void failsafe_ekf_event();
void failsafe_ekf_off_event(void);
void esc_calibration_startup_check();
void esc_calibration_passthrough();
void esc_calibration_auto();
bool should_disarm_on_failsafe();
void failsafe_radio_on_event();
void failsafe_radio_off_event();
void failsafe_battery_event(void);
void failsafe_gcs_check();
void failsafe_gcs_off_event(void);
void failsafe_terrain_check();
void failsafe_terrain_set_status(bool data_ok);
void failsafe_terrain_on_event();
void set_mode_RTL_or_land_with_pause(mode_reason_t reason);
void update_events();
void failsafe_enable();
void failsafe_disable();
void fence_check();
void fence_send_mavlink_status(mavlink_channel_t chan);
void update_sensor_status_flags(void);
bool set_mode(control_mode_t mode, mode_reason_t reason);
bool gcs_set_mode(uint8_t mode) { return set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND); }
void update_flight_mode();
void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode);
bool mode_requires_GPS(control_mode_t mode);
bool mode_has_manual_throttle(control_mode_t mode);
bool mode_allows_arming(control_mode_t mode, bool arming_from_gcs);
void notify_flight_mode(control_mode_t mode);
void heli_init();
void check_dynamic_flight(void);
void update_heli_control_dynamics(void);
void heli_update_landing_swash();
void heli_update_rotor_speed_targets();
bool heli_acro_init(bool ignore_checks);
void heli_acro_run();
bool heli_stabilize_init(bool ignore_checks);
void heli_stabilize_run();
void read_inertia();
bool land_complete_maybe();
void update_land_and_crash_detectors();
void update_land_detector();
void update_throttle_thr_mix();
void update_ground_effect_detector(void);
void landinggear_update();
void update_notify();
void motor_test_output();
bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc);
uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec);
void motor_test_stop();
void arm_motors_check();
void auto_disarm_check();
bool init_arm_motors(bool arming_from_gcs);
void init_disarm_motors();
void motors_output();
void lost_vehicle_check();
void run_nav_updates(void);
void calc_distance_and_bearing();
void calc_wp_distance();
void calc_wp_bearing();
void calc_home_distance_and_bearing();
void run_autopilot();
void perf_info_reset();
void perf_ignore_this_loop();
void perf_info_check_loop_time(uint32_t time_in_micros);
uint16_t perf_info_get_num_loops();
uint32_t perf_info_get_max_time();
uint32_t perf_info_get_min_time();
uint16_t perf_info_get_num_long_running();
uint32_t perf_info_get_num_dropped();
Vector3f pv_location_to_vector(const Location& loc);
float pv_alt_above_origin(float alt_above_home_cm);
float pv_alt_above_home(float alt_above_origin_cm);
float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
float pv_distance_to_home_cm(const Vector3f &destination);
void default_dead_zones();
void init_rc_in();
void init_rc_out();
void enable_motor_output();
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();
void init_barometer(bool full_calibration);
void read_barometer(void);
void init_rangefinder(void);
void read_rangefinder(void);
bool rangefinder_alt_ok();
void init_compass();
void init_optflow();
void update_optical_flow(void);
void init_precland();
void update_precland();
void read_battery(void);
void read_receiver_rssi(void);
void epm_update();
void gripper_update();
void terrain_update();
void terrain_logging();
bool terrain_use();
void report_batt_monitor();
void report_frame();
void report_radio();
void report_ins();
void report_flight_modes();
void report_optflow();
void print_radio_values();
void print_switch(uint8_t p, uint8_t m, bool b);
void print_accel_offsets_and_scaling(void);
void print_gyro_offsets(void);
void report_compass();
void print_blanks(int16_t num);
void print_divider(void);
void print_enabled(bool b);
void report_version();
void read_control_switch();
bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check);
bool check_duplicate_auxsw(void);
void reset_control_switch();
uint8_t read_3pos_switch(uint8_t chan);
void read_aux_switches();
void init_aux_switches();
void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag);
void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag);
void save_trim();
void auto_trim();
void init_ardupilot();
void startup_INS_ground();
bool calibrate_gyros();
bool position_ok();
bool ekf_position_ok();
bool optflow_position_ok();
void update_auto_armed();
void check_usb_mux(void);
bool should_log(uint32_t mask);
void set_default_frame_class();
void allocate_motors(void);
uint8_t get_frame_mav_type();
const char* get_frame_string();
bool current_mode_has_user_takeoff(bool must_navigate);
bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate);
void takeoff_timer_start(float alt_cm);
void takeoff_stop();
void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate);
void print_hit_enter();
void tuning();
void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
Location_Class terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const;
bool do_guided(const AP_Mission::Mission_Command& cmd);
void do_takeoff(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
void do_land(const AP_Mission::Mission_Command& cmd);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_circle(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
void do_spline_wp(const AP_Mission::Mission_Command& cmd);
#if NAV_GUIDED == ENABLED
void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
#endif
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);
void do_yaw(const AP_Mission::Mission_Command& cmd);
void do_change_speed(const AP_Mission::Mission_Command& cmd);
void do_set_home(const AP_Mission::Mission_Command& cmd);
void do_roi(const AP_Mission::Mission_Command& cmd);
void do_mount_control(const AP_Mission::Mission_Command& cmd);
#if CAMERA == ENABLED
void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
#endif
#if PARACHUTE == ENABLED
void do_parachute(const AP_Mission::Mission_Command& cmd);
#endif
#if GRIPPER_ENABLED == ENABLED
void do_gripper(const AP_Mission::Mission_Command& cmd);
#endif
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
bool verify_circle(const AP_Mission::Mission_Command& cmd);
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
#if NAV_GUIDED == ENABLED
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
#endif
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
void log_init(void);
void run_cli(AP_HAL::UARTDriver *port);
void init_capabilities(void);
void dataflash_periodic(void);
void accel_cal_update(void);
public:
void mavlink_delay_cb();
void failsafe_check();
int8_t dump_log(uint8_t argc, const Menu::arg *argv);
int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
int8_t select_logs(uint8_t argc, const Menu::arg *argv);
bool print_log_menu(void);
int8_t process_logs(uint8_t argc, const Menu::arg *argv);
int8_t main_menu_help(uint8_t, const Menu::arg*);
int8_t setup_mode(uint8_t argc, const Menu::arg *argv);
int8_t setup_factory(uint8_t argc, const Menu::arg *argv);
int8_t setup_set(uint8_t argc, const Menu::arg *argv);
int8_t setup_show(uint8_t argc, const Menu::arg *argv);
int8_t esc_calib(uint8_t argc, const Menu::arg *argv);
int8_t test_mode(uint8_t argc, const Menu::arg *argv);
int8_t test_baro(uint8_t argc, const Menu::arg *argv);
int8_t test_compass(uint8_t argc, const Menu::arg *argv);
int8_t test_ins(uint8_t argc, const Menu::arg *argv);
int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
int8_t test_relay(uint8_t argc, const Menu::arg *argv);
int8_t test_shell(uint8_t argc, const Menu::arg *argv);
int8_t test_rangefinder(uint8_t argc, const Menu::arg *argv);
int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
};
#define MENU_FUNC(func) FUNCTOR_BIND(&copter, &Copter::func, int8_t, uint8_t, const Menu::arg *)
extern const AP_HAL::HAL& hal;
extern Copter copter;
using AP_HAL::mill
#pragma once
#include <AP_Avoidance/AP_Avoidance.h>
// Provide Copter-specific implementation of avoidance. While most of
// the logic for doing the actual avoidance is present in
// AP_Avoidance, this class allows Copter to override base
// functionality - for example, not doing anything while landed.
class AP_Avoidance_Copter : public AP_Avoidance {
public:
AP_Avoidance_Copter(AP_AHRS &ahrs, class AP_ADSB &adsb) :
AP_Avoidance(ahrs, adsb) { }
private:
// helper function to set modes and always succeed
void set_mode_else_try_RTL_else_LAND(control_mode_t mode);
protected:
// override avoidance handler
MAV_COLLISION_ACTION handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) override;
// override recovery handler
void handle_recovery(uint8_t recovery_action) override;
// check flight mode is avoid_adsb
bool check_flightmode(bool allow_mode_change);
// vertical avoidance handler
bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
// horizontal avoidance handler
bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
// perpendicular (3 dimensional) avoidance handler
bool handle_avoidance_perpendicular(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
// control mode before avoidance began
control_mode_t prev_control_mode = RTL;
};
#pragma once
#include <AP_Common/AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 120;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus
// ArduCopterMega)
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// values within that range to identify different branches.
//
static const uint16_t k_software_type = 10; // 0 for APM
// trunk
// Parameter identities.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number.
This number is used by
// AP_Param to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them.
When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration.
Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
//
AP_Param load/save code depends on the values here to identify
//
variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
k_param_ins_old, // *** Deprecated, remove with next eeprom number change
k_param_ins, // libraries/AP_InertialSensor variables
k_param_NavEKF2_old, // deprecated - remove
k_param_NavEKF2,
k_param_g2, // 2nd block of parameters
k_param_NavEKF3,
// simulation
k_param_sitl = 10,
// barometer object (needed for SITL)
k_param_barometer,
// scheduler object (for debugging)
k_param_scheduler,
// relay object
k_param_relay,
// (old) EPM object
k_param_epm_unused,
// BoardConfig object
k_param_BoardConfig,
// GPS object
k_param_gps,
// Parachute object
k_param_parachute,
// Landing gear object
k_param_landinggear, // 18
// Input Management object
k_param_input_manager, // 19
// Misc
//
k_param_log_bitmask_old = 20, // Deprecated
k_param_log_last_filenumber, // *** Deprecated - remove
// with next eeprom number
// change
k_param_toy_yaw_rate, // deprecated - remove
k_param_crosstrack_min_distance,
// deprecated - remove with next eeprom number change
k_param_rssi_pin, // unused, replaced by rssi_ library parameters
k_param_throttle_accel_enabled, // deprecated - remove
k_param_wp_yaw_behavior,
k_param_acro_trainer,
k_param_pilot_velocity_z_max,
k_param_circle_rate, // deprecated - remove
k_param_rangefinder_gain,
k_param_ch8_option,
k_param_arming_check_old, // deprecated - remove
k_param_sprayer,
k_param_angle_max,
k_param_gps_hdop_good,
k_param_battery,
k_param_fs_batt_mah,
k_param_angle_rate_max, // remove
k_param_rssi_range, // unused, replaced by rssi_ library parameters
k_param_rc_feel_rp,
k_param_NavEKF, // deprecated - remove
k_param_mission, // mission library
k_param_rc_13_old,
k_param_rc_14_old,
k_param_rally,
k_param_poshold_brake_rate,
k_param_poshold_brake_angle_max,
k_param_pilot_accel_z,
k_param_serial0_baud, // deprecated - remove
k_param_serial1_baud, // deprecated - remove
k_param_serial2_baud, // deprecated - remove
k_param_land_repositioning,
k_param_rangefinder, // rangefinder object
k_param_fs_ekf_thresh,
k_param_terrain,
k_param_acro_rp_expo,
k_param_throttle_deadzone,
k_param_optflow,
k_param_dcmcheck_thresh, // deprecated - remove
k_param_log_bitmask,
k_param_cli_enabled,
k_param_throttle_filt,
k_param_throttle_behavior,
k_param_pilot_takeoff_alt, // 64
// 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove
k_param_gpslock_limit, // deprecated - remove
k_param_geofence_limit, // deprecated - remove
k_param_altitude_limit, // deprecated - remove
k_param_fence,
k_param_gps_glitch, // deprecated
k_param_baro_glitch, // 71 - deprecated
// AP_ADSB Library
k_param_adsb, // 72
k_param_notify, // 73
// 74: precision landing object
k_param_precland = 74,
//
// 75: Singlecopter, CoaxCopter
//
k_param_single_servo_1 = 75, // remove
k_param_single_servo_2, // remove
k_param_single_servo_3, // remove
k_param_single_servo_4, // 78 - remove
//
// 80: Heli
//
k_param_heli_servo_1 = 80, // remove
k_param_heli_servo_2, // remove
k_param_heli_servo_3, // remove
k_param_heli_servo_4, // remove
k_param_heli_pitch_ff, // remove
k_param_heli_roll_ff, // remove
k_param_heli_yaw_ff, // remove
k_param_heli_stab_col_min, // remove
k_param_heli_stab_col_max, // remove
k_param_heli_servo_rsc, // 89 = full! - remove
//
// 90: misc2
//
k_param_motors = 90,
k_param_disarm_delay,
k_param_fs_crash_check,
k_param_throw_motor_start,
k_param_terrain_follow, // 94
k_param_avoid,
k_param_avoidance_adsb,
// 97: RSSI
k_param_rssi = 97,
//
// 100: Inertial Nav
//
k_param_inertial_nav = 100, // deprecated
k_param_wp_nav,
k_param_attitude_control,
k_param_pos_control,
k_param_circle_nav, // 104
// 110: Telemetry control
//
k_param_gcs0 = 110,
k_param_gcs1,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial1_baud_old, // deprecated
k_param_telem_delay,
k_param_gcs2,
k_param_serial2_baud_old, // deprecated
k_param_serial2_protocol, // deprecated
k_param_serial_manager,
k_param_ch9_option,
k_param_ch10_option,
k_param_ch11_option,
k_param_ch12_option,
k_param_takeoff_trigger_dz,
k_param_gcs3,
k_param_gcs_pid_mask, // 126
//
// 135 : reserved for Solo until features merged with master
//
k_param_rtl_speed_cms = 135,
k_param_fs_batt_curr_rtl,
k_param_rtl_cone_slope, // 137
//
// 140: Sensor parameters
//
k_param_imu = 140, // deprecated - can be deleted
k_param_battery_monitoring = 141, // deprecated - can be deleted
k_param_volt_div_ratio, // deprecated - can be deleted
k_param_curr_amp_per_volt, // deprecated - can be deleted
k_param_input_voltage, // deprecated - can be deleted
k_param_pack_capacity, // deprecated - can be deleted
k_param_compass_enabled,
k_param_compass,
k_param_rangefinder_enabled_old, // deprecated
k_param_frame_type,
k_param_optflow_enabled, // deprecated
k_param_fs_batt_voltage,
k_param_ch7_option,
k_param_auto_slew_rate, // deprecated - can be deleted
k_param_rangefinder_type_old, // deprecated
k_param_super_simple = 155,
k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
k_param_copter_leds_mode, // deprecated - remove with next eeprom number change
k_param_ahrs, // AHRS group // 159
//
// 160: Navigation parameters
//
k_param_rtl_altitude = 160,
k_param_crosstrack_gain,
// deprecated - remove with next eeprom number change
k_param_rtl_loiter_time,
k_param_rtl_alt_final,
k_param_tilt_comp,
//164
deprecated - remove with next eeprom number change
//
// Camera and mount parameters
//
k_param_camera = 165,
k_param_camera_mount,
k_param_camera_mount2, // deprecated
//
// Batery monitoring parameters
//
k_param_battery_volt_pin = 168, // deprecated - can be deleted
k_param_battery_curr_pin, // 169 deprecated - can be deleted
//
// 170: Radio settings
//
k_param_rc_1_old = 170,
k_param_rc_2_old,
k_param_rc_3_old,
k_param_rc_4_old,
k_param_rc_5_old,
k_param_rc_6_old,
k_param_rc_7_old,
k_param_rc_8_old,
k_param_rc_10_old,
k_param_rc_11_old,
k_param_throttle_min, // remove
k_param_throttle_max, // remove
k_param_failsafe_throttle,
k_param_throttle_fs_action, // remove
k_param_failsafe_throttle_value,
k_param_throttle_trim, // remove
k_param_esc_calibrate,
k_param_radio_tuning,
k_param_radio_tuning_high,
k_param_radio_tuning_low,
k_param_rc_speed = 192,
k_param_failsafe_battery_enabled,
k_param_throttle_mid, // remove
k_param_failsafe_gps_enabled, // remove
k_param_rc_9_old,
k_param_rc_12_old,
k_param_failsafe_gcs,
k_param_rcmap, // 199
//
// 200: flight modes
//
k_param_flight_mode1 = 200,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
k_param_simple_modes,
//
// 210: Waypoint data
//
k_param_waypoint_mode = 210, // remove
k_param_command_total, // remove
k_param_command_index, // remove
k_param_command_nav_index, // remove
k_param_waypoint_radius, // remove
k_param_circle_radius, // remove
k_param_waypoint_speed_max, // remove
k_param_land_speed,
k_param_auto_velocity_z_min, // remove
k_param_auto_velocity_z_max, // remove - 219
k_param_land_speed_high,
//
// 220: PI/D Controllers
//
k_param_acro_rp_p = 221,
k_param_axis_lock_p, // remove
k_param_pid_rate_roll, // remove
k_param_pid_rate_pitch, // remove
k_param_pid_rate_yaw, // remove
k_param_p_stabilize_roll, // remove
k_param_p_stabilize_pitch, // remove
k_param_p_stabilize_yaw, // remove
k_param_p_pos_xy,
k_param_p_loiter_lon, // remove
k_param_pid_loiter_rate_lat, // remove
k_param_pid_loiter_rate_lon, // remove
k_param_pid_nav_lat, // remove
k_param_pid_nav_lon, // remove
k_param_p_alt_hold,
k_param_p_vel_z,
k_param_pid_optflow_roll, // remove
k_param_pid_optflow_pitch, // remove
k_param_acro_balance_roll_old, // remove
k_param_acro_balance_pitch_old, // remove
k_param_pid_accel_z,
k_param_acro_balance_roll,
k_param_acro_balance_pitch,
k_param_acro_yaw_p,
k_param_autotune_axis_bitmask,
k_param_autotune_aggressiveness,
k_param_pi_vel_xy,
k_param_fs_ekf_action,
k_param_rtl_climb_min,
k_param_rpm_sensor,
k_param_autotune_min_d, // 251
k_param_arming, // 252 - AP_Arming
k_param_DataFlash = 253, // 253 - Logging Group
// 254,255: reserved
// the k_param_* space is 9-bits in size
// 511: reserved
};
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 telem_delay;
#if CLI_ENABLED == ENABLED
AP_Int8 cli_enabled;
#endif
AP_Float throttle_filt;
AP_Int16 throttle_behavior;
AP_Int16 takeoff_trigger_dz;
AP_Float pilot_takeoff_alt;
AP_Int16 rtl_altitude;
AP_Int16 rtl_speed_cms;
AP_Float rtl_cone_slope;
AP_Float rangefinder_gain;
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered
AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position
AP_Int8 compass_enabled;
AP_Int8 super_simple;
AP_Int16 rtl_alt_final;
AP_Int16 rtl_climb_min; // rtl minimum climb in cm
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec
AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees
// Waypoints
//
AP_Int32 rtl_loiter_time;
AP_Int16 land_speed;
AP_Int16 land_speed_high;
AP_Int16 pilot_velocity_z_max; // maximum vertical velocity the pilot may request
AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request
// Throttle
//
AP_Int8 failsafe_throttle;
AP_Int16 failsafe_throttle_value;
AP_Int16 throttle_deadzone;
// Flight modes
//
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
AP_Int8 simple_modes;
// Misc
//
AP_Int32 log_bitmask;
AP_Int8 esc_calibrate;
AP_Int8 radio_tuning;
AP_Int16 radio_tuning_high;
AP_Int16 radio_tuning_low;
AP_Int8 frame_type;
AP_Int8 ch7_option;
AP_Int8 ch8_option;
AP_Int8 ch9_option;
AP_Int8 ch10_option;
AP_Int8 ch11_option;
AP_Int8 ch12_option;
AP_Int8 disarm_delay;
AP_Int8 land_repositioning;
AP_Int8 fs_ekf_action;
AP_Int8 fs_crash_check;
AP_Float fs_ekf_thresh;
AP_Int16 gcs_pid_mask;
AP_Int8 throw_motor_start;
AP_Int8 terrain_follow;
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
// Acro parameters
AP_Float acro_rp_p;
AP_Float acro_yaw_p;
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
// PI/D controllers
AC_PI_2D pi_vel_xy;
AC_P p_vel_z;
AC_PID pid_accel_z;
AC_P p_pos_xy;
AC_P p_alt_hold;
// Autotune
AP_Int8 autotune_axis_bitmask;
AP_Float autotune_aggressiveness;
AP_Float autotune_min_d;
// Note: keep initializers here in the same order as they are declared
// above.
Parameters() :
// PID controller
initial P
initial I initial D initial imax initial filt hz pid rate
//---------------------------------------------------------------------------------------------------------------------------------
pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME),
p_vel_z (VEL_Z_P),
pid_accel_z (ACCEL_Z_P, ACCEL_Z_I, ACCEL_Z_D, ACCEL_Z_IMAX, ACCEL_Z_FILT_HZ, MAIN_LOOP_SECONDS),
// P controller
initial P
//----------------------------------------------------------------------
p_pos_xy (POS_XY_P),
p_alt_hold (ALT_HOLD_P)
{
}
};
/*
2nd block of parameters, to avoid going past 256 top level keys
*/
class ParametersG2 {
public:
ParametersG2(void);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
// altitude at which nav control can start in takeoff
AP_Float wp_navalt_min;
// button checking
AP_Button button;
// vehicle statistics
AP_Stats stats;
#if GRIPPER_ENABLED
AP_Gripper gripper;
#endif
// Throw mode parameters
AP_Int8 throw_nextmode;
AP_Int8 throw_type;
// ground effect compensation enable/disable
AP_Int8 gndeffect_comp_enabled;
// beacon (non-GPS positioning) library
AP_Beacon beacon;
#if PROXIMITY_ENABLED == ENABLED
// proximity (aka object avoidance) library
AP_Proximity proximity;
#endif
// whether to enforce acceptance of packets only from sysid_my_gcs
AP_Int8 sysid_enforce;
#if ADVANCED_FAILSAFE == ENABLED
// advanced failsafe library
AP_AdvancedFailsafe_Copter afs;
#endif
// developer options
AP_Int32 dev_options;
// acro exponent parameters
AP_Float acro_y_expo;
AP_Float acro_thr_mid;
// frame class
AP_Int8 frame_class;
// RC input channels
RC_Channels rc_channels;
// control over servo output ranges
SRV_Channels servo_channels;
};
extern const AP_Param::Info var_info[];
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
/*
advanced failsafe support for copter
*/
#if ADVANCED_FAILSAFE == ENABLED
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
/*
a plane specific AP_AdvancedFailsafe class
*/
class AP_AdvancedFailsafe_Copter : public AP_AdvancedFailsafe
{
public:
AP_AdvancedFailsafe_Copter(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap);
// called to set all outputs to termination state
void terminate_vehicle(void);
protected:
// setup failsafe values for if FMU firmware stops running
void setup_IO_failsafe(void);
// return the AFS mapped control mode
enum control_mode afs_mode(void);
};
#endif // ADVANCED_FAILSAFE
#pragma once
#include <AP_Arming/AP_Arming.h>
class AP_Arming_Copter : public AP_Arming
{
public:
AP_Arming_Copter(const AP_AHRS_NavEKF &ahrs_ref, const AP_Baro &baro, Compass &compass,
const AP_BattMonitor &battery, const AP_InertialNav_NavEKF &inav,
const AP_InertialSensor &ins) :
AP_Arming(ahrs_ref, baro, compass, battery),
_inav(inav),
_ins(ins),
_ahrs_navekf(ahrs_ref)
{
}
void update(void);
bool all_checks_passing(bool arming_from_gcs);
void pre_arm_rc_checks(bool display_failure);
protected:
bool pre_arm_checks(bool display_failure) override;
bool pre_arm_gps_checks(bool display_failure);
bool pre_arm_ekf_attitude_check();
bool pre_arm_terrain_check(bool display_failure);
bool pre_arm_proximity_check(bool display_failure);
bool arm_checks(bool display_failure, bool arming_from_gcs);
// NOTE! the following check functions *DO* call into AP_Arming:
bool ins_checks(bool display_failure) override;
bool compass_checks(bool display_failure) override;
bool gps_checks(bool display_failure) override;
// NOTE! the following check functions *DO NOT* call into AP_Arming!
bool fence_checks(bool display_failure);
bool board_voltage_checks(bool display_failure);
bool parameter_checks(bool display_failure);
bool motor_checks(bool display_failure);
bool pilot_throttle_checks(bool display_failure);
bool barometer_checks(bool display_failure);
bool rc_calibration_checks(bool display_failure);
void set_pre_arm_check(bool b);
void set_pre_arm_rc_check(bool b);
enum HomeState home_status() const override;
private:
void gcs_send_text(MAV_SEVERITY severity, const char *str);
const AP_InertialNav_NavEKF &_inav;
const AP_InertialSensor &_ins;
const AP_AHRS_NavEKF &_ahrs_navekf;
};
#pragma once
#include <AP_Rally/AP_Rally.h>
#include <AP_AHRS/AP_AHRS.h>
class AP_Rally_Copter : public AP_Rally
{
public:
// constructor
AP_Rally_Copter(AP_AHRS &ahrs) : AP_Rally(ahrs) {};
private:
bool is_valid(const Location &rally_point) const override;
};
#pragma once
#include <GCS_MAVLink/GCS.h>
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
class GCS_MAVLINK_Copter : public GCS_MAVLINK
{
public:
void data_stream_send(void) override;
protected:
uint32_t telem_delay() const override;
bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) override;
private:
void handleMessage(mavlink_message_t * msg) override;
bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
bool try_send_message(enum ap_message id) override;
void packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg) override;
};
//
#pragma once
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
//
// DO NOT EDIT this file to adjust your configuration. Create your own
// APM_Config.h and use APM_Config.h.example as a reference.
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
///
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Default and automatic configuration details.
//
// Notes for maintainers:
//
// - Try to keep this file organised in the same order as APM_Config.h.example
//
#include "defines.h"
///
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
/// change in your local copy of APM_Config.h.
///
#include "APM_Config.h"
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
#ifndef CONFIG_HAL_BOARD
#error CONFIG_HAL_BOARD must be defined to build ArduCopter
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_MODE OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#define MAGNETOMETER ENABLED
// run at 400Hz on all systems
# define MAIN_LOOP_RATE 400
# define MAIN_LOOP_SECONDS 0.0025f
# define MAIN_LOOP_MICROS 2500
#ifndef ARMING_DELAY_SEC
# define ARMING_DELAY_SEC 2.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// FRAME_CONFIG
//
#ifndef FRAME_CONFIG
# define FRAME_CONFIG MULTICOPTER_FRAME
#endif
/////////////////////////////////////////////////////////////////////////////////
// TradHeli defaults
#if FRAME_CONFIG == HELI_FRAME
# define RC_FAST_SPEED 125
# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AHEAD
# define THR_MIN_DEFAULT 0
# define AUTOTUNE_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// PWM control
// default RC speed in Hz
#ifndef RC_FAST_SPEED
# define RC_FAST_SPEED 490
#endif
//////////////////////////////////////////////////////////////////////////////
// Rangefinder
//
#ifndef RANGEFINDER_ENABLED
# define RANGEFINDER_ENABLED ENABLED
#endif
#ifndef RANGEFINDER_HEALTH_MAX
# define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder
#endif
#ifndef RANGEFINDER_GAIN_DEFAULT
# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
#endif
#ifndef THR_SURFACE_TRACKING_VELZ_MAX
# define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
#endif
#ifndef RANGEFINDER_TIMEOUT_MS
# define RANGEFINDER_TIMEOUT_MS 1000 // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
#endif
#ifndef RANGEFINDER_WPNAV_FILT_HZ
# define RANGEFINDER_WPNAV_FILT_HZ 0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class
#endif
#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF
# define RANGEFINDER_TILT_CORRECTION ENABLED
#endif
#ifndef RANGEFINDER_GLITCH_ALT_CM
# define RANGEFINDER_GLITCH_ALT_CM 200 // amount of rangefinder change to be considered a glitch
#endif
#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES
# define RANGEFINDER_GLITCH_NUM_SAMPLES 3 // number of rangefinder glitches in a row to take new reading
#endif
//////////////////////////////////////////////////////////////////////////////
// Proximity sensor
//
#ifndef PROXIMITY_ENABLED
# define PROXIMITY_ENABLED ENABLED
#endif
#ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef FS_BATT_VOLTAGE_DEFAULT
# define FS_BATT_VOLTAGE_DEFAULT 10.5f // default battery voltage below which failsafe will be triggered
#endif
#ifndef FS_BATT_MAH_DEFAULT
# define FS_BATT_MAH_DEFAULT 0 // default battery capacity (in mah) below which failsafe will be triggered
#endif
#ifndef BOARD_VOLTAGE_MIN
# define BOARD_VOLTAGE_MIN 4.3f // min board voltage in volts for pre-arm checks
#endif
#ifndef BOARD_VOLTAGE_MAX
# define BOARD_VOLTAGE_MAX 5.8f // max board voltage in volts for pre-arm checks
#endif
// prearm GPS hdop check
#ifndef GPS_HDOP_GOOD_DEFAULT
# define GPS_HDOP_GOOD_DEFAULT 140 // minimum hdop that represents a good position. used during pre-arm checks if fence is enabled
#endif
// GCS failsafe
#ifndef FS_GCS
# define FS_GCS DISABLED
#endif
#ifndef FS_GCS_TIMEOUT_MS
# define FS_GCS_TIMEOUT_MS 5000 // gcs failsafe triggers after 5 seconds with no GCS heartbeat
#endif
// Radio failsafe while using RC_override
#ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS
# define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS 1000 // RC Radio failsafe triggers after 1 second while using RC_override from ground station
#endif
// Radio failsafe
#ifndef FS_RADIO_TIMEOUT_MS
#define FS_RADIO_TIMEOUT_MS 500 // RC Radio Failsafe triggers after 500 miliseconds with No RC Input
#endif
// missing terrain data failsafe
#ifndef FS_TERRAIN_TIMEOUT_MS
#define FS_TERRAIN_TIMEOUT_MS 5000 // 5 seconds of missing terrain data will trigger failsafe (RTL)
#endif
#ifndef PREARM_DISPLAY_PERIOD
# define PREARM_DISPLAY_PERIOD 30
#endif
// pre-arm baro vs inertial nav max alt disparity
#ifndef PREARM_MAX_ALT_DISPARITY_CM
# define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters
#endif
//////////////////////////////////////////////////////////////////////////////
// EKF Failsafe
#ifndef FS_EKF_ACTION_DEFAULT
# define FS_EKF_ACTION_DEFAULT FS_EKF_ACTION_LAND // EKF failsafe triggers land by default
#endif
#ifndef FS_EKF_THRESHOLD_DEFAULT
# define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
#endif
#ifndef EKF_ORIGIN_MAX_DIST_M
# define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km
#endif
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
#ifndef MAGNETOMETER
# define MAGNETOMETER ENABLED
#endif
#ifndef COMPASS_CAL_STICK_GESTURE_TIME
#define COMPASS_CAL_STICK_GESTURE_TIME 2.0f // 2 seconds
#endif
#ifndef COMPASS_CAL_STICK_DELAY
#define COMPASS_CAL_STICK_DELAY 5.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
#ifndef OPTFLOW
# define OPTFLOW ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Auto Tuning
#ifndef AUTOTUNE_ENABLED
# define AUTOTUNE_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Crop Sprayer
#ifndef SPRAYER
# define SPRAYER ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Precision Landing with companion computer or IRLock sensor
#ifndef PRECISION_LANDING
# define PRECISION_LANDING ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//
gripper
#ifndef GRIPPER_ENABLED
# define GRIPPER_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Parachute release
#ifndef PARACHUTE
# define PARACHUTE ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ADSB support
#ifndef ADSB_ENABLED
# define ADSB_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Nav-Guided - allows external nav computer to control vehicle
#ifndef NAV_GUIDED
# define NAV_GUIDED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE
//
#ifndef FLIGHT_MODE_1
# define FLIGHT_MODE_1 STABILIZE
#endif
#ifndef FLIGHT_MODE_2
# define FLIGHT_MODE_2 STABILIZE
#endif
#ifndef FLIGHT_MODE_3
# define FLIGHT_MODE_3 STABILIZE
#endif
#ifndef FLIGHT_MODE_4
# define FLIGHT_MODE_4 STABILIZE
#endif
#ifndef FLIGHT_MODE_5
# define FLIGHT_MODE_5 STABILIZE
#endif
#ifndef FLIGHT_MODE_6
# define FLIGHT_MODE_6 STABILIZE
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle Failsafe
//
#ifndef FS_THR_VALUE_DEFAULT
# define FS_THR_VALUE_DEFAULT 975
#endif
//////////////////////////////////////////////////////////////////////////////
// Takeoff
//
#ifndef PILOT_TKOFF_ALT_DEFAULT
# define PILOT_TKOFF_ALT_DEFAULT 0 // default final alt above home for pilot initiated takeoff
#endif
//////////////////////////////////////////////////////////////////////////////
// Landing
//
#ifndef LAND_SPEED
# define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s
#endif
#ifndef LAND_START_ALT
# define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent
#endif
#ifndef LAND_REPOSITION_DEFAULT
# define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing
#endif
#ifndef LAND_WITH_DELAY_MS
# define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event
#endif
#ifndef LAND_CANCEL_TRIGGER_THR
# define LAND_CANCEL_TRIGGER_THR 700 // land is cancelled by input throttle above 700
#endif
#ifndef LAND_RANGEFINDER_MIN_ALT_CM
#define LAND_RANGEFINDER_MIN_ALT_CM 200
#endif
//////////////////////////////////////////////////////////////////////////////
// Landing Detector
//
#ifndef LAND_DETECTOR_TRIGGER_SEC
# define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing
#endif
#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC
# define LAND_DETECTOR_MAYBE_TRIGGER_SEC 0.2f // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over)
#endif
#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF
# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter
#endif
#ifndef LAND_DETECTOR_ACCEL_MAX
# define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s
#endif
//////////////////////////////////////////////////////////////////////////////
// CAMERA TRIGGER AND CONTROL
//
#ifndef CAMERA
# define CAMERA ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// MOUNT (ANTENNA OR CAMERA)
//
#ifndef MOUNT
# define MOUNT ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Flight mode definitions
//
// Acro Mode
#ifndef ACRO_RP_P
# define ACRO_RP_P 4.5f
#endif
#ifndef ACRO_YAW_P
# define ACRO_YAW_P 4.5f
#endif
#ifndef ACRO_LEVEL_MAX_ANGLE
# define ACRO_LEVEL_MAX_ANGLE 3000
#endif
#ifndef ACRO_BALANCE_ROLL
#define ACRO_BALANCE_ROLL 1.0f
#endif
#ifndef ACRO_BALANCE_PITCH
#define ACRO_BALANCE_PITCH 1.0f
#endif
#ifndef ACRO_RP_EXPO_DEFAULT
#define ACRO_RP_EXPO_DEFAULT 0.3f
#endif
#ifndef ACRO_Y_EXPO_DEFAULT
#define ACRO_Y_EXPO_DEFAULT 0.0f
#endif
#ifndef ACRO_THR_MID_DEFAULT
#define ACRO_THR_MID_DEFAULT 0.0f
#endif
// RTL Mode
#ifndef RTL_ALT_FINAL
# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
#endif
#ifndef RTL_ALT
# define RTL_ALT
1500 // default alt to return to home in cm, 0 = Maintain current altitude
#endif
#ifndef RTL_ALT_MIN
# define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m)
#endif
#ifndef RTL_CLIMB_MIN_DEFAULT
# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL
#endif
#ifndef RTL_ABS_MIN_CLIMB
# define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb
#endif
#ifndef RTL_CONE_SLOPE_DEFAULT
# define RTL_CONE_SLOPE_DEFAULT 3.0f // slope of RTL cone (height / distance). 0 = No cone
#endif
#ifndef RTL_MIN_CONE_SLOPE
# define RTL_MIN_CONE_SLOPE 0.5f // minimum slope of cone
#endif
#ifndef RTL_LOITER_TIME
# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before beginning final descent
#endif
// AUTO Mode
#ifndef WP_YAW_BEHAVIOR_DEFAULT
# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL
#endif
#ifndef AUTO_YAW_SLEW_RATE
# define AUTO_YAW_SLEW_RATE 60 // degrees/sec
#endif
#ifndef YAW_LOOK_AHEAD_MIN_SPEED
# define YAW_LOOK_AHEAD_MIN_SPEED 100 // minimum ground speed in cm/s required before copter is aimed at ground course
#endif
// Super Simple mode
#ifndef SUPER_SIMPLE_RADIUS
# define SUPER_SIMPLE_RADIUS 1000
#endif
//////////////////////////////////////////////////////////////////////////////
// Stabilize Rate Control
//
#ifndef ROLL_PITCH_YAW_INPUT_MAX
# define ROLL_PITCH_YAW_INPUT_MAX 4500 // roll, pitch and yaw input range
#endif
#ifndef DEFAULT_ANGLE_MAX
# define DEFAULT_ANGLE_MAX 4500 // ANGLE_MAX parameters default value
#endif
#ifndef ANGLE_RATE_MAX
# define ANGLE_RATE_MAX 18000 // default maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
#endif
//////////////////////////////////////////////////////////////////////////////
// Loiter position control gains
//
#ifndef POS_XY_P
# define POS_XY_P
1.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// Stop mode defaults
//
#ifndef BRAKE_MODE_SPEED_Z
# define BRAKE_MODE_SPEED_Z 250 // z-axis speed in cm/s in Brake Mode
#endif
#ifndef BRAKE_MODE_DECEL_RATE
# define BRAKE_MODE_DECEL_RATE 750 // acceleration rate in cm/s/s in Brake Mode
#endif
//////////////////////////////////////////////////////////////////////////////
// Velocity (horizontal) gains
//
#ifndef VEL_XY_P
# define VEL_XY_P 1.0f
#endif
#ifndef VEL_XY_I
# define VEL_XY_I 0.5f
#endif
#ifndef VEL_XY_IMAX
# define VEL_XY_IMAX 1000
#endif
#ifndef VEL_XY_FILT_HZ
# define VEL_XY_FILT_HZ 5.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// PosHold parameter defaults
//
#ifndef POSHOLD_ENABLED
# define POSHOLD_ENABLED ENABLED // PosHold flight mode enabled by default
#endif
#ifndef POSHOLD_BRAKE_RATE_DEFAULT
# define POSHOLD_BRAKE_RATE_DEFAULT 8 // default POSHOLD_BRAKE_RATE param value. Rotation rate during braking in deg/sec
#endif
#ifndef POSHOLD_BRAKE_ANGLE_DEFAULT
# define POSHOLD_BRAKE_ANGLE_DEFAULT 3000 // default POSHOLD_BRAKE_ANGLE param value. Max lean angle during braking in centi-degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle control gains
//
#ifndef THR_DZ_DEFAULT
# define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter
#endif
#ifndef ALT_HOLD_P
# define ALT_HOLD_P 1.0f
#endif
// Velocity (vertical) control gains
#ifndef VEL_Z_P
# define VEL_Z_P 5.0f
#endif
// Accel (vertical) control gains
#ifndef ACCEL_Z_P
# define ACCEL_Z_P 0.50f
#endif
#ifndef ACCEL_Z_I
# define ACCEL_Z_I 1.00f
#endif
#ifndef ACCEL_Z_D
# define ACCEL_Z_D 0.0f
#endif
#ifndef ACCEL_Z_IMAX
# define ACCEL_Z_IMAX 800
#endif
#ifndef ACCEL_Z_FILT_HZ
# define ACCEL_Z_FILT_HZ 20.0f
#endif
// default maximum vertical velocity and acceleration the pilot may request
#ifndef PILOT_VELZ_MAX
# define PILOT_VELZ_MAX 250 // maximum vertical velocity in cm/s
#endif
#ifndef PILOT_ACCEL_Z_DEFAULT
# define PILOT_ACCEL_Z_DEFAULT 250 // vertical acceleration in cm/s/s while altitude is under pilot control
#endif
// max distance in cm above or below current location that will be used for the alt target when transitioning to alt-hold mode
#ifndef ALT_HOLD_INIT_MAX_OVERSHOOT
# define ALT_HOLD_INIT_MAX_OVERSHOOT 200
#endif
// the acceleration used to define the distance-velocity curve
#ifndef ALT_HOLD_ACCEL_MAX
# define ALT_HOLD_ACCEL_MAX 250 // if you change this you must also update the duplicate declaration in AC_WPNav.h
#endif
#ifndef AUTO_DISARMING_DELAY
# define AUTO_DISARMING_DELAY 10
#endif
//////////////////////////////////////////////////////////////////////////////
// Throw mode configuration
//
#ifndef THROW_HIGH_SPEED
# define THROW_HIGH_SPEED 500.0f // vehicle much reach this total 3D speed in cm/s (or be free falling)
#endif
#ifndef THROW_VERTICAL_SPEED
# define THROW_VERTICAL_SPEED 50.0f // motors start when vehicle reaches this total 3D speed in cm/s
#endif
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
// Default logging bitmask
#ifndef DEFAULT_LOG_BITMASK
# define DEFAULT_LOG_BITMASK \
MASK_LOG_ATTITUDE_MED | \
MASK_LOG_GPS | \
MASK_LOG_PM | \
MASK_LOG_CTUN | \
MASK_LOG_NTUN | \
MASK_LOG_RCIN | \
MASK_LOG_IMU | \
MASK_LOG_CMD | \
MASK_LOG_CURRENT | \
MASK_LOG_RCOUT | \
MASK_LOG_OPTFLOW | \
MASK_LOG_COMPASS | \
MASK_LOG_CAMERA | \
MASK_LOG_MOTBATT
#endif
//////////////////////////////////////////////////////////////////////////////
// Fence, Rally and Terrain and AC_Avoidance defaults
//
// Enable/disable Fence
#ifndef AC_FENCE
#define AC_FENCE ENABLED
#endif
#ifndef AC_RALLY
#define AC_RALLY ENABLED
#endif
#ifndef AC_TERRAIN
#define AC_TERRAIN ENABLED
#endif
#if AC_TERRAIN && !AC_RALLY
#error Terrain relies on Rally which is disabled
#endif
#ifndef AC_AVOID_ENABLED
#define AC_AVOID_ENABLED ENABLED
#endif
#if AC_AVOID_ENABLED && !PROXIMITY_ENABLED
#error AC_Avoidance relies on PROXIMITY_ENABLED which is disabled
#endif
#if AC_AVOID_ENABLED && !AC_FENCE
#error AC_Avoidance relies on AC_FENCE which is disabled
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED
#endif
//use this to completely disable FRSKY TELEM
#ifndef FRSKY_TELEM_ENABLED
# define FRSKY_TELEM_ENABLED ENABLED
#endif
#ifndef ADVANCED_FAILSAFE
# define ADVANCED_FAILSAFE DISABLED
#endif
// User specific config file. Any items listed in config.h can be overridden here.
// If you used to define your CONFIG_APM_HARDWARE setting here, it is no longer
// valid! You should switch to using a HAL_BOARD flag in your local config.mk.
// uncomment the lines below to disable features (flash sizes listed are for APM2 boards and will underestimate savings on Pixhawk and other boards)
//#define LOGGING_ENABLED DISABLED // disable dataflash logging to save 11K of flash space
//#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space
//#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
//#define AC_FENCE DISABLED // disable fence to save 2k of flash
//#define CAMERA DISABLED // disable camera trigger to save 1k of flash
//#define RANGEFINDER_ENABLED DISABLED // disable rangefinder to save 1k of flash
//#define PROXIMITY_ENABLED DISABLED // disable proximity sensors
//#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash
//#define AC_RALLY DISABLED // disable rally points library (must also disable terrain which relies on rally)
//#define AC_TERRAIN DISABLED // disable terrain library
//#define PARACHUTE DISABLED // disable parachute release to save 1k of flash
//#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space
//#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
//#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space
//#define FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry
//#define ADSB_ENABLED DISABLED // disable ADSB support
//#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor
//#define SPRAYER DISABLED // disable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
// features below are disabled by default on all boards
//#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes
//#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE // disable mode changes from GCS during Radio failsafes. Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link
//#define ADVANCED_FAILSAFE ENABLED // enabled advanced failsafe which allows running a portion of the mission in failsafe events
// other settings
//#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000)
//#define HIL_MODE HIL_MODE_SENSORS // build for hardware-in-the-loop simulation
// User Hooks : For User Developed code that you wish to run
// Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below).
//#define USERHOOK_VARIABLES "UserVariables.h"
// Put your custom code into the UserCode.cpp with function names matching those listed below and ensure the appropriate #define below is uncommented below
//#define USERHOOK_INIT userhook_init(); // for code to be run once at startup
//#define USERHOOK_FASTLOOP userhook_FastLoop(); // for code to be run at 100hz
//#define USERHOOK_50HZLOOP userhook_50Hz(); // for code to be run at 50hz
//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop(); // for code to be run at 10hz
//#define USERHOOK_SLOWLOOP userhook_SlowLoop(); // for code to be run at 3.3hz
//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop(); // for code to be run at 1hz
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
// Just so that it's completely clear...
#define ENABLED 1
#define DISABLED 0
// this avoids a very common config error
#define ENABLE ENABLED
#define DISABLE DISABLED
// Autopilot Yaw Mode enumeration
enum autopilot_yaw_mode {
AUTO_YAW_HOLD = 0, // pilot controls the heading
AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted)
AUTO_YAW_ROI = 2, // point towards a location held in roi_WP (no pilot input accepted)
AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted)
AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving
AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed
};
// Ch6... Ch12 aux switch control
#define AUX_SWITCH_PWM_TRIGGER_HIGH 1800 // pwm value above which the ch7 or ch8 option will be invoked
#define AUX_SWITCH_PWM_TRIGGER_LOW 1200 // pwm value below which the ch7 or ch8 option will be disabled
#define CH6_PWM_TRIGGER_HIGH 1800
#define CH6_PWM_TRIGGER_LOW 1200
// values used by the ap.ch7_opt and ap.ch8_opt flags
#define AUX_SWITCH_LOW 0 // indicates auxiliary switch is in the low position (pwm <1200)
#define AUX_SWITCH_MIDDLE 1 // indicates auxiliary switch is in the middle position (pwm >1200, <1800)
#define AUX_SWITCH_HIGH 2 // indicates auxiliary switch is in the high position (pwm >1800)
// Aux Switch enumeration
enum aux_sw_func {
AUXSW_DO_NOTHING = 0, // aux switch disabled
AUXSW_FLIP = 2, // flip
AUXSW_SIMPLE_MODE = 3, // change to simple mode
AUXSW_RTL = 4, // change to RTL flight mode
AUXSW_SAVE_TRIM = 5, // save current position as level
AUXSW_SAVE_WP = 7, // save mission waypoint or RTL if in auto mode
AUXSW_CAMERA_TRIGGER = 9, // trigger camera servo or relay
AUXSW_RANGEFINDER = 10, // allow enabling or disabling rangefinder in flight which helps avoid surface tracking when you are far above the ground
AUXSW_FENCE = 11, // allow enabling or disabling fence in flight
AUXSW_RESETTOARMEDYAW = 12, // deprecated. changes yaw to be same as when quad was armed
AUXSW_SUPERSIMPLE_MODE = 13, // change to simple mode in middle, super simple at top
AUXSW_ACRO_TRAINER = 14, // low = disabled, middle = leveled, high = leveled and limited
AUXSW_SPRAYER = 15, // enable/disable the crop sprayer
AUXSW_AUTO = 16, // change to auto flight mode
AUXSW_AUTOTUNE = 17, // auto tune
AUXSW_LAND = 18, // change to LAND flight mode
AUXSW_GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on
AUXSW_PARACHUTE_ENABLE = 21, // Parachute enable/disable
AUXSW_PARACHUTE_RELEASE = 22, // Parachute release
AUXSW_PARACHUTE_3POS = 23, // Parachute disable, enable, release with 3 position switch
AUXSW_MISSION_RESET = 24, // Reset auto mission to start from first command
AUXSW_ATTCON_FEEDFWD = 25, // enable/disable the roll and pitch rate feed forward
AUXSW_ATTCON_ACCEL_LIM = 26, // enable/disable the roll, pitch and yaw accel limiting
AUXSW_RETRACT_MOUNT = 27, // Retract Mount
AUXSW_RELAY = 28, // Relay pin on/off (only supports first relay)
AUXSW_LANDING_GEAR = 29, // Landing gear controller
AUXSW_LOST_COPTER_SOUND = 30, // Play lost copter sound
AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch
AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch
AUXSW_BRAKE = 33, // Brake flight mode
AUXSW_RELAY2 = 34, // Relay2 pin on/off (in Mission planner set CH8_OPT = 34)
AUXSW_RELAY3 = 35, // Relay3 pin on/off (in Mission planner set CH9_OPT = 35)
AUXSW_RELAY4 = 36, // Relay4 pin on/off (in Mission planner set CH10_OPT = 36)
AUXSW_THROW = 37, // change to THROW flight mode
AUXSW_AVOID_ADSB = 38, // enable AP_Avoidance library
AUXSW_PRECISION_LOITER = 39, // enable precision loiter
AUXSW_AVOID_PROXIMITY = 40, // enable object avoidance using proximity sensors (ie. horizontal lidar)
AUXSW_ARMDISARM = 41, // arm or disarm vehicle
AUXSW_SWITCH_MAX,
};
// Frame types
#define UNDEFINED_FRAME 0
#define MULTICOPTER_FRAME 1
#define HELI_FRAME 2
// HIL enumerations
#define HIL_MODE_DISABLED 0
#define HIL_MODE_SENSORS 1
// Auto Pilot Modes enumeration
enum control_mode_t {
STABILIZE = 0, // manual airframe angle with manual throttle
ACRO = 1, // manual body-frame angular rate with manual throttle
ALT_HOLD = 2, // manual airframe angle with automatic throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
LOITER = 5, // automatic horizontal acceleration with automatic throttle
RTL = 6, // automatic return to launching point
CIRCLE = 7, // automatic circular flight with automatic throttle
LAND = 9, // automatic landing with horizontal position control
DRIFT = 11, // semi-automous position, yaw and throttle control
SPORT = 13, // manual earth-frame angular rate control with manual throttle
FLIP = 14, // automatically flip the vehicle on the roll axis
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
BRAKE = 17, // full-brake using inertial/GPS system, no pilot input
THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude
};
enum mode_reason_t {
MODE_REASON_UNKNOWN=0,
MODE_REASON_TX_COMMAND,
MODE_REASON_GCS_COMMAND,
MODE_REASON_RADIO_FAILSAFE,
MODE_REASON_BATTERY_FAILSAFE,
MODE_REASON_GCS_FAILSAFE,
MODE_REASON_EKF_FAILSAFE,
MODE_REASON_GPS_GLITCH,
MODE_REASON_MISSION_END,
MODE_REASON_THROTTLE_LAND_ESCAPE,
MODE_REASON_FENCE_BREACH,
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_BRAKE_TIMEOUT,
MODE_REASON_FLIP_COMPLETE,
MODE_REASON_AVOIDANCE,
MODE_REASON_AVOIDANCE_RECOVERY,
MODE_REASON_THROW_COMPLETE,
};
// Tuning enumeration
enum tuning_func {
TUNING_NONE = 0, //
TUNING_STABILIZE_ROLL_PITCH_KP = 1, // stabilize roll/pitch angle controller's P term
TUNING_STABILIZE_YAW_KP = 3, // stabilize yaw heading controller's P term
TUNING_RATE_ROLL_PITCH_KP = 4, // body frame roll/pitch rate controller's P term
TUNING_RATE_ROLL_PITCH_KI = 5, // body frame roll/pitch rate controller's I term
TUNING_YAW_RATE_KP = 6, // body frame yaw rate controller's P term
TUNING_THROTTLE_RATE_KP = 7, // throttle rate controller's P term (desired rate to acceleration or motor output)
TUNING_WP_SPEED = 10, // maximum speed to next way point (0 to 10m/s)
TUNING_LOITER_POSITION_KP = 12, // loiter distance controller's P term (position error to speed)
TUNING_HELI_EXTERNAL_GYRO = 13, // TradHeli specific external tail gyro gain
TUNING_ALTITUDE_HOLD_KP = 14, // altitude hold controller's P term (alt error to desired rate)
TUNING_RATE_ROLL_PITCH_KD = 21, // body frame roll/pitch rate controller's D term
TUNING_VEL_XY_KP = 22, // loiter rate controller's P term (speed error to tilt angle)
TUNING_ACRO_RP_KP = 25, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_YAW_RATE_KD = 26, // body frame yaw rate controller's D term
TUNING_VEL_XY_KI = 28, // loiter rate controller's I term (speed error to tilt angle)
TUNING_AHRS_YAW_KP = 30, // ahrs's compass effect on yaw angle (0 = very low, 1 = very high)
TUNING_AHRS_KP = 31, // accelerometer effect on roll/pitch angle (0=low)
TUNING_ACCEL_Z_KP = 34, // accel based throttle controller's P term
TUNING_ACCEL_Z_KI = 35, // accel based throttle controller's I term
TUNING_ACCEL_Z_KD = 36, // accel based throttle controller's D term
TUNING_DECLINATION = 38, // compass declination in radians
TUNING_CIRCLE_RATE = 39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
TUNING_ACRO_YAW_KP = 40, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_RANGEFINDER_GAIN = 41, // rangefinder gain
TUNING_EKF_VERTICAL_POS = 42, // EKF's baro vs accel (higher rely on accels more, baro impact is reduced). Range should be 0.2 ~ 4.0? 2.0 is default
TUNING_EKF_HORIZONTAL_POS = 43, // EKF's gps vs accel (higher rely on accels more, gps impact is reduced). Range should be 1.0 ~ 3.0? 1.5 is default
TUNING_EKF_ACCEL_NOISE = 44, // EKF's accel noise (lower means trust accels more, gps & baro less). Range should be 0.02 ~ 0.5 0.5 is default (but very robust at that level)
TUNING_RC_FEEL_RP = 45, // roll-pitch input smoothing
TUNING_RATE_PITCH_KP = 46, // body frame pitch rate controller's P term
TUNING_RATE_PITCH_KI = 47, // body frame pitch rate controller's I term
TUNING_RATE_PITCH_KD = 48, // body frame pitch rate controller's D term
TUNING_RATE_ROLL_KP = 49, // body frame roll rate controller's P term
TUNING_RATE_ROLL_KI = 50, // body frame roll rate controller's I term
TUNING_RATE_ROLL_KD = 51, // body frame roll rate controller's D term
TUNING_RATE_PITCH_FF = 52, // body frame pitch rate controller FF term
TUNING_RATE_ROLL_FF = 53, // body frame roll rate controller FF term
TUNING_RATE_YAW_FF = 54, // body frame yaw rate controller FF term
TUNING_RATE_MOT_YAW_HEADROOM = 55, // motors yaw headroom minimum
TUNING_RATE_YAW_FILT = 56 // yaw rate input filter
};
// Acro Trainer types
#define ACRO_TRAINER_DISABLED 0
#define ACRO_TRAINER_LEVELING 1
#define ACRO_TRAINER_LIMITED 2
// RC Feel roll/pitch definitions
#define RC_FEEL_RP_VERY_SOFT 0
#define RC_FEEL_RP_SOFT 25
#define RC_FEEL_RP_MEDIUM 50
#define RC_FEEL_RP_CRISP 75
#define RC_FEEL_RP_VERY_CRISP 100
// Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter
#define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received)
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last
#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers)
// Auto modes
enum AutoMode {
Auto_TakeOff,
Auto_WP,
Auto_Land,
Auto_RTL,
Auto_CircleMoveToEdge,
Auto_Circle,
Auto_Spline,
Auto_NavGuided,
Auto_Loiter,
Auto_NavPayloadPlace,
};
// Guided modes
enum GuidedMode {
Guided_TakeOff,
Guided_WP,
Guided_Velocity,
Guided_PosVel,
Guided_Angle,
};
// RTL states
enum RTLState {
RTL_InitialClimb,
RTL_ReturnHome,
RTL_LoiterAtHome,
RTL_FinalDescent,
RTL_Land
};
// Alt_Hold states
enum AltHoldModeState {
AltHold_MotorStopped,
AltHold_Takeoff,
AltHold_Flying,
AltHold_Landed
};
// Loiter states
enum LoiterModeState {
Loiter_MotorStopped,
Loiter_Takeoff,
Loiter_Flying,
Loiter_Landed
};
// Sport states
enum SportModeState {
Sport_MotorStopped,
Sport_Takeoff,
Sport_Flying,
Sport_Landed
};
// Flip states
enum FlipState {
Flip_Start,
Flip_Roll,
Flip_Pitch_A,
Flip_Pitch_B,
Flip_Recover,
Flip_Abandon
};
// Throw stages
enum ThrowModeStage {
Throw_Disarmed,
Throw_Detecting,
Throw_Uprighting,
Throw_HgtStabilise,
Throw_PosHold
};
// Throw types
enum ThrowModeType {
ThrowType_Upward = 0,
ThrowType_Drop = 1
};
enum LandStateType {
LandStateType_FlyToLocation = 0,
LandStateType_Descending = 1
};
enum PayloadPlaceStateType {
PayloadPlaceStateType_FlyToLocation,
PayloadPlaceStateType_Calibrating_Hover_Start,
PayloadPlaceStateType_Calibrating_Hover,
PayloadPlaceStateType_Descending_Start,
PayloadPlaceStateType_Descending,
PayloadPlaceStateType_Releasing_Start,
PayloadPlaceStateType_Releasing,
PayloadPlaceStateType_Released,
PayloadPlaceStateType_Ascending_Start,
PayloadPlaceStateType_Ascending,
PayloadPlaceStateType_Done,
};
// bit options for DEV_OPTIONS parameter
enum DevOptions {
DevOptionADSBMAVLink = 1,
};
// Logging parameters
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
#define LOG_CONTROL_TUNING_MSG 0x04
#define LOG_NAV_TUNING_MSG 0x05
#define LOG_PERFORMANCE_MSG 0x06
#define LOG_OPTFLOW_MSG 0x0C
#define LOG_EVENT_MSG 0x0D
#define LOG_PID_MSG 0x0E // deprecated
#define LOG_INAV_MSG 0x11 // deprecated
#define LOG_CAMERA_MSG_DEPRECATED 0x12 // deprecated
#define LOG_ERROR_MSG 0x13
#define LOG_DATA_INT16_MSG 0x14
#define LOG_DATA_UINT16_MSG 0x15
#define LOG_DATA_INT32_MSG 0x16
#define LOG_DATA_UINT32_MSG 0x17
#define LOG_DATA_FLOAT_MSG 0x18
#define LOG_AUTOTUNE_MSG 0x19
#define LOG_AUTOTUNEDETAILS_MSG 0x1A
#define LOG_MOTBATT_MSG 0x1E
#define LOG_PARAMTUNE_MSG 0x1F
#define LOG_HELI_MSG 0x20
#define LOG_PRECLAND_MSG 0x21
#define LOG_GUIDEDTARGET_MSG 0x22
#define LOG_THROW_MSG 0x23
#define LOG_PROXIMITY_MSG 0x24
#define LOG_BEACON_MSG 0x25
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_GPS (1<<2)
#define MASK_LOG_PM (1<<3)
#define MASK_LOG_CTUN (1<<4)
#define MASK_LOG_NTUN (1<<5)
#define MASK_LOG_RCIN (1<<6)
#define MASK_LOG_IMU (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CURRENT (1<<9)
#define MASK_LOG_RCOUT (1<<10)
#define MASK_LOG_OPTFLOW (1<<11)
#define MASK_LOG_PID (1<<12)
#define MASK_LOG_COMPASS (1<<13)
#define MASK_LOG_INAV (1<<14) // deprecated
#define MASK_LOG_CAMERA (1<<15)
#define MASK_LOG_MOTBATT (1UL<<17)
#define MASK_LOG_IMU_FAST (1UL<<18)
#define MASK_LOG_IMU_RAW (1UL<<19)
#define MASK_LOG_ANY 0xFFFF
// DATA - event logging
#define DATA_AP_STATE 7
#define DATA_SYSTEM_TIME_SET 8
#define DATA_INIT_SIMPLE_BEARING 9
#define DATA_ARMED 10
#define DATA_DISARMED 11
#define DATA_AUTO_ARMED 15
#define DATA_LAND_COMPLETE_MAYBE 17
#define DATA_LAND_COMPLETE 18
#define DATA_NOT_LANDED 28
#define DATA_LOST_GPS 19
#define DATA_FLIP_START 21
#define DATA_FLIP_END 22
#define DATA_SET_HOME 25
#define DATA_SET_SIMPLE_ON 26
#define DATA_SET_SIMPLE_OFF 27
#define DATA_SET_SUPERSIMPLE_ON 29
#define DATA_AUTOTUNE_INITIALISED 30
#define DATA_AUTOTUNE_OFF 31
#define DATA_AUTOTUNE_RESTART 32
#define DATA_AUTOTUNE_SUCCESS 33
#define DATA_AUTOTUNE_FAILED 34
#define DATA_AUTOTUNE_REACHED_LIMIT 35
#define DATA_AUTOTUNE_PILOT_TESTING 36
#define DATA_AUTOTUNE_SAVEDGAINS 37
#define DATA_SAVE_TRIM 38
#define DATA_SAVEWP_ADD_WP 39
#define DATA_FENCE_ENABLE 41
#define DATA_FENCE_DISABLE 42
#define DATA_ACRO_TRAINER_DISABLED 43
#define DATA_ACRO_TRAINER_LEVELING 44
#define DATA_ACRO_TRAINER_LIMITED 45
#define DATA_GRIPPER_GRAB 46
#define DATA_GRIPPER_RELEASE 47
#define DATA_PARACHUTE_DISABLED 49
#define DATA_PARACHUTE_ENABLED 50
#define DATA_PARACHUTE_RELEASED 51
#define DATA_LANDING_GEAR_DEPLOYED 52
#define DATA_LANDING_GEAR_RETRACTED 53
#define DATA_MOTORS_EMERGENCY_STOPPED 54
#define DATA_MOTORS_EMERGENCY_STOP_CLEARED 55
#define DATA_MOTORS_INTERLOCK_DISABLED 56
#define DATA_MOTORS_INTERLOCK_ENABLED 57
#define DATA_ROTOR_RUNUP_COMPLETE 58 // Heli only
#define DATA_ROTOR_SPEED_BELOW_CRITICAL 59 // Heli only
#define DATA_EKF_ALT_RESET 60
#define DATA_LAND_CANCELLED_BY_PILOT 61
#define DATA_EKF_YAW_RESET 62
#define DATA_AVOIDANCE_ADSB_ENABLE 63
#define DATA_AVOIDANCE_ADSB_DISABLE 64
#define DATA_AVOIDANCE_PROXIMITY_ENABLE 65
#define DATA_AVOIDANCE_PROXIMITY_DISABLE 66
#define DATA_GPS_PRIMARY_CHANGED 67
// Centi-degrees to radians
#define DEGX100 5729.57795f
// Error message sub systems and error codes
#define ERROR_SUBSYSTEM_MAIN 1
#define ERROR_SUBSYSTEM_RADIO 2
#define ERROR_SUBSYSTEM_COMPASS 3
#define ERROR_SUBSYSTEM_OPTFLOW 4
#define ERROR_SUBSYSTEM_FAILSAFE_RADIO 5
#define ERROR_SUBSYSTEM_FAILSAFE_BATT 6
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7 // not used
#define ERROR_SUBSYSTEM_FAILSAFE_GCS 8
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLIGHT_MODE 10
#define ERROR_SUBSYSTEM_GPS 11 // not used
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_FLIP 13
#define ERROR_SUBSYSTEM_AUTOTUNE 14
#define ERROR_SUBSYSTEM_PARACHUTE 15
#define ERROR_SUBSYSTEM_EKFCHECK 16
#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17
#define ERROR_SUBSYSTEM_BARO 18
#define ERROR_SUBSYSTEM_CPU 19
#define ERROR_SUBSYSTEM_FAILSAFE_ADSB 20
#define ERROR_SUBSYSTEM_TERRAIN 21
#define ERROR_SUBSYSTEM_NAVIGATION 22
#define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN 23
#define ERROR_SUBSYSTEM_EKF_PRIMARY 24
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
#define ERROR_CODE_UNHEALTHY 4
// subsystem specific error codes -- radio
#define ERROR_CODE_RADIO_LATE_FRAME 2
// subsystem specific error codes -- failsafe_thr, batt, gps
#define ERROR_CODE_FAILSAFE_RESOLVED 0
#define ERROR_CODE_FAILSAFE_OCCURRED 1
// subsystem specific error codes -- compass
#define ERROR_CODE_COMPASS_FAILED_TO_READ 2
// subsystem specific error codes -- main
#define ERROR_CODE_MAIN_INS_DELAY 1
// subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1
#define ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL 2
// subsystem specific error codes -- flip
#define ERROR_CODE_FLIP_ABANDONED 2
// subsystem specific error codes -- terrain
#define ERROR_CODE_MISSING_TERRAIN_DATA 2
// subsystem specific error codes -- navigation
#define ERROR_CODE_FAILED_TO_SET_DESTINATION 2
#define ERROR_CODE_RESTARTED_RTL 3
#define ERROR_CODE_FAILED_CIRCLE_INIT 4
#define ERROR_CODE_DEST_OUTSIDE_FENCE 5
// parachute failed to deploy because of low altitude or landed
#define ERROR_CODE_PARACHUTE_TOO_LOW 2
#define ERROR_CODE_PARACHUTE_LANDED 3
// EKF check definitions
#define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2
#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0
// Baro specific error codes
#define ERROR_CODE_BARO_GLITCH 2
// Arming Check Enable/Disable bits
#define ARMING_CHECK_NONE 0x00
#define ARMING_CHECK_ALL 0x01
#define ARMING_CHECK_BARO 0x02
#define ARMING_CHECK_COMPASS 0x04
#define ARMING_CHECK_GPS 0x08
#define ARMING_CHECK_INS 0x10
#define ARMING_CHECK_PARAMETERS 0x20
#define ARMING_CHECK_RC 0x40
#define ARMING_CHECK_VOLTAGE 0x80
// Radio failsafe definitions (FS_THR parameter)
#define FS_THR_DISABLED 0
#define FS_THR_ENABLED_ALWAYS_RTL 1
#define FS_THR_ENABLED_CONTINUE_MISSION 2
#define FS_THR_ENABLED_ALWAYS_LAND 3
// Battery failsafe definitions (FS_BATT_ENABLE parameter)
#define FS_BATT_DISABLED 0 // battery failsafe disabled
#define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe
#define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe
// GCS failsafe definitions (FS_GCS_ENABLE parameter)
#define FS_GCS_DISABLED 0
#define FS_GCS_ENABLED_ALWAYS_RTL 1
#define FS_GCS_ENABLED_CONTINUE_MISSION 2
// EKF failsafe definitions (FS_EKF_ACTION parameter)
#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe
#define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe
#define FS_EKF_ACTION_LAND_EVEN_STABILIZE 3 // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize
// for mavlink SET_POSITION_TARGET messages
#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1) | (1<<2))
#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE ((1<<3) | (1<<4) | (1<<5))
#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE ((1<<6) | (1<<7) | (1<<8))
#define MAVLINK_SET_POS_TYPE_MASK_FORCE (1<<9)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<10)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11)
// for PILOT_THR_BHV parameter
#define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0)
#define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND (1<<1)
#define THR_BEHAVE_DISARM_ON_LAND_DETECT (1<<2)
