Stéphane Legru
Minimu 9v with an mbed, working on arduino program
Diff: Compass.h
- Revision:
- 0:fe2fd7f5dac3
diff -r 000000000000 -r fe2fd7f5dac3 Compass.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Compass.h Sun Nov 17 17:59:25 2013 +0000
@@ -0,0 +1,436 @@
+#include <L3GD20.h>
+#include <LSM303DLHC.h>
+
+L3GD20 gyro(p28, p27);
+Serial debug(USBTX,USBRX);
+LSM303DLHC compass(p28, p27);
+
+// Uncomment the below line to use this axis definition:
+ // X axis pointing forward
+ // Y axis pointing to the right
+ // and Z axis pointing down.
+// Positive pitch : nose up
+// Positive roll : right wing down
+// Positive yaw : clockwise
+int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
+// Uncomment the below line to use this axis definition:
+ // X axis pointing forward
+ // Y axis pointing to the left
+ // and Z axis pointing up.
+// Positive pitch : nose down
+// Positive roll : right wing down
+// Positive yaw : counterclockwise
+//int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
+
+// tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168
+
+// LSM303 accelerometer: 8 g sensitivity
+// 3.8 mg/digit; 1 g = 256
+#define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer
+
+#define ToRad(x) ((x)*0.01745329252) // *pi/180
+#define ToDeg(x) ((x)*57.2957795131) // *180/pi
+
+// L3G4200D gyro: 2000 dps full scale
+// 70 mdps/digit; 1 dps = 0.07
+#define Gyro_Gain_X 0.07 //X axis Gyro gain
+#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
+#define Gyro_Gain_Z 0.07 //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
+
+// LSM303 magnetometer calibration constants; use the Calibrate example from
+// the Pololu LSM303 library to find the right values for your board
+#define M_X_MIN -580
+#define M_Y_MIN -650
+#define M_Z_MIN -770
+#define M_X_MAX 495
+#define M_Y_MAX 480
+#define M_Z_MAX 370
+//MIN x: -576.000000 y: -645.000000 z: -767.000000
+//MAX x: 489.000000 y: 475.000000 z: 363.000000
+
+#define Kp_ROLLPITCH 0.02
+#define Ki_ROLLPITCH 0.00002
+#define Kp_YAW 1.2
+#define Ki_YAW 0.00002
+
+/*For debugging purposes*/
+//OUTPUTMODE=1 will print the corrected data,
+//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
+#define OUTPUTMODE 1
+
+//#define PRINT_DCM 0 //Will print the whole direction cosine matrix
+#define PRINT_ANALOGS 0 //Will print the analog raw data
+#define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw
+
+#define STATUS_LED 13
+
+float G_Dt=0.02; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible
+
+Timer tim; //general purpuse timer
+long timer=0;
+long timer_old;
+long timer24=0; //Second timer used to print values
+int AN[6]; //array that stores the gyro and accelerometer data
+int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors
+
+int gyro_x;
+int gyro_y;
+int gyro_z;
+int accel_x;
+int accel_y;
+int accel_z;
+int magnetom_x;
+int magnetom_y;
+int magnetom_z;
+float c_magnetom_x;
+float c_magnetom_y;
+float c_magnetom_z;
+float MAG_Heading;
+
+float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
+float Gyro_Vector[3]= {0,0,0};//Store the gyros turn 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};
+
+// Euler angles
+float roll;
+float pitch;
+float yaw;
+
+float errorRollPitch[3]= {0,0,0};
+float errorYaw[3]= {0,0,0};
+
+unsigned int counter=0;
+int gyro_sat=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 }
+};
+
+float min(float x, float y)
+{
+ if(x < y)
+ {return x;}
+ else
+ {return y;}
+}
+float max(float x, float y)
+{
+ if(x > y)
+ {return x;}
+ else
+ {return y;}
+}
+
+void Compass_Heading()
+{
+ float MAG_X;
+ float MAG_Y;
+ float cos_roll;
+ float sin_roll;
+ float cos_pitch;
+ float sin_pitch;
+
+ cos_roll = cos(roll);
+ sin_roll = sin(roll);
+ cos_pitch = cos(pitch);
+ sin_pitch = sin(pitch);
+
+ // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
+ c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.5;
+ c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*0.5;
+ c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*0.5;
+
+ // Tilt compensated Magnetic filed X:
+ MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
+ // Tilt compensated Magnetic filed Y:
+ MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
+ // Magnetic Heading
+ MAG_Heading = atan2(-MAG_Y,MAG_X);
+}
+float Vector_Dot_Product(float vector1[3],float vector2[3])
+{
+ float op=0;
+
+ for(int c=0; c<3; c++)
+ {
+ op = op + (vector1[c]*vector2[c]);
+ }
+
+ return op;
+}
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+{
+ vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+ vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+ vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar.
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+ for(int c=0; c<3; c++)
+ {
+ vectorOut[c]=vectorIn[c]*scale2;
+ }
+}
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+ for(int c=0; c<3; c++)
+ {
+ vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+ }
+}
+/**************************************************/
+void Normalize(void)
+{
+ float error=0;
+ float temporary[3][3];
+ float renorm=0;
+
+ error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
+
+ Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+ Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+
+ Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
+ Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
+
+ Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+
+ renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
+ Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+
+ renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
+ Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+
+ renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
+ Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+}
+
+float constrain(float x,float a, float b)
+{
+ if(x <a)
+ {
+ return a;
+ }
+ if(x > b)
+ {
+ return b;
+ }
+ return x;
+}
+/**************************************************/
+void Drift_correction(void)
+{
+ float mag_heading_x;
+ float mag_heading_y;
+ float errorCourse;
+ //Compensation the Roll, Pitch and Yaw drift.
+ static float Scaled_Omega_P[3];
+ static float Scaled_Omega_I[3];
+ float Accel_magnitude;
+ float Accel_weight;
+
+
+ //*****Roll and Pitch***************
+
+ // Calculate the magnitude of the accelerometer vector
+ Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+ Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+ // Dynamic weighting of accelerometer info (reliability filter)
+ // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+ Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
+
+ Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+ Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+
+ Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
+
+ //*****YAW***************
+ // We make the gyro YAW drift correction based on compass magnetic heading
+
+ mag_heading_x = cos(MAG_Heading);
+ mag_heading_y = sin(MAG_Heading);
+ errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
+ Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+
+ Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
+ Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
+
+ Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+}
+/**************************************************/
+/*
+void Accel_adjust(void)
+{
+ Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
+ Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
+}
+*/
+/**************************************************/
+/**************************************************/
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+ float op[3];
+ for(int x=0; x<3; x++)
+ {
+ for(int y=0; y<3; y++)
+ {
+ for(int w=0; w<3; w++)
+ {
+ op[w]=a[x][w]*b[w][y];
+ }
+ mat[x][y]=0;
+ mat[x][y]=op[0]+op[1]+op[2];
+ }
+ }
+}
+void Matrix_update(void)
+{
+ Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
+ Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
+ Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
+
+ Accel_Vector[0]=accel_x;
+ Accel_Vector[1]=accel_y;
+ Accel_Vector[2]=accel_z;
+
+ Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_P[0]); //adding proportional term
+ Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_I[0]); //adding Integrator term
+
+ //Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
+
+ Update_Matrix[0][0]=0;
+ Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+ Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+ Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+ Update_Matrix[1][1]=0;
+ Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+ Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+ Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+ Update_Matrix[2][2]=0;
+
+ Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+ for(int x=0; x<3; x++) //Matrix Addition (update)
+ {
+ for(int y=0; y<3; y++)
+ {
+ DCM_Matrix[x][y] = DCM_Matrix[x][y]+ Temporary_Matrix[x][y];
+ }
+ }
+}
+
+void Euler_angles(void)
+{
+ pitch = -asin(DCM_Matrix[2][0]);
+ roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+}
+
+
+void Gyro_Init()
+{
+ //gyro.init(); OK
+ char data = 0x0F;
+ gyro.write_reg(GYR_ADDRESS, L3GD20_CTRL_REG1, data); // normal power mode, all axes enabled, 100 Hz
+ data = 0x20;
+ gyro.write_reg(GYR_ADDRESS, L3GD20_CTRL_REG4, data); // 2000 dps full scale
+}
+
+void Read_Gyro(float* gx, float* gy, float* gz)
+{
+ gyro.read(gx, gy, gz);
+ AN[0] = *gx;
+ AN[1] = *gy;
+ AN[2] = *gz;
+ gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
+ gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
+ gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
+}
+
+void Accel_Init()
+{
+// compass.init(); OK
+ char data = 0x47;
+ compass.write_reg(0x32, CTRL_REG1_A, data); // normal power mode, all axes enabled, 50 Hz ACC
+ data = 0x28;
+ compass.write_reg(0x32, CTRL_REG4_A, data); // 8 g full scale: FS = 10 on DLHC; high resolution output mode ACC
+}
+
+// Reads x,y and z accelerometer registers
+void Read_Accel(float* ax, float* ay, float* az)
+{
+ compass.readacc(ax, ay, az);
+
+ AN[3] = *ax;
+ AN[4] = *ay;
+ AN[5] = *az;
+ accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
+ accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
+ accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
+}
+
+void Compass_Init()
+{
+ char data = 0x00;
+ compass.write_reg(0x3c, CTRL_REG1_A, data); // continuous conversion mode
+ // 15 Hz default
+}
+
+void Read_Compass(float* mx, float* my, float* mz)
+{
+ compass.readcomp(mx, my, mz);
+
+ magnetom_x = SENSOR_SIGN[6] * (*mx);
+ magnetom_y = SENSOR_SIGN[7] * (*my);
+ magnetom_z = SENSOR_SIGN[8] * (*mz);
+}
+
+void printdata(Serial debug)
+{
+ debug.printf("!");
+ debug.printf("ANG:");
+ debug.printf("%lf",ToDeg(roll));
+ debug.printf(",");
+ debug.printf("%lf",ToDeg(pitch));
+ debug.printf(",");
+ debug.printf("%lf",ToDeg(yaw));
+ debug.printf("\n\r");
+
+}
+
+long convert_to_dec(float x)
+{
+ return x*10000000;
+}
+//Computes the dot product of two vectors
+
+