ardupilot/Arducopter/ArduCopter.h

254 lines
7.2 KiB
C
Raw Normal View History

/*
ArduCopter 1.3 - August 2010
www.ArduCopter.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
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 <http://www.gnu.org/licenses/>.
*/
#include "WProgram.h"
/*******************************************************************/
// ArduPilot Mega specific hardware and software settings
//
// DO NOT EDIT unless you are absolytely sure of your doings.
// User configurable settings are on UserConfig.h
/*******************************************************************/
/**************************************************************/
// Special features that might disapear in future releases
/* APM Hardware definitions */
#define LED_Yellow 36
#define LED_Red 35
#define LED_Green 37
#define RELE_pin 47
#define SW1_pin 41
#define SW2_pin 40
//#define VDIV1 AN1
//#define VDIV2 AN2
//#define VDIV3 AN3
//#define VDIV4 AN4
//#define AN5
//#define AN6
// 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.41 //Y axis Gyro gain
#define Gyro_Gain_Z 0.41 //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_pitch=0;
float command_rx_yaw=0;
int control_roll; // PID control results
int control_pitch;
int control_yaw;
float K_aux;
// 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
#ifdef UseBMP
unsigned long abs_press = 0;
unsigned long abs_press_filt = 0;
unsigned long abs_press_gnd = 0;
int ground_temperature = 0; //
int temp_unfilt = 0;
long ground_alt = 0; // Ground altitude from gps at startup in centimeters
long press_alt = 0; // Pressure altitude
#endif
#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;
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
int Sonar_Counter=0;
// AP_mode : 1=> Position hold 2=>Stabilization assist mode (normal mode)
byte AP_mode = 2;
byte FL_mode = 0; // Flight mode : 0 => Stable, 1 => Acro mode. Stable as default
// 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;
// Serial communication
char queryType;
long tlmTimer = 0;
// Arming/Disarming
uint8_t Arming_counter=0;
uint8_t Disarming_counter=0;