Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cleaned up and documented each global vavriable 

Added Wind compensation for Stability
Acro mode fixes
This commit is contained in:
Jason Short 2012-01-03 22:54:29 -08:00
parent 123ce533bb
commit a70fdc58a1
1 changed files with 481 additions and 248 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

729
ArduCopter/ArduCopter.pde
View File

@ -304,291 +304,508 @@ static const char* flight_mode_strings[] = {
8 TBD 8 TBD
*/ */
// temp //Documentation of GLobals:
////////////////////////////////////////////////////////////////////////////////
// The GPS based velocity calculated by offsetting the Latitude and Longitude
// updated after GPS read - 5-10hz
static int16_t x_GPS_speed; static int16_t x_GPS_speed;
static int16_t y_GPS_speed; static int16_t y_GPS_speed;
// The synthesized velocity calculated by fancy filtering and fusion
// updated at 50hz
static int16_t x_actual_speed; static int16_t x_actual_speed;
static int16_t y_actual_speed; static int16_t y_actual_speed;
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t x_rate_error; static int16_t x_rate_error;
static int16_t y_rate_error; static int16_t y_rate_error;
////////////////////////////////////////////////////////////////////////////////
// Radio // Radio
// ----- ////////////////////////////////////////////////////////////////////////////////
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
static byte control_mode = STABILIZE; static byte control_mode = STABILIZE;
static byte old_control_mode = STABILIZE; // This is the state of simple mode.
static byte oldSwitchPosition; // for remembering the control mode switch // Set in the control_mode.pde file when the control switch is read
static int16_t motor_out[11];
static int16_t motor_filtered[11]; // added to try and deal with biger motors
static bool do_simple = false; static bool do_simple = false;
// 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 byte oldSwitchPosition;
// This is used to look for change in the control switch
static byte old_control_mode = STABILIZE;
////////////////////////////////////////////////////////////////////////////////
// Motor Output
////////////////////////////////////////////////////////////////////////////////
// This is the array of PWM values being sent to the motors
static int16_t motor_out[11];
// This is the array of PWM values being sent to the motors that has been lightly filtered with a simple LPF
// This was added to try and deal with biger motors
static int16_t motor_filtered[11];
////////////////////////////////////////////////////////////////////////////////
// Mavlink/HIL control
////////////////////////////////////////////////////////////////////////////////
// Used to track the GCS based control input
// Allow override of RC channel values for HIL
static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; static int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
// Status flag that tracks whether we are under GCS control
static bool rc_override_active = false; static bool rc_override_active = false;
// Status flag that tracks whether we are under GCS control
static uint32_t rc_override_fs_timer = 0; static uint32_t rc_override_fs_timer = 0;
////////////////////////////////////////////////////////////////////////////////
// Heli // Heli
// ---- ////////////////////////////////////////////////////////////////////////////////
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
static float heli_rollFactor[3], heli_pitchFactor[3]; // only required for 3 swashplate servos static float heli_rollFactor[3], heli_pitchFactor[3]; // only required for 3 swashplate servos
static int16_t heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly static int16_t heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly
static int32_t heli_servo_out[4]; // used for servo averaging for analog servos static int32_t heli_servo_out[4]; // used for servo averaging for analog servos
static int16_t heli_servo_out_count = 0; // use for servo averaging static int16_t heli_servo_out_count; // use for servo averaging
#endif #endif
////////////////////////////////////////////////////////////////////////////////
// Failsafe // Failsafe
// -------- ////////////////////////////////////////////////////////////////////////////////
static boolean failsafe; // did our throttle dip below the failsafe value? // A status flag for the failsafe state
static boolean ch3_failsafe; // did our throttle dip below the failsafe value?
static boolean motor_armed; static boolean failsafe;
static boolean motor_auto_armed; // if true, // A status flag for arming the motors. This is the arming that is performed when the
// Yaw control is held right or left while throttle is low.
static boolean motor_armed;
// A status flag for whether or not we should allow AP to take over copter
// This is tied to the throttle. If the throttle = 0 or low/nuetral, then we do not allow
// the APM to take control of the copter.
static boolean motor_auto_armed;
////////////////////////////////////////////////////////////////////////////////
// PIDs // 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; static Vector3f omega;
// This is used to hold radio tuning values for in-flight CH6 tuning
float tuning_value; float tuning_value;
////////////////////////////////////////////////////////////////////////////////
// LED output // LED output
// ---------- ////////////////////////////////////////////////////////////////////////////////
static boolean motor_light; // status of the Motor safety // status of LED based on the motor_armed variable
static boolean GPS_light; // status of the GPS light // Flashing indicates we are not armed
// Solid indicates Armed state
static boolean motor_light;
// Flashing indicates we are reading the GPS Strings
// Solid indicates we have full 3D lock and can navigate
static boolean GPS_light;
// This is current status for the LED lights state machine
// setting this value changes the output of the LEDs
static byte led_mode = NORMAL_LEDS; static byte led_mode = NORMAL_LEDS;
////////////////////////////////////////////////////////////////////////////////
// GPS variables // GPS variables
// ------------- ////////////////////////////////////////////////////////////////////////////////
static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage // This is used to scale GPS values for EEPROM storage
static float scaleLongUp = 1; // used to reverse longitude scaling // 10^7 times Decimal GPS means 1 == 1cm
static float scaleLongDown = 1; // used to reverse longitude scaling // This approximation makes calculations integer and it's easy to read
static byte ground_start_count = 10; // have we achieved first lock and set Home? static const float t7 = 10000000.0;
static bool did_ground_start = false; // have we ground started after first arming // We use atan2 and other trig techniques to calaculate angles
// We need to scale the longitude up to make these calcs work
static float scaleLongUp = 1;
// Sometimes we need to remove the scaling for distance calcs
static float scaleLongDown = 1;
// Location & Navigation
// --------------------- ////////////////////////////////////////////////////////////////////////////////
static bool nav_ok; // Mavlink specific
////////////////////////////////////////////////////////////////////////////////
// Used by Mavlink for unknow reasons
static const float radius_of_earth = 6378100; // meters static const float radius_of_earth = 6378100; // meters
// Used by Mavlink for unknow reasons
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.81; // meters/ sec^2
static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int32_t nav_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int32_t home_bearing; // used to track difference in angle
static byte wp_control; // used to control - navgation or loiter
static byte command_nav_index; // current command memory location ////////////////////////////////////////////////////////////////////////////////
static byte prev_nav_index; // Location & Navigation
static byte command_cond_index; // current command memory location ////////////////////////////////////////////////////////////////////////////////
//static byte command_nav_ID; // current command ID // Status flag indicating we have data that can be used to navigate
//static byte command_cond_ID; // current command ID // Set by a GPS read with 3D fix, or an optical flow read
static byte wp_verify_byte; // used for tracking state of navigating waypoints static bool nav_ok;
// This is the angle from the copter to the "next_WP" location in degrees * 100
static int32_t target_bearing;
// This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing;
// This is the angle from the copter to the "home" location in degrees * 100
static int32_t home_bearing;
// Status of the Waypoint tracking mode. Options include:
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
static byte wp_control;
// Register containing the index of the current navigation command in the mission script
static uint8_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 uint8_t wp_verify_byte; // used for tracking state of navigating waypoints
// used to limit the speed ramp up of WP navigation
// Acceleration is limited to .5m/s/s
static int16_t waypoint_speed_gov;
// Used to track how many cm we are from the "next_WP" location
static int32_t long_error, lat_error;
////////////////////////////////////////////////////////////////////////////////
// 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; static float cos_roll_x = 1;
static float cos_pitch_x = 1; static float cos_pitch_x = 1;
static float cos_yaw_x = 1; static float cos_yaw_x = 1;
static float sin_pitch_y, sin_yaw_y, sin_roll_y; static float sin_pitch_y, sin_yaw_y, sin_roll_y;
static int32_t initial_simple_bearing; // used for Simple mode
static float simple_sin_y, simple_cos_x;
static int8_t jump = -10; // used to track loops in jump command
static int16_t waypoint_speed_gov;
////////////////////////////////////////////////////////////////////////////////
// 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 int32_t initial_simple_bearing;
////////////////////////////////////////////////////////////////////////////////
// Circle Mode / Loiter control
////////////////////////////////////////////////////////////////////////////////
// used to determin the desired location in Circle mode
// increments at circle_rate / second
static float circle_angle; static float circle_angle;
// replace with param // used to control the speed of Circle mode
// units are in radians, default is 5° per second
static const float circle_rate = 0.0872664625; static const float circle_rate = 0.0872664625;
// used to track the delat in Circle Mode
static int32_t old_target_bearing;
// deg : how many times to circle * 360 for Loiter/Circle Mission command
static int16_t loiter_total;
// deg : how far we have turned around a waypoint
static int16_t loiter_sum;
// How long we should stay in Loiter Mode for mission scripting
static uint16_t loiter_time_max;
// How long have we been loitering - The start time in millis
static uint32_t loiter_time;
// The synthetic location created to make the copter do circles around a WP
static struct Location circle_WP;
// Acro
////////////////////////////////////////////////////////////////////////////////
// CH7 control
////////////////////////////////////////////////////////////////////////////////
// Used to enable Jose's flip code
// when true the Roll/Pitch/Throttle control is sent to the flip state machine
#if CH7_OPTION == CH7_FLIP #if CH7_OPTION == CH7_FLIP
static bool do_flip = false; static bool do_flip = false;
#endif #endif
// Used to track the CH7 toggle state.
// When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
// This allows advanced functionality to know when to execute
static boolean trim_flag; static boolean trim_flag;
// This register tracks the current Mission Command index when writing
// a mission using CH7 in flight
static int8_t CH7_wp_index; static int8_t CH7_wp_index;
// Airspeed
// --------
static int16_t airspeed; // m/s * 100
static float thrust = .005; // for estimating the velocity
// Location Errors
// ---------------
static int32_t long_error, lat_error; // temp for debugging
////////////////////////////////////////////////////////////////////////////////
// Battery Sensors // Battery Sensors
// --------------- ////////////////////////////////////////////////////////////////////////////////
static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter // Battery Voltage of total battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter static float battery_voltage = LOW_VOLTAGE * 1.05;
static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter // Battery Voltage of cell 1, initialized above threshold for filter
static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter static float battery_voltage1 = LOW_VOLTAGE * 1.05;
static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter // Battery Voltage of cells 1 + 2, initialized above threshold for filter
static float battery_voltage2 = LOW_VOLTAGE * 1.05;
// Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
static float battery_voltage3 = LOW_VOLTAGE * 1.05;
// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
static float battery_voltage4 = LOW_VOLTAGE * 1.05;
// refers to the instant amp draw – based on an Attopilot Current sensor
static float current_amps; static float current_amps;
// refers to the total amps drawn – based on an Attopilot Current sensor
static float current_total; static float current_total;
// Used to track if the battery is low - LED output flashes when the batt is low
static bool low_batt = false; static bool low_batt = false;
// Barometer Sensor variables
// -------------------------- ////////////////////////////////////////////////////////////////////////////////
static int32_t abs_pressure; // Altitude
////////////////////////////////////////////////////////////////////////////////
// The pressure at home location - calibrated at arming
static int32_t ground_pressure; static int32_t ground_pressure;
static int16_t ground_temperature; // The ground temperature at home location - calibrated at arming
static int16_t ground_temperature;
// Altitude Sensor variables // The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP
// ---------------------- static int32_t altitude_error;
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR // The cm/s we are moving up or down - Positive = UP
static int32_t altitude_error; // meters * 100 we are off in altitude static int16_t climb_rate;
// The altitude as reported by Sonar in cm – Values are 20 to 700 generally.
static int16_t climb_rate; // m/s * 100
static int16_t sonar_alt; static int16_t sonar_alt;
// The previous altitude as reported by Sonar in cm for calculation of Climb Rate
static int16_t old_sonar_alt; static int16_t old_sonar_alt;
// The climb_rate as reported by sonar in cm/s
static int16_t sonar_rate; static int16_t sonar_rate;
// The altitude as reported by Baro in cm – Values can be quite high
static int32_t baro_alt; static int32_t baro_alt;
// The previous altitude as reported by Baro in cm for calculation of Climb Rate
static int32_t old_baro_alt; static int32_t old_baro_alt;
// The climb_rate as reported by Baro in cm/s
static int16_t baro_rate; static int16_t baro_rate;
////////////////////////////////////////////////////////////////////////////////
// flight mode specific // 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 byte yaw_mode; static byte yaw_mode;
// The current desired control scheme for roll and pitch / navigation
static byte roll_pitch_mode; static byte roll_pitch_mode;
// The current desired control scheme for altitude hold
static byte throttle_mode; static byte throttle_mode;
static boolean takeoff_complete; // Flag for using take-off controls
static int32_t takeoff_timer; // time since we throttled up
static boolean land_complete;
static int32_t old_alt; // used for managing altitude rates
static int16_t velocity_land;
static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
static int16_t manual_boost; // used in adjust altitude to make changing alt faster
static int16_t angle_boost; // used in adjust altitude to make changing alt faster
// Loiter management ////////////////////////////////////////////////////////////////////////////////
// ----------------- // flight specific
static int32_t original_target_bearing; // deg * 100, used to check we are not passing the WP ////////////////////////////////////////////////////////////////////////////////
static int32_t old_target_bearing; // used to track difference in angle // Flag for monitoring the status of flight
// We must be in the air with throttle for 5 seconds before this flag is true
// This flag is reset when we are in a manual throttle mode with 0 throttle or disarmed
static boolean takeoff_complete;
// Used to record the most recent time since we enaged the throttle to take off
static int32_t takeoff_timer;
// Used to see if we have landed and if we should shut our engines - not fully implemented
static boolean land_complete = true;
static int16_t loiter_total; // deg : how many times to loiter * 360 // used to manually override throttle in interactive Alt hold modes
static int16_t loiter_sum; // deg : how far we have turned around a waypoint static int16_t manual_boost;
static uint32_t loiter_time; // millis : when we started LOITER mode // An additional throttle added to keep the copter at the same altitude when banking
static unsigned loiter_time_max; // millis : how long to stay in LOITER mode static int16_t angle_boost;
// these are the values for navigation control functions ////////////////////////////////////////////////////////////////////////////////
// ---------------------------------------------------- // Navigation general
static int32_t nav_roll; // deg * 100 : target roll angle ////////////////////////////////////////////////////////////////////////////////
static int32_t nav_pitch; // deg * 100 : target pitch angle // The location of the copter in relation to home, updated every GPS read
static int32_t nav_yaw; // deg * 100 : target yaw angle static int32_t home_to_copter_bearing;
static int32_t home_to_copter_bearing; // deg * 100 : target yaw angle // distance between plane and home in meters (not cm!!!)
static int32_t auto_yaw; // deg * 100 : target yaw angle static int32_t home_distance;
// distance between plane and next_WP in meters (not cm!!!)
static int32_t wp_distance;
static int16_t nav_lat; // for error calcs ////////////////////////////////////////////////////////////////////////////////
static int16_t nav_lon; // for error calcs // 3D Location vectors
static int16_t nav_lat_p; // for error calcs ////////////////////////////////////////////////////////////////////////////////
static int16_t nav_lon_p; // for error calcs // 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;
// Flag for if we have g_gps lock and have set the home location
static boolean home_is_set;
// Current location of the copter
static struct Location current_loc;
// Next WP is the desired location of the copter - the next waypoint or loiter location
static struct Location next_WP;
// Prev WP is used to get the optimum path from one WP to the next
static struct Location prev_WP;
// 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;
// Holds the current loaded command from the EEPROM for guided mode
static struct Location guided_WP;
static int16_t nav_throttle; // 0-1000 for throttle control ////////////////////////////////////////////////////////////////////////////////
// Crosstrack
////////////////////////////////////////////////////////////////////////////////
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error; static int16_t crosstrack_error;
static uint32_t throttle_integrator; // used to integrate throttle output to predict battery life
static float throttle_avg = THROTTLE_CRUISE;
static bool invalid_throttle; // used to control when we calculate nav_throttle
//static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
static int32_t command_yaw_start; // what angle were we to begin with ////////////////////////////////////////////////////////////////////////////////
static uint32_t command_yaw_start_time; // when did we start turning // 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 desired bank towards North (Positive) or South (Negative)
// Don't be fooled by the fact that Pitch is reversed from Roll in its sign!
static int16_t nav_lat;
// The desired bank towards East (Positive) or West (Negative)
static int16_t nav_lon;
// This may go away, but for now I'm tracking the desired bank before we apply the Wind compensation I term
// This is mainly for debugging
static int16_t nav_lat_p;
static int16_t nav_lon_p;
////////////////////////////////////////////////////////////////////////////////
// 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;
// This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control
// that is generated when the climb rate is within a certain threshold
static float throttle_avg = THROTTLE_CRUISE;
// This is a flag used to trigger the updating of nav_throttle at 10hz
static bool invalid_throttle;
// Used to track the altitude offset for climbrate control
static int32_t target_altitude;
////////////////////////////////////////////////////////////////////////////////
// Navigation Yaw control
////////////////////////////////////////////////////////////////////////////////
// The Commanded Yaw from the autopilot.
static int32_t nav_yaw;
// A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot
static int32_t auto_yaw;
// Used to manage the Yaw hold capabilities -
// Options include: no tracking, point at next wp, or at a target
static byte yaw_tracking = MAV_ROI_WPNEXT;
// In AP Mission scripting we have a fine level of control for Yaw
// This is our initial angle for relative Yaw movements
static int32_t command_yaw_start;
// Timer values used to control the speed of Yaw movements
static uint32_t command_yaw_start_time;
static uint16_t command_yaw_time; // how long we are turning static uint16_t command_yaw_time; // how long we are turning
static int32_t command_yaw_end; // what angle are we trying to be static int32_t command_yaw_end; // what angle are we trying to be
static int32_t command_yaw_delta; // how many degrees will we turn // how many degrees will we turn
static int16_t command_yaw_speed; // how fast to turn static int32_t command_yaw_delta;
// Deg/s we should turn
static int16_t command_yaw_speed;
// Direction we will turn – 1 = CW, 0 or -1 = CCW
static byte command_yaw_dir; static byte command_yaw_dir;
// Direction we will turn – 1 = relative, 0 = Absolute
static byte command_yaw_relative; static byte command_yaw_relative;
// Yaw will point at this location if yaw_tracking is set to MAV_ROI_LOCATION
static struct Location target_WP;
static int16_t auto_level_counter;
// Waypoints
// ---------
static int32_t home_distance; // meters - distance between plane and next waypoint
static int32_t wp_distance; // meters - distance between plane and next waypoint
static int32_t wp_totalDistance; // meters - distance between old and next waypoint
//static byte next_wp_index; // Current active command index
// repeating event control ////////////////////////////////////////////////////////////////////////////////
// ----------------------- // Repeat Mission Scripting Command
static byte event_id; // what to do - see defines ////////////////////////////////////////////////////////////////////////////////
static uint32_t event_timer; // when the event was asked for in ms // The type of repeating event - Toggle a servo channel, Toggle the APM1 relay, etc
static uint16_t event_delay; // how long to delay the next firing of event in millis static byte event_id;
static int16_t event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice // Used to manage the timimng of repeating events
static int16_t event_value; // per command value, such as PWM for servos static uint32_t event_timer;
static int16_t event_undo_value; // the value used to undo commands // How long to delay the next firing of event in millis
//static byte repeat_forever; static uint16_t event_delay;
//static byte undo_event; // counter for timing the undo // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
static int16_t event_repeat;
// per command value, such as PWM for servos
static int16_t event_value;
// the stored value used to undo commands - such as original PWM command
static int16_t event_undo_value;
// delay command ////////////////////////////////////////////////////////////////////////////////
// -------------- // Delay Mission Scripting Command
////////////////////////////////////////////////////////////////////////////////
static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
static int32_t condition_start; static int32_t condition_start;
// land command
// ------------
static int32_t land_start; // when we intiated command in millis()
static int32_t original_alt; // altitide reference for start of command
// 3D Location vectors ////////////////////////////////////////////////////////////////////////////////
// ------------------- // Auto Landing
static struct Location home; // home location ////////////////////////////////////////////////////////////////////////////////
static struct Location prev_WP; // last waypoint // Time when we intiated command in millis - used for controlling decent rate
static struct Location current_loc; // current location static int32_t land_start;
static struct Location next_WP; // next waypoint // The orginal altitude used to base our new altitude during decent
static struct Location target_WP; // where do we want to you towards? static int32_t original_alt;
static struct Location command_nav_queue; // command preloaded
static struct Location command_cond_queue; // command preloaded
static struct Location guided_WP; // guided mode waypoint
static int32_t target_altitude; // used for
static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location
////////////////////////////////////////////////////////////////////////////////
// IMU variables
////////////////////////////////////////////////////////////////////////////////
// Integration time for the gyros (DCM algorithm)
// Updated with th efast loop
static float G_Dt = 0.02;
// The rotated accelerometer values
// Used by Z accel control, updated at 10hz
Vector3f accels_rot; Vector3f accels_rot;
// this is just me playing with the sensors ////////////////////////////////////////////////////////////////////////////////
// the 2 code is not functioning and you should try 1 instead
#if ACCEL_ALT_HOLD == 2
static float Z_integrator;
static float Z_gain = 3;
static float Z_offset = 0;
#endif
// IMU variables
// -------------
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
// Performance monitoring // Performance monitoring
// ---------------------- ////////////////////////////////////////////////////////////////////////////////
static int32_t perf_mon_timer; // Used to manage the rate of performance logging messages
//static float imu_health; // Metric based on accel gain deweighting static int16_t perf_mon_counter;
// The number of GPS fixes we have had
static int16_t gps_fix_count; static int16_t gps_fix_count;
// gps_watchdog check for bad reads and if we miss 12 in a row, we stop navigating
// by lowering nav_lat and navlon to 0 gradually
static byte gps_watchdog; static byte gps_watchdog;
static int pmTest1;
// System Timers // System Timers
// -------------- // --------------
static uint32_t fast_loopTimer; // Time in miliseconds of main control loop // Time in microseconds of main control loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops static uint32_t fast_loopTimer;
// Time in microseconds of 50hz control loop
static uint32_t fiftyhz_loopTimer; static uint32_t fiftyhz_loopTimer;
// Counters for branching from 10 hz control loop
static byte medium_loopCounter;
// Counters for branching from 3 1/3hz control loop
static byte slow_loopCounter; static byte slow_loopCounter;
static int16_t superslow_loopCounter; // Counters for branching at 1 hz
static byte simple_timer; // for limiting the execution of flight mode thingys
static float dTnav; // Delta Time in milliseconds for navigation computations
static uint32_t nav_loopTimer; // used to track the elapsed ime for GPS nav
static byte counter_one_herz; static byte counter_one_herz;
static bool GPS_enabled = false; // Stat machine counter for Simple Mode
static bool new_radio_frame; static byte simple_counter;
// used to track the elapsed time between GPS reads
static uint32_t nav_loopTimer;
// Delta Time in milliseconds for navigation computations, updated with every good GPS read
static float dTnav;
// Counters for branching from 4 minute control loop used to save Compass offsets
static int16_t superslow_loopCounter;
// Tracks if GPS is enabled based on statup routine
// If we do not detect GPS at startup, we stop trying and assume GPS is not connected
static bool GPS_enabled = false;
// Set true if we have new PWM data to act on from the Radio
static bool new_radio_frame;
// Used to auto exit the in-flight leveler
static int16_t auto_level_counter;
// Reference to the AP relay object - APM1 only
AP_Relay relay; AP_Relay relay;
// APM2 only
#if USB_MUX_PIN > 0 #if USB_MUX_PIN > 0
static bool usb_connected; static bool usb_connected;
#endif #endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Top-level logic // Top-level logic
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -645,13 +862,13 @@ void loop()
super_slow_loop(); super_slow_loop();
counter_one_herz = 0; counter_one_herz = 0;
} }
perf_mon_counter++;
if (millis() - perf_mon_timer > 1200 /*20000*/) { if (perf_mon_counter > 600 ) {
if (g.log_bitmask & MASK_LOG_PM) if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance(); Log_Write_Performance();
gps_fix_count = 0; gps_fix_count = 0;
perf_mon_timer = millis(); perf_mon_counter = 0;
} }
//PORTK &= B10111111; //PORTK &= B10111111;
} }
@ -1034,6 +1251,9 @@ static void update_optical_flow(void)
static void update_GPS(void) static void update_GPS(void)
{ {
// A counter that is used to grab at least 10 reads before commiting the Home location
static byte ground_start_count = 10;
g_gps->update(); g_gps->update();
update_GPS_light(); update_GPS_light();
@ -1044,6 +1264,7 @@ static void update_GPS(void)
if(gps_watchdog < 12){ if(gps_watchdog < 12){
gps_watchdog++; gps_watchdog++;
}else{ }else{
// after 12 reads we guess we may have lost GPS signal, stop navigating
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways // we have lost GPS signal for a moment. Reduce our error to avoid flyaways
nav_roll >>= 1; nav_roll >>= 1;
nav_pitch >>= 1; nav_pitch >>= 1;
@ -1145,6 +1366,9 @@ void update_yaw_mode(void)
void update_roll_pitch_mode(void) void update_roll_pitch_mode(void)
{ {
int control_roll, control_pitch;
// hack to do auto_flip - need to remove, no one is using.
#if CH7_OPTION == CH7_FLIP #if CH7_OPTION == CH7_FLIP
if (do_flip){ if (do_flip){
roll_flip(); roll_flip();
@ -1152,45 +1376,34 @@ void update_roll_pitch_mode(void)
} }
#endif #endif
int control_roll = 0, control_pitch = 0;
//read_radio();
if(do_simple && new_radio_frame){
new_radio_frame = false;
simple_timer++;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
if (simple_timer == 1){
// roll
simple_cos_x = sin(radians(90 - delta));
}else if (simple_timer > 2){
// pitch
simple_sin_y = cos(radians(90 - delta));
simple_timer = 0;
}
// Rotate input by the initial bearing
control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y;
control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x);
g.rc_1.control_in = control_roll;
g.rc_2.control_in = control_pitch;
}
switch(roll_pitch_mode){ switch(roll_pitch_mode){
case ROLL_PITCH_ACRO: case ROLL_PITCH_ACRO:
// ACRO does not get SIMPLE mode ability
g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in); g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
break; break;
case ROLL_PITCH_STABLE: case ROLL_PITCH_STABLE:
// apply SIMPLE mode transform
if(do_simple && new_radio_frame){
update_simple_mode();
}
#if WIND_COMP_STAB == 1
// in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch);
#else
// in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
#endif
break; break;
case ROLL_PITCH_AUTO: case ROLL_PITCH_AUTO:
// apply SIMPLE mode transform
if(do_simple && new_radio_frame){
update_simple_mode();
}
// mix in user control with Nav control // mix in user control with Nav control
control_roll = g.rc_1.control_mix(nav_roll); control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch); control_pitch = g.rc_2.control_mix(nav_pitch);
@ -1198,6 +1411,38 @@ void update_roll_pitch_mode(void)
g.rc_2.servo_out = get_stabilize_pitch(control_pitch); g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
break; break;
} }
// clear new radio frame info
new_radio_frame = false;
}
// new radio frame is used to make sure we only call this at 50hz
void update_simple_mode(void)
{
float simple_sin_y, simple_cos_x;
// used to manage state machine
// which improves speed of function
simple_counter++;
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
if (simple_counter == 1){
// roll
simple_cos_x = sin(radians(90 - delta));
}else if (simple_counter > 2){
// pitch
simple_sin_y = cos(radians(90 - delta));
simple_counter = 0;
}
// Rotate input by the initial bearing
int control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y;
int control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x);
g.rc_1.control_in = control_roll;
g.rc_2.control_in = control_pitch;
} }
#define THROTTLE_FILTER_SIZE 4 #define THROTTLE_FILTER_SIZE 4
@ -1214,9 +1459,14 @@ void update_throttle_mode(void)
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
g.rc_3.servo_out = heli_get_angle_boost(g.rc_3.control_in); g.rc_3.servo_out = heli_get_angle_boost(g.rc_3.control_in);
#else #else
angle_boost = get_angle_boost(g.rc_3.control_in); if (control_mode == ACRO){
g.rc_3.servo_out = g.rc_3.control_in + angle_boost; g.rc_3.servo_out = g.rc_3.control_in;
}else{
angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
}
#endif #endif
// calc average throttle // calc average throttle
if ((g.rc_3.control_in > MINIMUM_THROTTLE)){ if ((g.rc_3.control_in > MINIMUM_THROTTLE)){
//throttle_avg = throttle_avg * .98 + rc_3.control_in * .02; //throttle_avg = throttle_avg * .98 + rc_3.control_in * .02;
@ -1226,13 +1476,11 @@ void update_throttle_mode(void)
// Code to manage the Copter state // Code to manage the Copter state
if ((millis() - takeoff_timer) > 5000){ if ((millis() - takeoff_timer) > 5000){
// we must be in the air by now // we must be in the air by now
// stop resetting rate_I() takeoff_complete = true;
takeoff_complete = true; land_complete = false;
}else{ }else{
// reset these I terms to prevent flips on takeoff // reset these I terms to prevent flips on takeoff
g.pi_rate_roll.reset_I(); reset_rate_I();
g.pi_rate_pitch.reset_I();
} }
}else{ }else{
// we are on the ground // we are on the ground
@ -1247,10 +1495,7 @@ void update_throttle_mode(void)
// reset out i terms and takeoff timer // reset out i terms and takeoff timer
// ----------------------------------- // -----------------------------------
g.pi_stabilize_roll.reset_I(); reset_rate_I();
g.pi_stabilize_pitch.reset_I();
g.pi_rate_roll.reset_I();
g.pi_rate_pitch.reset_I();
// remember our time since takeoff // remember our time since takeoff
// ------------------------------- // -------------------------------
@ -1422,9 +1667,8 @@ static void update_navigation()
break; break;
case STABILIZE: case STABILIZE:
//wp_control = NO_NAV_MODE; wp_control = NO_NAV_MODE;
//update_nav_wp(); update_nav_wp();
break; break;
} }
@ -1490,12 +1734,6 @@ static void update_trig(void){
// updated at 10hz // updated at 10hz
static void update_altitude() static void update_altitude()
{ {
altitude_sensor = BARO;
//current_loc.alt = g_gps->altitude - gps_base_alt;
//climb_rate = (g_gps->altitude - old_baro_alt) * 10;
//old_baro_alt = g_gps->altitude;
//baro_alt = g_gps->altitude;
#if HIL_MODE == HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_ATTITUDE
// we are in the SIM, fake out the baro and Sonar // we are in the SIM, fake out the baro and Sonar
int fake_relative_alt = g_gps->altitude - gps_base_alt; int fake_relative_alt = g_gps->altitude - gps_base_alt;
@ -1611,16 +1849,8 @@ static void tuning(){
switch(g.radio_tuning){ switch(g.radio_tuning){
/*
case CH6_THRUST:
g.rc_6.set_range(0,1000); // 0 to 1
//Z_gain = tuning_value * 5;
thrust = tuning_value * .2;
break;*/
case CH6_DAMP: case CH6_DAMP:
g.rc_6.set_range(0,1500); // 0 to 1 g.rc_6.set_range(0,1500); // 0 to 1
//thrust = tuning_value * .2;
g.stablize_d.set(tuning_value); g.stablize_d.set(tuning_value);
break; break;
@ -1745,11 +1975,11 @@ static void update_nav_wp()
if (circle_angle > 6.28318531) if (circle_angle > 6.28318531)
circle_angle -= 6.28318531; circle_angle -= 6.28318531;
target_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); circle_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp);
target_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle)); circle_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle));
// calc the lat and long error to the target // calc the lat and long error to the target
calc_location_error(&target_WP); calc_location_error(&circle_WP);
// use error as the desired rate towards the target // use error as the desired rate towards the target
// nav_lon, nav_lat is calculated // nav_lon, nav_lat is calculated
@ -1759,25 +1989,31 @@ static void update_nav_wp()
// rotate pitch and roll to the copter frame of reference // rotate pitch and roll to the copter frame of reference
calc_loiter_pitch_roll(); calc_loiter_pitch_roll();
// debug
//int angleTest = degrees(circle_angle); //int angleTest = degrees(circle_angle);
//int nroll = nav_roll; //int nroll = nav_roll;
//int npitch = nav_pitch; //int npitch = nav_pitch;
//Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll); //Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll);
}else if(wp_control == WP_MODE){ }else if(wp_control == WP_MODE){
int16_t speed = calc_desired_speed(g.waypoint_speed_max);
// use error as the desired rate towards the target // use error as the desired rate towards the target
calc_nav_rate(g.waypoint_speed_max); calc_nav_rate(speed);
// rotate pitch and roll to the copter frame of reference // rotate pitch and roll to the copter frame of reference
//calc_nav_pitch_roll(); //calc_nav_pitch_roll();
calc_loiter_pitch_roll(); calc_loiter_pitch_roll();
}else if(wp_control == NO_NAV_MODE){ }else if(wp_control == NO_NAV_MODE){
nav_roll = 0; // calc the Iterms for Loiter based on velocity
nav_pitch = 0; //if ((x_actual_speed + y_actual_speed) == 0)
if (g_gps->ground_speed < 50)
// calc the Iterms for Loiter based on velicity calc_wind_compensation();
//calc_position_hold(); else
reduce_wind_compensation();
// rotate nav_lat, nav_lon to roll and pitch
calc_loiter_pitch_roll();
} }
} }
@ -1796,7 +2032,4 @@ static void update_auto_yaw()
auto_yaw = target_bearing; auto_yaw = target_bearing;
} }
// MAV_ROI_NONE = basic Yaw hold // MAV_ROI_NONE = basic Yaw hold
} }