// 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;

// deg : how many times to circle as specified by mission command static uint8_t circle_desired_rotations;

static float circle_angular_acceleration; // circle mode's angular acceleration static float circle_angular_velocity; // circle mode's angular velocity static float circle_angular_velocity_max; // circle mode's max angular velocity // How long we should stay in Loiter Mode for mission scripting (time in seconds) static uint16_t loiter_time_max;

// How long have we been loitering - The start time in millis static uint32_t loiter_time;

//////////////////////////////////////////////////////////////////////////////// // CH7 and CH8 save waypoint control

//////////////////////////////////////////////////////////////////////////////// // This register tracks the current Mission Command index when writing // a mission using Ch7 or Ch8 aux switches in flight static int8_t aux_switch_wp_index;

//////////////////////////////////////////////////////////////////////////////// // Battery Sensors

//////////////////////////////////////////////////////////////////////////////// static AP_BattMonitor battery;

//////////////////////////////////////////////////////////////////////////////// // Altitude

//////////////////////////////////////////////////////////////////////////////// // The (throttle) controller desired altitude in cm static float controller_desired_alt;

// The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP static int32_t altitude_error;

// The cm/s we are moving up or down based on filtered data - Positive = UP static int16_t climb_rate;

// The altitude as reported by Sonar in cm – Values are 20 to 700 generally. static int16_t sonar_alt;

static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar static float target_sonar_alt; // desired altitude in cm above the ground // The altitude as reported by Baro in cm – Values can be quite high static int32_t baro_alt;

static int16_t saved_toy_throttle;

//////////////////////////////////////////////////////////////////////////////// // flight modes

//////////////////////////////////////////////////////////////////////////////// // Flight modes are combinations of Roll/Pitch, Yaw and Throttle control modes // Each Flight mode is a unique combination of these modes //

// The current desired control scheme for Yaw static uint8_t yaw_mode;

// The current desired control scheme for roll and pitch / navigation static uint8_t roll_pitch_mode;

// The current desired control scheme for altitude hold static uint8_t throttle_mode;

//////////////////////////////////////////////////////////////////////////////// // flight specific

//////////////////////////////////////////////////////////////////////////////// // An additional throttle added to keep the copter at the same altitude when banking

static int16_t angle_boost; // counter to verify landings static uint16_t land_detector;

//////////////////////////////////////////////////////////////////////////////// // 3D Location vectors

//////////////////////////////////////////////////////////////////////////////// // home location is stored when we have a good GPS lock and arm the copter // Can be reset each the copter is re-armed static struct Location home; // Current location of the copter static struct Location current_loc;

// Holds the current loaded command from the EEPROM for navigation static struct Location command_nav_queue;

// Holds the current loaded command from the EEPROM for conditional scripts static struct Location command_cond_queue;

//////////////////////////////////////////////////////////////////////////////// // Navigation Roll/Pitch functions

//////////////////////////////////////////////////////////////////////////////// // all angles are deg * 100 : target yaw angle // The Commanded ROll from the autopilot. static int32_t nav_roll;

// The Commanded pitch from the autopilot. negative Pitch means go forward. static int32_t nav_pitch;

// The Commanded ROll from the autopilot based on optical flow sensor. static int32_t of_roll;

// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward. static int32_t of_pitch;

//////////////////////////////////////////////////////////////////////////////// // Navigation Throttle control

//////////////////////////////////////////////////////////////////////////////// // The Commanded Throttle from the autopilot.

static int16_t nav_throttle; // 0-1000 for throttle control

// This is a simple counter to track the amount of throttle used during flight

// This could be useful later in determining and debuging current usage and predicting battery life static uint32_t throttle_integrator;

//////////////////////////////////////////////////////////////////////////////// // Navigation Yaw control

//////////////////////////////////////////////////////////////////////////////// // The Commanded Yaw from the autopilot. static int32_t nav_yaw; static uint8_t yaw_timer;

// Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION static Vector3f yaw_look_at_WP;

// bearing from current location to the yaw_look_at_WP static int32_t yaw_look_at_WP_bearing;

// yaw used for YAW_LOOK_AT_HEADING yaw_mode static int32_t yaw_look_at_heading; // Deg/s we should turn

static int16_t yaw_look_at_heading_slew;

////////////////////////////////////////////////////////////////////////////////