Minimu 9v with an mbed, working on arduino program
Compass.h
- Committer:
- patsteph
- Date:
- 2013-11-17
- Revision:
- 0:fe2fd7f5dac3
File content as of revision 0:fe2fd7f5dac3:
#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