AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
minimu9.cpp
- Committer:
- krmreynolds
- Date:
- 2012-04-12
- Revision:
- 0:dc35364e2291
- Child:
- 1:3272ece36ce1
File content as of revision 0:dc35364e2291:
/* mbed L3G4200D Library version 0.1 * Copyright (c) 2012 Prediluted * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ /* * Communication for the minimu9, ported from the original Polulu minIMU-9 Arduino code */ #include <stdlib.h> #include <algorithm> #include <iostream> #include <vector> #include <math.h> #include "minimu9.h" #include "L3G4200D.h" #include "LSM303.h" #include "Matrix.h" /* * void setup() replaced with the constructor */ minimu9::minimu9() { // Calibration constants defaults, will need to calibrate then use // set_calibration_constants() to have the proper values for your board M_X_MIN = -570; M_Y_MIN = -746; M_Z_MIN = -416; M_X_MAX = 477; M_Y_MAX = 324; M_Z_MAX = 652; /*For debugging purposes*/ //OUTPUTMODE=1 will print the corrected data, //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) OUTPUTMODE = 1; //#define PRINT_DCM 0 //Will print the whole direction cosine matrix PRINT_ANALOGS = 0; //Will print the analog raw data PRINT_EULER = 1; //Will print the Euler angles Roll, Pitch and Yaw PRINT_DCM = 0; PRINT_SERIAL = 0; //PRINT_SERIAL = 1; t.start(); #if PRINT_SERIAL == 1 Serial serial(USBTX, USBRX); #endif gyro = new L3G4200D( p9, p10 ); compass = new LSM303( p9, p10 ); I2C i2c(p9, p10); wait(1.5); /* * Give variables initial values */ G_Dt=0.02; timer=0; timer24=0; counter=0; gyro_sat=0; memcpy(SENSOR_SIGN, (int [9]) { 1,1,1,-1,-1,-1,1,1,1 }, 9*sizeof(int)); memcpy(AN_OFFSET, (int [6]) { 0,0,0,0,0,0 }, 6*sizeof(int)); memcpy(Accel_Vector, (float [3]) { 0,0,0 }, 3*sizeof(float)); memcpy(Gyro_Vector, (float [3]) { 0,0,0 }, 3*sizeof(float)); memcpy(Omega_Vector, (float [3]) { 0,0,0 }, 3*sizeof(float)); memcpy(Omega_P, (float [3]) { 0,0,0 }, 3*sizeof(float)); memcpy(Omega_I, (float [3]) { 0,0,0 }, 3*sizeof(float)); memcpy(Omega, (float [3]) { 0,0,0 }, 3*sizeof(float)); memcpy(errorRollPitch, (float [3]) { 0,0,0 }, 3*sizeof(float)); memcpy(errorYaw, (float [3]) { 0,0,0 }, 3*sizeof(float)); memcpy( DCM_Matrix, (float[3][3]) { { 1,0,0 }, { 0,1,0 }, { 0,0,1 } }, 9*sizeof(float) ); memcpy( Update_Matrix, (float[3][3]) { { 0,1,2 }, { 3,4,5 }, { 6,7,8 } }, 9*sizeof(float) ); memcpy( Temporary_Matrix, (float[3][3]) { { 0,0,0 }, { 0,0,0 }, { 0,0,0 } }, 9*sizeof(float) ); /* * Initialize the accel, compass, and gyro */ // Accel compass->accRegisterWrite(LSM303::ACC_CTRL_REG1, 0x24); // normal power mode, all axes enabled, 50 Hz compass->accRegisterWrite(LSM303::ACC_CTRL_REG4, 0x30 ); // 8g full scale // Compass compass->magRegisterWrite(LSM303::MAG_MR_REG, 0x00); // continuous conversion mode // Gyro gyro->registerWrite(L3G4200D::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz gyro->registerWrite(L3G4200D::CTRL_REG4, 0x20); // 2000 dps full scale #if PRINT_SERIAL == 1 serial.printf( "initiatied\n" ); } #endif for (int i=0; i<32; i++) { // We take some readings... Read_Gyro(); Read_Accel(); for (int y=0; y<6; y++) // Cumulate values AN_OFFSET[y] += AN[y]; wait(0.2); } for (int y=0; y<6; y++) AN_OFFSET[y] = AN_OFFSET[y]/32; AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5]; #if PRINT_SERIAL == 1 serial.printf("Offset:\n"); for (int y=0; y<6; y++) { serial.printf( "%d", AN_OFFSET[y] ); } serial.printf( "\n" ); #endif wait(2); timer=t.read_ms(); wait(0.2); counter=0; #if PRINT_SERIAL == 1 while ( 1 ) { get_data(); } #endif } void minimu9::get_data() { //Main Loop if ((t.read_ms()-timer)>=20) { // Main loop runs at 50Hz counter++; timer_old = timer; timer=t.read_ms(); if (timer>timer_old) G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0; // *** DCM algorithm // Data adquisition Read_Gyro(); // This read gyro data Read_Accel(); // Read I2C accelerometer if (counter > 5) { // Read compass data at 10Hz... (5 loop runs) counter=0; Read_Compass(); // Read I2C magnetometer Compass_Heading(); // Calculate magnetic heading } // Calculations... Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); // *** if ( 1 == PRINT_SERIAL ) { print_data(); } } } long convert_to_dec(float x) { return x*10000000; } void minimu9::Read_Gyro() { gyro->read(); std::vector<short> g = gyro->read(); AN[0] = g[0]; AN[1] = g[1]; AN[2] = g[2]; 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]); } // Reads x,y and z accelerometer registers void minimu9::Read_Accel() { std::vector<short> a = compass->accRead(); AN[3] = a[0]; AN[4] = a[1]; AN[5] = a[2]; 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 minimu9::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.0; 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); } void minimu9::Read_Compass() { std::vector<short> m = compass->magRead(); magnetom_x = SENSOR_SIGN[6] * m[0]; magnetom_y = SENSOR_SIGN[7] * m[1]; magnetom_z = SENSOR_SIGN[8] * m[2]; } void minimu9::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); } /**************************************************/ void minimu9::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 = 1 - 2 * abs( 1 - Accel_magnitude ); if ( Accel_weight < 0 ) { Accel_weight = 0; } else if ( Accel_weight > 1 ) { Accel_weight = 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);//.01 proportional of YAW. Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001 Integrator Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I } void minimu9::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_I[0]); //adding proportional term Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term #if OUTPUTMODE==1 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; #else // Uncorrected data (no drift correction) Update_Matrix[0][0]=0; Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z Update_Matrix[1][1]=0; Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; Update_Matrix[2][2]=0; #endif 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]+=Temporary_Matrix[x][y]; } } } void minimu9::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 minimu9::print_data(void) { Serial serial(USBTX, USBRX); serial.printf("!"); #if PRINT_EULER == 1 serial.printf("ANG:"); serial.printf("%f, ", ToDeg(roll)); serial.printf("%f, ", ToDeg(pitch)); serial.printf("%f, ", ToDeg(yaw)); #endif #if PRINT_ANALOGS==1 serial.printf(",AN:"); serial.printf("%d, ", AN[0]); serial.printf("%d, ", AN[1]); serial.printf("%d, ", AN[2]); serial.printf("%d, ", AN[3]); serial.printf("%d, ", AN[4]); serial.printf("%d, ", AN[5]); serial.printf("%f, ", c_magnetom_x); serial.printf("%f, ", c_magnetom_y); serial.printf("%f, ", c_magnetom_z); #endif #if PRINT_DCM == 1 serial.printf (",DCM:"); serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][0])); serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][1])); serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][2])); serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][0])); serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][1])); serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][2])); serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][0])); serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][1])); serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][2])); #endif serial.printf("\n"); } float minimu9::get_pitch( void ) { return ToDeg( pitch ); } float minimu9::get_roll( void ) { return ToDeg( roll ); } float minimu9::get_yaw( void ) { return ToDeg( yaw ); } int min ( int a, int b ) { if ( b < a ) return b; return a; } int max ( int a, int b ) { if ( b > a ) return b; return a; } void minimu9::Calibrate_compass( void ) { Serial serial(USBTX, USBRX); int running_min[3] = {2047, 2047, 2047}, running_max[3] = {-2048, -2048, -2048}; while ( 1 ) { std::vector<short> m = compass->magRead(); for ( int i = 0; i < 3; i++ ) { running_min[i] = min(running_min[i], m[i]); running_max[i] = max(running_max[i], m[i]); } serial.printf("M min "); serial.printf("X: %d", (int)running_min[0] ); serial.printf(" Y: %d", (int)running_min[1]); serial.printf(" Z: %d", (int)running_min[2]); serial.printf(" M max "); serial.printf("X: %d", (int)running_max[0] ); serial.printf(" Y: %d", (int)running_max[1]); serial.printf(" Z: %d",(int)running_max[2]); serial.printf("\n"); } } void minimu9::set_calibration_values( int x_min, int y_min, int z_min, int x_max, int y_max, int z_max ) { M_X_MIN = x_min; M_Y_MIN = y_min; M_Z_MIN = z_min; M_X_MAX = x_max; M_Y_MAX = y_max; M_Z_MAX = z_max; } void minimu9::set_print_settings( int mode, int analogs, int euler, int dcm, int serial ) { /*For debugging purposes*/ //OUTPUTMODE=1 will print the corrected data, //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) OUTPUTMODE = mode; //#define PRINT_DCM 0 //Will print the whole direction cosine matrix PRINT_ANALOGS = analogs; //Will print the analog raw data PRINT_EULER = euler; //Will print the Euler angles Roll, Pitch and Yaw PRINT_DCM = dcm; PRINT_SERIAL = serial; //PRINT_SERIAL = 1; }