-
-/* for i2c */
-//uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
-
-
-double Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
-double Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
-double Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector
-
-double Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
-double Omega_P[3]= {0,0,0};//Omega Proportional correction
-double Omega_I[3]= {0,0,0};//Omega Integrator
-double Omega[3]= {0,0,0};
-
-double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
-
-double DCM_Matrix[3][3]= {
- {
- 1,0,0 }
- ,{
- 0,1,0 }
- ,{
- 0,0,1 }
-};
-
-double Temporary_Matrix[3][3]={
- {
- 0,0,0 }
- ,{
- 0,0,0 }
- ,{
- 0,0,0 }
-};
-
-double errorRollPitch[3]= {0,0,0};
-double errorYaw[3]= {0,0,0};
-double errorCourse=180;
-double COGX=0; //Course overground X axis
-double COGY=1; //Course overground Y axis
-
-
-
-#define OUTPUTMODE 2
-
-#define ToRad(x) (x*0.01745329252) // *pi/180
-#define ToDeg(x) (x*57.2957795131) // *180/pi
-
-/*
-#define Gyro_Gain_X 0.92 //X axis Gyro gain
-#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
-#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
-*/
-#define Gyro_Gain_X (1.) //X axis Gyro gain
-#define Gyro_Gain_Y (1.) //Y axis Gyro gain
-#define Gyro_Gain_Z (1.) //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
-
-double G_Dt=0.02; // Integration time (DCM algorithm)
-
-#define GRAVITY 1.01 //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 Kp_ROLLPITCH (1.515/GRAVITY)
-#define Ki_ROLLPITCH (0.00101/GRAVITY)
-
-#define Kp_YAW 1.2
-//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
-#define Ki_YAW 0.00005
-
-#define MAGNETIC_DECLINATION 4.0
-
-#define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
-
-
-int mag_offset[3];
-double Heading;
-double Heading_X;
-double Heading_Y;
-
-
-// Euler angles
-double roll;
-double pitch;
-double yaw;
-
-int SENSOR_SIGN[]= {
- 1,1,1, // GYROX, GYROY, GYROZ,
- 1,1,1, // ACCELX, ACCELY, ACCELZ,
- 1,1,1 // MAGX, MAGY, MAGZ,
-};
-
-#if 0
-
-double read_adc(uint8_t index)