DCM Code ported from Arduino for FRDM-KL25Z
Dependents: minimu_data_capture minimu_data_capture
Fork of DCM_AHRS by
Diff: minimu9.cpp
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
- Child:
- 2:85214374e094
--- a/minimu9.cpp Thu Apr 12 13:47:23 2012 +0000 +++ b/minimu9.cpp Mon Apr 23 14:31:08 2012 +0000 @@ -1,77 +1,284 @@ -/* 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 "mbed.h" #include "minimu9.h" +#include "LSM303.h" #include "L3G4200D.h" -#include "LSM303.h" #include "Matrix.h" +minimu9::minimu9( void ) { + Serial pc(USBTX, USBRX); + t.start(); + initialize_values(); + gyro = new L3G4200D( p9, p10 ); + compass = new LSM303( p9, p10 ); + + // Initiate the accel + compass->writeAccReg(LSM303_CTRL_REG1_A, 0x27); // normal power mode, all axes enabled, 50 Hz + compass->writeAccReg(LSM303_CTRL_REG4_A, 0x30); // 8 g full scale + + // Initiate the compass + compass->init(); + compass->writeMagReg(LSM303_MR_REG_M, 0x00); // continuous conversion mode + + // Initiate the gyro + gyro->writeReg(L3G4200D_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz + gyro->writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale + + wait( 0.02 ); + + 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.02 ); + } + + for (int y=0; y<6; y++) + AN_OFFSET[y] = AN_OFFSET[y]/32; + + AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5]; + + //Serial.println("Offset:"); + for ( int y=0; y<6; y++ ) { + pc.printf( "%d\n", AN_OFFSET[y] ); + } + wait( 2 ); + + timer=t.read_ms(); + wait(0.02); + counter=0; +} + +bool minimu9::update() { //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(); + // *** + + print_data(); + return true; + } + return false; +} + +void minimu9::read_gyro() { + gyro->read(); + + AN[0] = gyro->g.x; + AN[1] = gyro->g.y; + AN[2] = gyro->g.z; + 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() { + compass->readAcc(); + + AN[3] = compass->a.x; + AN[4] = compass->a.y; + AN[5] = compass->a.z; + 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 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; +void minimu9::read_compass() { + compass->readMag(); + + magnetom_x = SENSOR_SIGN[6] * compass->m.x; + magnetom_y = SENSOR_SIGN[7] * compass->m.y; + magnetom_z = SENSOR_SIGN[8] * compass->m.z; +} + +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.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); +} - /*For debugging purposes*/ - //OUTPUTMODE=1 will print the corrected data, - //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) - OUTPUTMODE = 1; +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*************** - //#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; + // 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),0,1; // + if( 1 < Accel_weight ) { + Accel_weight = 1; + } + else if( 0 > Accel_weight ) { + Accel_weight = 0; + } + + 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 - PRINT_SERIAL = 0; - //PRINT_SERIAL = 1; + 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 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 - t.start(); -#if PRINT_SERIAL == 1 - Serial serial(USBTX, USBRX); +#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 - gyro = new L3G4200D( p9, p10 ); - compass = new LSM303( p9, p10 ); - I2C i2c(p9, p10); + + 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]; + } + } +} - wait(1.5); - /* - * Give variables initial values - */ +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]); +} + + +float minimu9::get_roll( void ) { + return ToDeg( roll ); +} +float minimu9::get_pitch( void ) { + return ToDeg( pitch ); +} +float minimu9::get_yaw( void ) { + return ToDeg( yaw ); +} + + +void minimu9::initialize_values( void ) { G_Dt=0.02; timer=0; timer24=0; @@ -79,7 +286,8 @@ gyro_sat=0; memcpy(SENSOR_SIGN, (int [9]) { - 1,1,1,-1,-1,-1,1,1,1 + 1,-1,-1,-1,1,1,1,-1,-1 + //1,1,1,-1,-1,-1,1,1,1 }, 9*sizeof(int)); memcpy(AN_OFFSET, (int [6]) { 0,0,0,0,0,0 @@ -123,381 +331,21 @@ 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 DEBUG_MODE == 1 + Serial pc(USBTX, USBRX); + pc.printf("!"); #if PRINT_EULER == 1 - serial.printf("ANG:"); - serial.printf("%f, ", ToDeg(roll)); - serial.printf("%f, ", ToDeg(pitch)); - serial.printf("%f, ", ToDeg(yaw)); + pc.printf("ANG:%.2f,%.2f,%.2f", ToDeg(roll), ToDeg(pitch), 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); + pc.printf(",AN:%d,%d,%d,%d,%d,%d", AN[0], AN[1], AN[2], AN[3], AN[4], AN[5] ); + pc.printf(",%.2f,%.2f,%.2f", c_magnetom_x, c_magnetom_y, c_magnetom_z ); +#endif + pc.printf("\n"); #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; -}