/* www.ArduCopter.com - www.DIYDrones.com Copyright (c) 2010. All rights reserved. An Open Source Arduino based multicopter. File : Arducopter.h Version : v1.0, Aug 27, 2010 Author(s): ArduCopter Team Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, Jani Hirvinen, Ken McEwans, Roberto Navoni, Sandro Benigno, Chris Anderson 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 . * ************************************************************** * ChangeLog: * ************************************************************** * TODO: * ************************************************************** */ #include "WProgram.h" /* ************************************************************** */ /* APM Hardware definitions */ #define LED_Yellow 36 // A_LED #define LED_Green 37 // B_LED #define LED_Red 35 // C_LED #define A_LED_PIN LED_Green // For legacy issues #define B_LED_PIN LED_Yellow #define C_LED_PIN LED_Red // Programmable hardware switches/relays #define RELAY 47 // Onboard relay (PL2) #define SW1 41 // Push button close to I2C port (PG0) #define SW2 40 // Slide switch next to DIP switched (PG1) // Due limitations of Arduino libraries, these pins needs to be controlled differently so no real PIN numbers //#define DIP1 (PE7) //#define DIP2 (PE6) //#define DIP3 (PL6) //#define DIP4 (PL/) /* ************************************************************** */ /* Expansion PIN's that people can use for various things. */ // AN0 - 7 are located at edge of IMU PCB "above" pressure sensor and Expansion port // AN0 - 5 are also located next to voltage dividers and sliding SW2 switch // AN0 - 3 has 10kOhm resistor in serial, include 3.9kOhm to make it as voltage divider // AN4 - 5 are direct GPIO pins from atmega1280 and they are the latest pins next to SW2 switch // Look more ArduCopter Wiki for voltage dividers and other ports #define AN0 54 // resistor, vdiv use, divider 1 closest to relay #define AN1 55 // resistor, vdiv use, divider 2 #define AN2 56 // resistor, vdiv use, divider 3 #define AN3 57 // resistor, vdiv use, divider 4 closest to SW2 #define AN4 58 // direct GPIO pin, default as analog input, next to SW2 switch #define AN5 59 // direct GPIO pin, default as analog input, next to SW2 switch #define AN6 60 // direct GPIO pin, default as analog input, close to Pressure sensor, Expansion Ports #define AN7 61 // direct GPIO pin, default as analog input, close to Pressure sensor, Expansion Ports // AN8 - 15 are located at edge of IMU PCB "above" pressure sensor and Expansion port // AN8 - 15 PINs are not connected anywhere, they are located as last 8 pins on edge of the board above Expansion Ports // even pins (8,10,12,14) are at edge of board, Odd pins (9,11,13,15) are on inner row #define AN8 62 // NC #define AN9 63 // NC #define AN10 64 // NC #define AN11 65 // NC #define AN12 66 // NC #define AN13 67 // NC #define AN14 68 // NC #define AN15 69 // NC // Defines for Voltage Dividers #define VDIV1 AN0 // AN0 as default and primary #define VDIV2 AN1 // AN1 for secondary battery #define VDIV3 AN2 #define VDIV4 AN3 /* ************************************************** */ #define EE_LAST_LOG_PAGE 0xE00 #define EE_LAST_LOG_NUM 0xE02 #define EE_LOG_1_START 0xE04 /* ************************************************** */ /* Serial port definitions */ #define SERIAL0_BAUD 38400 // this is the main USB out 38400 57600 115200 #define SERIAL1_BAUD 115200 #define SERIAL2_BAUD 115200 #define SERIAL3_BAUD 115200 #ifdef SerXbee // Xbee/Telemetry port //#define SerBau 115200 // Baud setting moved close next to port selection #define SerPri Serial3.print #define SerPrln Serial3.println #define SerRea Serial3.read #define SerAva Serial3.available #define SerRea Serial3.read #define SerFlu Serial3.flush #define SerBeg Serial3.begin #define SerPor "FTDI" #endif #ifndef SerXbee // If we don't have SerXbee set, it means we have are using Serial0 //#define SerBau 115200 // Baud setting moved close next to port selection #define SerPri Serial.print #define SerPrln Serial.println #define SerRea Serial.read #define SerAva Serial.available #define SerRea Serial.read #define SerFlu Serial.flush #define SerBeg Serial.begin #define SerPor "Telemetry" #endif /* *********************************************** */ // IMU definitions // Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ uint8_t sensors[6] = {1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Hardware // Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ, MAGX, MAGY, MAGZ int SENSOR_SIGN[]={ 1, -1, -1, -1, 1, 1, -1, -1, -1}; //{-1,1,-1,1,-1,1,-1,-1,-1}; /* APM Hardware definitions, END */ /* *********************************************** */ /* General definitions */ #define TRUE 1 #define FALSE 0 #define ON 1 #define OFF 0 // ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 // Tested value : 408 #define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square #define ToRad(x) (x*0.01745329252) // *pi/180 #define ToDeg(x) (x*57.2957795131) // *180/pi // IDG500 Sensitivity (from datasheet) => 2.0mV/º/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : #define Gyro_Gain_X 0.4 //X axis Gyro gain #define Gyro_Gain_Y 0.4 //Y axis Gyro gain #define Gyro_Gain_Z 0.4 //Z axis Gyro gain #define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second /*For debugging purposes*/ #define OUTPUTMODE 1 //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data int AN[6]; //array that store the 6 ADC channels int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers int gyro_temp; float G_Dt=0.02; // Integration time for the gyros (DCM algorithm) float Accel_Vector[3]= {0, 0, 0}; // Store the acceleration in a vector float Accel_Vector_unfiltered[3]= {0, 0, 0}; // Store the acceleration in a vector float Gyro_Vector[3]= {0, 0, 0}; // Store the gyros rutn rate in a vector float Omega_Vector[3]= {0, 0, 0}; // Corrected Gyro_Vector data float Omega_P[3]= {0, 0, 0}; // Omega Proportional correction float Omega_I[3]= {0, 0, 0}; // Omega Integrator float Omega[3]= {0, 0, 0}; //float Accel_magnitude; //float Accel_weight; float errorRollPitch[3] = {0, 0, 0}; float errorYaw[3] = {0, 0, 0}; float errorCourse = 0; float COGX = 0; // Course overground X axis float COGY = 1; // Course overground Y axis float roll = 0; float pitch = 0; float yaw = 0; unsigned int counter = 0; float DCM_Matrix[3][3]= { { 1,0,0 }, { 0,1,0 }, { 0,0,1 }}; float Update_Matrix[3][3]={ { 0,1,2 }, { 3,4,5 }, { 6,7,8 }}; //Gyros here float Temporary_Matrix[3][3]={ { 0,0,0 }, { 0,0,0 }, { 0,0,0 }}; // GPS variables float speed_3d=0; int GPS_ground_speed=0; // Main timers long timer=0; long timer_old; long GPS_timer; long GPS_timer_old; float GPS_Dt=0.2; // GPS Dt // Attitude control variables float command_rx_roll=0; // User commands float command_rx_roll_old; float command_rx_roll_diff; float command_rx_pitch=0; float command_rx_pitch_old; float command_rx_pitch_diff; float command_rx_yaw=0; float command_rx_yaw_diff; int control_roll; // PID control results int control_pitch; int control_yaw; //float K_aux; boolean SW_DIP1; // closest to SW2 slider switch boolean SW_DIP2; boolean SW_DIP3; boolean SW_DIP4; // closest to header pins // Attitude PID controls float roll_I=0; float roll_D; float err_roll; float pitch_I=0; float pitch_D; float err_pitch; float yaw_I=0; float yaw_D; float err_yaw; //Position control long target_longitude; long target_lattitude; byte target_position; float gps_err_roll; float gps_err_roll_old; float gps_roll_D; float gps_roll_I=0; float gps_err_pitch; float gps_err_pitch_old; float gps_pitch_D; float gps_pitch_I=0; float command_gps_roll; float command_gps_pitch; //Altitude control int Initial_Throttle; int target_sonar_altitude; int err_altitude; int err_altitude_old; float command_altitude; float altitude_I; float altitude_D; //Pressure Sensor variables long target_baro_altitude; long press_alt = 0; #define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO #define AIRSPEED_PIN 1 // Need to correct value #define BATTERY_PIN 1 // Need to correct value #define RELAY_PIN 47 #define LOW_VOLTAGE 11.4 // Pack voltage at which to trigger alarm #define INPUT_VOLTAGE 5.2 // (Volts) voltage your power regulator is feeding your ArduPilot to have an accurate pressure and battery // level readings. (you need a multimeter to measure and set this of course) #define VOLT_DIV_RATIO 1.0 // Voltage divider ratio set with thru-hole resistor (see manual) float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage, initialized above threshold for filter // Sonar variables int Sonar_value=0; int Sonar_Counter=0; #define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters // AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode) byte AP_mode = 2; // Mode LED timers and variables, used to blink LED_Green byte gled_status = HIGH; long gled_timer; int gled_speed; long t0; int num_iter; float aux_debug; // Radio definitions int roll_mid; int pitch_mid; int yaw_mid; int Neutro_yaw; int ch_roll; int ch_pitch; int ch_throttle; int ch_yaw; int ch_aux; int ch_aux2; int frontMotor; int backMotor; int leftMotor; int rightMotor; byte motorArmed = 0; int minThrottle = 0; boolean flightOrientation = 0; // 0 = +, 1 = x this is read from DIP1 switch during system bootup // Serial communication char queryType; long tlmTimer = 0; // Arming/Disarming uint8_t Arming_counter=0; uint8_t Disarming_counter=0; // Performance monitoring // ---------------------- long perf_mon_timer = 0; float imu_health = 0; //Metric based on accel gain deweighting int G_Dt_max = 0; //Max main loop cycle time in milliseconds byte gyro_sat_count = 0; byte adc_constraints = 0; byte renorm_sqrt_count = 0; byte renorm_blowup_count = 0; int gps_fix_count = 0; byte gcs_messages_sent = 0; // System Timers // -------------- unsigned long fast_loopTimer = 0; // Time in miliseconds of main control loop unsigned long medium_loopTimer = 0; // Time in miliseconds of navigation control loop byte medium_loopCounter = 0; // Counters for branching from main control loop to slower loops byte slow_loopCounter = 0; // unsigned long deltaMiliSeconds = 0; // Delta Time in miliseconds unsigned long dTnav = 0; // Delta Time in milliseconds for navigation computations int mainLoop_count = 0; unsigned long elapsedTime = 0; // for doing custom events //unsigned int GPS_timer = 0; /* ******************************************************** */ /* Logging Stuff - These should be 1 (on) or 0 (off) */ #define LOG_ATTITUDE 1 // Logs basic attitude info #define LOG_GPS 1 // Logs GPS info #define LOG_PM 1 // Logs IMU performance monitoring info£ #define LOG_CTUN 0 // Logs control loop tuning info #define LOG_NTUN 0 // Logs navigation loop tuning info #define LOG_MODE 1 // Logs mode changes #define LOG_RAW 0 // Logs raw accel/gyro data #define LOG_SEN 1 // Logs sensor data // GCS Message ID's #define MSG_ACKNOWLEDGE 0x00 #define MSG_HEARTBEAT 0x01 #define MSG_ATTITUDE 0x02 #define MSG_LOCATION 0x03 #define MSG_PRESSURE 0x04 #define MSG_STATUS_TEXT 0x05 #define MSG_PERF_REPORT 0x06 #define MSG_COMMAND 0x22 #define MSG_VALUE 0x32 #define MSG_PID 0x42 #define MSG_TRIMS 0x50 #define MSG_MINS 0x51 #define MSG_MAXS 0x52 #define MSG_IMU_OUT 0x53 #define SEVERITY_LOW 1 #define SEVERITY_MEDIUM 2 #define SEVERITY_HIGH 3 #define SEVERITY_CRITICAL 4 // Debug options - set only one of these options to 1 at a time, set the others to 0 #define DEBUG_SUBSYSTEM 0 // 0 = no debug // 1 = Debug the Radio input // 2 = Debug the Servo output // 3 = Debug the Sensor input // 4 = Debug the GPS input // 5 = Debug the GPS input - RAW HEX OUTPUT // 6 = Debug the IMU // 7 = Debug the Control Switch // 8 = Debug the Servo DIP switches // 9 = Debug the Relay out // 10 = Debug the Magnetometer // 11 = Debug the ABS pressure sensor // 12 = Debug the stored waypoints // 13 = Debug the Throttle // 14 = Debug the Radio Min Max // 15 = Debug the EEPROM - Hex Dump #define DEBUG_LEVEL SEVERITY_LOW // SEVERITY_LOW // SEVERITY_MEDIUM // SEVERITY_HIGH // SEVERITY_CRITICAL // Following variables stored in EEPROM float KP_QUAD_ROLL; float KI_QUAD_ROLL; float STABLE_MODE_KP_RATE_ROLL; float KP_QUAD_PITCH; float KI_QUAD_PITCH; float STABLE_MODE_KP_RATE_PITCH; float KP_QUAD_YAW; float KI_QUAD_YAW; float STABLE_MODE_KP_RATE_YAW; float STABLE_MODE_KP_RATE; //NOT USED NOW float KP_GPS_ROLL; float KI_GPS_ROLL; float KD_GPS_ROLL; float KP_GPS_PITCH; float KI_GPS_PITCH; float KD_GPS_PITCH; float GPS_MAX_ANGLE; float KP_ALTITUDE; float KI_ALTITUDE; float KD_ALTITUDE; int acc_offset_x; int acc_offset_y; int acc_offset_z; int gyro_offset_roll; int gyro_offset_pitch; int gyro_offset_yaw; float Kp_ROLLPITCH; float Ki_ROLLPITCH; float Kp_YAW; float Ki_YAW; float GEOG_CORRECTION_FACTOR; int MAGNETOMETER; float Kp_RateRoll; float Ki_RateRoll; float Kd_RateRoll; float Kp_RatePitch; float Ki_RatePitch; float Kd_RatePitch; float Kp_RateYaw; float Ki_RateYaw; float Kd_RateYaw; float xmitFactor; float ch_roll_slope = 1; float ch_pitch_slope = 1; float ch_throttle_slope = 1; float ch_yaw_slope = 1; float ch_aux_slope = 1; float ch_aux2_slope = 1; float ch_roll_offset = 0; float ch_pitch_offset = 0; float ch_throttle_offset = 0; float ch_yaw_offset = 0; float ch_aux_offset = 0; float ch_aux2_offset = 0; // This function call contains the default values that are set to the ArduCopter // when a "Default EEPROM Value" command is sent through serial interface void defaultUserConfig() { KP_QUAD_ROLL = 4.0; KI_QUAD_ROLL = 0.15; STABLE_MODE_KP_RATE_ROLL = 1.2; KP_QUAD_PITCH = 4.0; KI_QUAD_PITCH = 0.15; STABLE_MODE_KP_RATE_PITCH = 1.2; KP_QUAD_YAW = 3.0; KI_QUAD_YAW = 0.15; STABLE_MODE_KP_RATE_YAW = 2.4; STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW KP_GPS_ROLL = 0.015; KI_GPS_ROLL = 0.005; KD_GPS_ROLL = 0.01; KP_GPS_PITCH = 0.015; KI_GPS_PITCH = 0.005; KD_GPS_PITCH = 0.01; GPS_MAX_ANGLE = 22; KP_ALTITUDE = 0.8; KI_ALTITUDE = 0.2; KD_ALTITUDE = 0.2; acc_offset_x = 2048; acc_offset_y = 2048; acc_offset_z = 2048; gyro_offset_roll = 1659; gyro_offset_pitch = 1650; gyro_offset_yaw = 1650; Kp_ROLLPITCH = 0.0014; Ki_ROLLPITCH = 0.00000015; Kp_YAW = 1.0; Ki_YAW = 0.00002; GEOG_CORRECTION_FACTOR = 0.87; MAGNETOMETER = 0; Kp_RateRoll = 1.95; Ki_RateRoll = 0.0; Kd_RateRoll = 0.0; Kp_RatePitch = 1.95; Ki_RatePitch = 0.0; Kd_RatePitch = 0.0; Kp_RateYaw = 3.2; Ki_RateYaw = 0.0; Kd_RateYaw = 0.0; xmitFactor = 0.32; roll_mid = 1500; pitch_mid = 1500; yaw_mid = 1500; ch_roll_slope = 1; ch_pitch_slope = 1; ch_throttle_slope = 1; ch_yaw_slope = 1; ch_aux_slope = 1; ch_aux2_slope = 1; ch_roll_offset = 0; ch_pitch_offset = 0; ch_throttle_offset = 0; ch_yaw_offset = 0; ch_aux_offset = 0; ch_aux2_offset = 0; } // EEPROM storage addresses #define KP_QUAD_ROLL_ADR 0 #define KI_QUAD_ROLL_ADR 8 #define STABLE_MODE_KP_RATE_ROLL_ADR 4 #define KP_QUAD_PITCH_ADR 12 #define KI_QUAD_PITCH_ADR 20 #define STABLE_MODE_KP_RATE_PITCH_ADR 16 #define KP_QUAD_YAW_ADR 24 #define KI_QUAD_YAW_ADR 32 #define STABLE_MODE_KP_RATE_YAW_ADR 28 #define STABLE_MODE_KP_RATE_ADR 36 // NOT USED NOW #define KP_GPS_ROLL_ADR 40 #define KI_GPS_ROLL_ADR 48 #define KD_GPS_ROLL_ADR 44 #define KP_GPS_PITCH_ADR 52 #define KI_GPS_PITCH_ADR 60 #define KD_GPS_PITCH_ADR 56 #define GPS_MAX_ANGLE_ADR 64 #define KP_ALTITUDE_ADR 68 #define KI_ALTITUDE_ADR 76 #define KD_ALTITUDE_ADR 72 #define acc_offset_x_ADR 80 #define acc_offset_y_ADR 84 #define acc_offset_z_ADR 88 #define gyro_offset_roll_ADR 92 #define gyro_offset_pitch_ADR 96 #define gyro_offset_yaw_ADR 100 #define Kp_ROLLPITCH_ADR 104 #define Ki_ROLLPITCH_ADR 108 #define Kp_YAW_ADR 112 #define Ki_YAW_ADR 116 #define GEOG_CORRECTION_FACTOR_ADR 120 #define MAGNETOMETER_ADR 124 #define XMITFACTOR_ADR 128 #define KP_RATEROLL_ADR 132 #define KI_RATEROLL_ADR 136 #define KD_RATEROLL_ADR 140 #define KP_RATEPITCH_ADR 144 #define KI_RATEPITCH_ADR 148 #define KD_RATEPITCH_ADR 152 #define KP_RATEYAW_ADR 156 #define KI_RATEYAW_ADR 160 #define KD_RATEYAW_ADR 164 #define CHROLL_MID 168 #define CHPITCH_MID 172 #define CHYAW_MID 176 #define ch_roll_slope_ADR 180 #define ch_pitch_slope_ADR 184 #define ch_throttle_slope_ADR 188 #define ch_yaw_slope_ADR 192 #define ch_aux_slope_ADR 196 #define ch_aux2_slope_ADR 200 #define ch_roll_offset_ADR 204 #define ch_pitch_offset_ADR 208 #define ch_throttle_offset_ADR 212 #define ch_yaw_offset_ADR 216 #define ch_aux_offset_ADR 220 #define ch_aux2_offset_ADR 224