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