#elif HIL_MODE == HIL_MODE_ATTITUDE static AP_ADC_HIL adc; static AP_InertialSensor_HIL ins;
static AP_AHRS_HIL ahrs(&ins, g_gps); static AP_GPS_HIL g_gps_driver;
static AP_Compass_HIL compass; // never used static AP_Baro_HIL barometer;
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
// When building for SITL we use the HIL barometer and compass drivers static SITL sitl; #endif
#else
#error Unrecognised HIL_MODE setting. #endif // HIL MODE
//////////////////////////////////////////////////////////////////////////////// // Optical flow sensor
//////////////////////////////////////////////////////////////////////////////// #if OPTFLOW == ENABLED
static AP_OpticalFlow_ADNS3080 optflow; #else
static AP_OpticalFlow optflow; #endif
//////////////////////////////////////////////////////////////////////////////// // GCS selection
//////////////////////////////////////////////////////////////////////////////// static GCS_MAVLINK gcs0; static GCS_MAVLINK gcs3;
//////////////////////////////////////////////////////////////////////////////// // SONAR selection
//////////////////////////////////////////////////////////////////////////////// //
ModeFilterInt16_Size3 sonar_mode_filter(1); #if CONFIG_SONAR == ENABLED
static AP_HAL::AnalogSource *sonar_analog_source; static AP_RangeFinder_MaxsonarXL *sonar; #endif
//////////////////////////////////////////////////////////////////////////////// // User variables
//////////////////////////////////////////////////////////////////////////////// #ifdef USERHOOK_VARIABLES #include USERHOOK_VARIABLES #endif
//////////////////////////////////////////////////////////////////////////////// // Global variables
////////////////////////////////////////////////////////////////////////////////
/* Radio values
* Channel assignments * 1 Ailerons (rudder if no ailerons) * 2 Elevator * 3 Throttle * 4 Rudder (if we have ailerons) * 5 Mode - 3 position switch * 6 User assignable * 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold >
1 second)
* 8 TBD
* Each Aux channel can be configured to have any of the available auxiliary functions assigned to it.
* See libraries/RC_Channel/RC_Channel_aux.h for more information */
//Documentation of GLobals: static union { struct {
uint8_t home_is_set : 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 do_flip : 1; // 7 // Used to enable flip code uint8_t takeoff_complete : 1; // 8
uint8_t land_complete : 1; // 9 // true if we have detected a landing
uint8_t new_radio_frame : 1; // 10 // Set true if we have new PWM data to act on from the Radio uint8_t CH7_flag : 2; // 11,12 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t CH8_flag : 2; // 13,14 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high
uint8_t usb_connected : 1; // 15 // true if APM is powered from USB connection uint8_t yaw_stopped : 1; // 16 // Used to manage the Yaw hold capabilities
uint8_t disable_stab_rate_limit : 1; // 17 // disables limits rate request from the stability controller
uint8_t rc_receiver_present : 1; // 18 // true if we have an rc receiver present (i.e. if we've ever received an update };
uint32_t value; } ap;
//////////////////////////////////////////////////////////////////////////////// // Radio
//////////////////////////////////////////////////////////////////////////////// // This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO, static int8_t control_mode = STABILIZE;
// Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch static uint8_t oldSwitchPosition; static RCMapper rcmap;
// receiver RSSI
static uint8_t receiver_rssi;
//////////////////////////////////////////////////////////////////////////////// // Failsafe
//////////////////////////////////////////////////////////////////////////////// static 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 gps : 1; // 3 // A status flag for the gps failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
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 } failsafe;
//////////////////////////////////////////////////////////////////////////////// // Motor Output
//////////////////////////////////////////////////////////////////////////////// #if FRAME_CONFIG == QUAD_FRAME #define MOTOR_CLASS AP_MotorsQuad #elif FRAME_CONFIG == TRI_FRAME #define MOTOR_CLASS AP_MotorsTri #elif FRAME_CONFIG == HEXA_FRAME #define MOTOR_CLASS AP_MotorsHexa #elif FRAME_CONFIG == Y6_FRAME #define MOTOR_CLASS AP_MotorsY6 #elif FRAME_CONFIG == OCTA_FRAME #define MOTOR_CLASS AP_MotorsOcta #elif FRAME_CONFIG == OCTA_QUAD_FRAME #define MOTOR_CLASS AP_MotorsOctaQuad #elif FRAME_CONFIG == HELI_FRAME #define MOTOR_CLASS AP_MotorsHeli #else
#error Unrecognised frame type #endif
#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); #elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7); #else
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); #endif
//////////////////////////////////////////////////////////////////////////////// // PIDs
////////////////////////////////////////////////////////////////////////////////
// This is a convienience accessor for the IMU roll rates. It's currently the raw IMU rates // and not the adjusted omega rates, but the name is stuck static Vector3f omega;
// This is used to hold radio tuning values for in-flight CH6 tuning float tuning_value;
// used to limit the rate that the pid controller output is logged so that it doesn't negatively affect performance static uint8_t pid_log_counter;
//////////////////////////////////////////////////////////////////////////////// // LED output
//////////////////////////////////////////////////////////////////////////////// // Blinking indicates GPS status
static uint8_t copter_leds_GPS_blink; // Blinking indicates battery status static uint8_t copter_leds_motor_blink; // Navigation confirmation blinks static int8_t copter_leds_nav_blink;
//////////////////////////////////////////////////////////////////////////////// // GPS variables
////////////////////////////////////////////////////////////////////////////////
// This is used to scale GPS values for EEPROM storage // 10^7 times Decimal GPS means 1 == 1cm
// This approximation makes calculations integer and it's easy to read static const float t7 = 10000000.0;
// We use atan2 and other trig techniques to calaculate angles // We need to scale the longitude up to make these calcs work
// to account for decreasing distance between lines of longitude away from the equator static float scaleLongUp = 1;
// Sometimes we need to remove the scaling for distance calcs static float scaleLongDown = 1;
//////////////////////////////////////////////////////////////////////////////// // Location & Navigation
//////////////////////////////////////////////////////////////////////////////// // This is the angle from the copter to the next waypoint in centi-degrees static int32_t wp_bearing;
// The original bearing to the next waypoint. used to point the nose of the copter at the next waypoint static int32_t original_wp_bearing;
// The location of home in relation to the copter in centi-degrees static int32_t home_bearing;
// distance between plane and home in cm static int32_t home_distance;
// distance between plane and next waypoint in cm. static uint32_t wp_distance;
// navigation mode - options include NAV_NONE, NAV_LOITER, NAV_CIRCLE, NAV_WP static uint8_t nav_mode;
// Register containing the index of the current navigation command in the mission script static int16_t command_nav_index;
// Register containing the index of the previous navigation command in the mission script // Used to manage the execution of conditional commands static uint8_t prev_nav_index;
// Register containing the index of the current conditional command in the mission script static uint8_t command_cond_index;
// Used to track the required WP navigation information // options include
// NAV_ALTITUDE - have we reached the desired altitude? // NAV_LOCATION - have we reached the desired location?
// NAV_DELAY - have we waited at the waypoint the desired time?
static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter target position
static int16_t control_roll; static int16_t control_pitch;
static uint8_t rtl_state; // records state of rtl (initial climb, returning home, etc) static uint8_t land_state; // records state of land (flying to location, descending)
//////////////////////////////////////////////////////////////////////////////// // Orientation
////////////////////////////////////////////////////////////////////////////////
// Convienience accessors for commonly used trig functions. These values are generated
// by the DCM through a few simple equations. They are used throughout the code where cos and sin // would normally be used.
// The cos values are defaulted to 1 to get a decent initial value for a level state static float cos_roll_x = 1.0; static float cos_pitch_x = 1.0; static float cos_yaw = 1.0; static float sin_yaw; static float sin_roll; static float sin_pitch;
//////////////////////////////////////////////////////////////////////////////// // 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. static float simple_cos_yaw = 1.0; static float simple_sin_yaw;
static int32_t super_simple_last_bearing; static float super_simple_cos_yaw = 1.0; static float super_simple_sin_yaw;
// Stores initial bearing when armed - initial simple bearing is modified in super simple mode so not suitable static int32_t initial_armed_bearing;
//////////////////////////////////////////////////////////////////////////////// // Rate contoller targets
////////////////////////////////////////////////////////////////////////////////
static uint8_t rate_targets_frame = EARTH_FRAME; // indicates whether rate targets provided in earth or body frame
static int32_t roll_rate_target_ef; static int32_t pitch_rate_target_ef; static int32_t yaw_rate_target_ef;
static int32_t roll_rate_target_bf; // body frame roll rate target static int32_t pitch_rate_target_bf; // body frame pitch rate target static int32_t yaw_rate_target_bf; // body frame yaw rate target
//////////////////////////////////////////////////////////////////////////////// // Throttle variables
////////////////////////////////////////////////////////////////////////////////
static int16_t throttle_accel_target_ef; // earth frame throttle acceleration target
static bool throttle_accel_controller_active; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directly static float throttle_avg; // g.throttle_cruise as a float
static int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only
static float target_alt_for_reporting; // target altitude in cm for reporting (logs and ground station)
//////////////////////////////////////////////////////////////////////////////// // ACRO Mode
//////////////////////////////////////////////////////////////////////////////// // Used to control Axis lock
static int32_t acro_roll; // desired roll angle while sport mode static int32_t acro_roll_rate; // desired roll rate while in acro mode static int32_t acro_pitch; // desired pitch angle while sport mode static int32_t acro_pitch_rate; // desired pitch rate while acro mode static int32_t acro_yaw_rate; // desired yaw rate while acro mode
static float acro_level_mix; // scales back roll, pitch and yaw inversely proportional to input from pilot
// Filters
#if FRAME_CONFIG == HELI_FRAME
//static LowPassFilterFloat rate_roll_filter; // Rate Roll filter //static LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter #endif // HELI_FRAME
//////////////////////////////////////////////////////////////////////////////// // Circle Mode / Loiter control
////////////////////////////////////////////////////////////////////////////////
Vector3f circle_center; // circle position expressed in cm from home location. x = lat, y = lon // angle from the circle center to the copter's desired location. Incremented at circle_rate / second static float circle_angle;
// the total angle (in radians) travelled static float circle_angle_total;