//uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
-float Accel_Vector[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 Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector
+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
-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};
+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};
-float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
+double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
-float DCM_Matrix[3][3]= {
+double DCM_Matrix[3][3]= {
{
1,0,0 }
,{
0,0,1 }
};
-float Temporary_Matrix[3][3]={
+double Temporary_Matrix[3][3]={
{
0,0,0 }
,{
0,0,0 }
};
-float errorRollPitch[3]= {0,0,0};
-float errorYaw[3]= {0,0,0};
-float errorCourse=180;
-float COGX=0; //Course overground X axis
-float COGY=1; //Course overground Y axis
+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 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
-float G_Dt=0.02; // Integration time (DCM algorithm)
+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
int mag_offset[3];
-float Heading;
-float Heading_X;
-float Heading_Y;
+double Heading;
+double Heading_X;
+double Heading_Y;
// Euler angles
-float roll;
-float pitch;
-float yaw;
+double roll;
+double pitch;
+double yaw;
int SENSOR_SIGN[]= {
1,1,1, // GYROX, GYROY, GYROZ,
#if 0
-float read_adc(uint8_t index)
+double read_adc(uint8_t index)
{
int16_t value;
switch(index) {
case 0:
itg3200_read_axe(0, &value);
- return (float) (SENSOR_SIGN[index] * value);
+ return (double) (SENSOR_SIGN[index] * value);
case 1:
itg3200_read_axe(1, &value);
- return (float) (SENSOR_SIGN[index] * value);
+ return (double) (SENSOR_SIGN[index] * value);
case 2:
itg3200_read_axe(2, &value);
- return (float) (SENSOR_SIGN[index] * value);
+ return (double) (SENSOR_SIGN[index] * value);
case 3:
bma150_read_axe(0, &value);
- return (float) (SENSOR_SIGN[index] * bma15_axe2g(value));
+ return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
case 4:
bma150_read_axe(1, &value);
- return (float) (SENSOR_SIGN[index] * bma15_axe2g(value));
+ return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
case 5:
bma150_read_axe(2, &value);
- return (float) (SENSOR_SIGN[index] * bma15_axe2g(value));
+ return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
}
return 0.0;
}
err = ak8500_read_all_axes(&axes);
if (err == 0) {
/*
- Magnet_Vector[0] = (float)SENSOR_SIGN[6] * (float)axes[0];
- Magnet_Vector[1] = (float)SENSOR_SIGN[7] * (float)axes[1];
- Magnet_Vector[2] = (float)SENSOR_SIGN[8] * (float)axes[2];
+ Magnet_Vector[0] = (double)SENSOR_SIGN[6] * (double)axes[0];
+ Magnet_Vector[1] = (double)SENSOR_SIGN[7] * (double)axes[1];
+ Magnet_Vector[2] = (double)SENSOR_SIGN[8] * (double)axes[2];
*/
/*
*/