DCM Code ported from Arduino for FRDM-KL25Z
Dependents: minimu_data_capture minimu_data_capture
Fork of DCM_AHRS by
minimu9.cpp@1:3272ece36ce1, 2012-04-23 (annotated)
- Committer:
- krmreynolds
- Date:
- Mon Apr 23 14:31:08 2012 +0000
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
- Child:
- 2:85214374e094
Ported L3G4200D and LSM303 from the Polulu website, dropped the horrible code that someone else built
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
krmreynolds | 0:dc35364e2291 | 1 | #include <math.h> |
krmreynolds | 1:3272ece36ce1 | 2 | #include "mbed.h" |
krmreynolds | 0:dc35364e2291 | 3 | #include "minimu9.h" |
krmreynolds | 1:3272ece36ce1 | 4 | #include "LSM303.h" |
krmreynolds | 0:dc35364e2291 | 5 | #include "L3G4200D.h" |
krmreynolds | 0:dc35364e2291 | 6 | #include "Matrix.h" |
krmreynolds | 0:dc35364e2291 | 7 | |
krmreynolds | 1:3272ece36ce1 | 8 | minimu9::minimu9( void ) { |
krmreynolds | 1:3272ece36ce1 | 9 | Serial pc(USBTX, USBRX); |
krmreynolds | 1:3272ece36ce1 | 10 | t.start(); |
krmreynolds | 1:3272ece36ce1 | 11 | initialize_values(); |
krmreynolds | 1:3272ece36ce1 | 12 | gyro = new L3G4200D( p9, p10 ); |
krmreynolds | 1:3272ece36ce1 | 13 | compass = new LSM303( p9, p10 ); |
krmreynolds | 1:3272ece36ce1 | 14 | |
krmreynolds | 1:3272ece36ce1 | 15 | // Initiate the accel |
krmreynolds | 1:3272ece36ce1 | 16 | compass->writeAccReg(LSM303_CTRL_REG1_A, 0x27); // normal power mode, all axes enabled, 50 Hz |
krmreynolds | 1:3272ece36ce1 | 17 | compass->writeAccReg(LSM303_CTRL_REG4_A, 0x30); // 8 g full scale |
krmreynolds | 1:3272ece36ce1 | 18 | |
krmreynolds | 1:3272ece36ce1 | 19 | // Initiate the compass |
krmreynolds | 1:3272ece36ce1 | 20 | compass->init(); |
krmreynolds | 1:3272ece36ce1 | 21 | compass->writeMagReg(LSM303_MR_REG_M, 0x00); // continuous conversion mode |
krmreynolds | 1:3272ece36ce1 | 22 | |
krmreynolds | 1:3272ece36ce1 | 23 | // Initiate the gyro |
krmreynolds | 1:3272ece36ce1 | 24 | gyro->writeReg(L3G4200D_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz |
krmreynolds | 1:3272ece36ce1 | 25 | gyro->writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale |
krmreynolds | 1:3272ece36ce1 | 26 | |
krmreynolds | 1:3272ece36ce1 | 27 | wait( 0.02 ); |
krmreynolds | 1:3272ece36ce1 | 28 | |
krmreynolds | 1:3272ece36ce1 | 29 | for (int i=0; i<32; i++) { // We take some readings... |
krmreynolds | 1:3272ece36ce1 | 30 | read_gyro(); |
krmreynolds | 1:3272ece36ce1 | 31 | read_accel(); |
krmreynolds | 1:3272ece36ce1 | 32 | for (int y=0; y<6; y++) // Cumulate values |
krmreynolds | 1:3272ece36ce1 | 33 | AN_OFFSET[y] += AN[y]; |
krmreynolds | 1:3272ece36ce1 | 34 | wait( 0.02 ); |
krmreynolds | 1:3272ece36ce1 | 35 | } |
krmreynolds | 1:3272ece36ce1 | 36 | |
krmreynolds | 1:3272ece36ce1 | 37 | for (int y=0; y<6; y++) |
krmreynolds | 1:3272ece36ce1 | 38 | AN_OFFSET[y] = AN_OFFSET[y]/32; |
krmreynolds | 1:3272ece36ce1 | 39 | |
krmreynolds | 1:3272ece36ce1 | 40 | AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5]; |
krmreynolds | 1:3272ece36ce1 | 41 | |
krmreynolds | 1:3272ece36ce1 | 42 | //Serial.println("Offset:"); |
krmreynolds | 1:3272ece36ce1 | 43 | for ( int y=0; y<6; y++ ) { |
krmreynolds | 1:3272ece36ce1 | 44 | pc.printf( "%d\n", AN_OFFSET[y] ); |
krmreynolds | 1:3272ece36ce1 | 45 | } |
krmreynolds | 1:3272ece36ce1 | 46 | wait( 2 ); |
krmreynolds | 1:3272ece36ce1 | 47 | |
krmreynolds | 1:3272ece36ce1 | 48 | timer=t.read_ms(); |
krmreynolds | 1:3272ece36ce1 | 49 | wait(0.02); |
krmreynolds | 1:3272ece36ce1 | 50 | counter=0; |
krmreynolds | 1:3272ece36ce1 | 51 | } |
krmreynolds | 1:3272ece36ce1 | 52 | |
krmreynolds | 1:3272ece36ce1 | 53 | bool minimu9::update() { //Main Loop |
krmreynolds | 1:3272ece36ce1 | 54 | if ((t.read_ms()-timer)>=20) { // Main loop runs at 50Hz |
krmreynolds | 1:3272ece36ce1 | 55 | counter++; |
krmreynolds | 1:3272ece36ce1 | 56 | timer_old = timer; |
krmreynolds | 1:3272ece36ce1 | 57 | timer=t.read_ms(); |
krmreynolds | 1:3272ece36ce1 | 58 | if (timer>timer_old) |
krmreynolds | 1:3272ece36ce1 | 59 | G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) |
krmreynolds | 1:3272ece36ce1 | 60 | else |
krmreynolds | 1:3272ece36ce1 | 61 | G_Dt = 0; |
krmreynolds | 1:3272ece36ce1 | 62 | |
krmreynolds | 1:3272ece36ce1 | 63 | // *** DCM algorithm |
krmreynolds | 1:3272ece36ce1 | 64 | // Data adquisition |
krmreynolds | 1:3272ece36ce1 | 65 | read_gyro(); // This read gyro data |
krmreynolds | 1:3272ece36ce1 | 66 | read_accel(); // Read I2C accelerometer |
krmreynolds | 1:3272ece36ce1 | 67 | |
krmreynolds | 1:3272ece36ce1 | 68 | if (counter > 5) { // Read compass data at 10Hz... (5 loop runs) |
krmreynolds | 1:3272ece36ce1 | 69 | counter=0; |
krmreynolds | 1:3272ece36ce1 | 70 | read_compass(); // Read I2C magnetometer |
krmreynolds | 1:3272ece36ce1 | 71 | compass_heading(); // Calculate magnetic heading |
krmreynolds | 1:3272ece36ce1 | 72 | } |
krmreynolds | 1:3272ece36ce1 | 73 | |
krmreynolds | 1:3272ece36ce1 | 74 | // Calculations... |
krmreynolds | 1:3272ece36ce1 | 75 | matrix_update(); |
krmreynolds | 1:3272ece36ce1 | 76 | normalize(); |
krmreynolds | 1:3272ece36ce1 | 77 | drift_correction(); |
krmreynolds | 1:3272ece36ce1 | 78 | euler_angles(); |
krmreynolds | 1:3272ece36ce1 | 79 | // *** |
krmreynolds | 1:3272ece36ce1 | 80 | |
krmreynolds | 1:3272ece36ce1 | 81 | print_data(); |
krmreynolds | 1:3272ece36ce1 | 82 | return true; |
krmreynolds | 1:3272ece36ce1 | 83 | } |
krmreynolds | 1:3272ece36ce1 | 84 | return false; |
krmreynolds | 1:3272ece36ce1 | 85 | } |
krmreynolds | 1:3272ece36ce1 | 86 | |
krmreynolds | 1:3272ece36ce1 | 87 | void minimu9::read_gyro() { |
krmreynolds | 1:3272ece36ce1 | 88 | gyro->read(); |
krmreynolds | 1:3272ece36ce1 | 89 | |
krmreynolds | 1:3272ece36ce1 | 90 | AN[0] = gyro->g.x; |
krmreynolds | 1:3272ece36ce1 | 91 | AN[1] = gyro->g.y; |
krmreynolds | 1:3272ece36ce1 | 92 | AN[2] = gyro->g.z; |
krmreynolds | 1:3272ece36ce1 | 93 | gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]); |
krmreynolds | 1:3272ece36ce1 | 94 | gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]); |
krmreynolds | 1:3272ece36ce1 | 95 | gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]); |
krmreynolds | 1:3272ece36ce1 | 96 | } |
krmreynolds | 1:3272ece36ce1 | 97 | |
krmreynolds | 1:3272ece36ce1 | 98 | |
krmreynolds | 1:3272ece36ce1 | 99 | // Reads x,y and z accelerometer registers |
krmreynolds | 1:3272ece36ce1 | 100 | void minimu9::read_accel() { |
krmreynolds | 1:3272ece36ce1 | 101 | compass->readAcc(); |
krmreynolds | 1:3272ece36ce1 | 102 | |
krmreynolds | 1:3272ece36ce1 | 103 | AN[3] = compass->a.x; |
krmreynolds | 1:3272ece36ce1 | 104 | AN[4] = compass->a.y; |
krmreynolds | 1:3272ece36ce1 | 105 | AN[5] = compass->a.z; |
krmreynolds | 1:3272ece36ce1 | 106 | accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]); |
krmreynolds | 1:3272ece36ce1 | 107 | accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]); |
krmreynolds | 1:3272ece36ce1 | 108 | accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]); |
krmreynolds | 1:3272ece36ce1 | 109 | } |
krmreynolds | 1:3272ece36ce1 | 110 | |
krmreynolds | 0:dc35364e2291 | 111 | |
krmreynolds | 1:3272ece36ce1 | 112 | void minimu9::read_compass() { |
krmreynolds | 1:3272ece36ce1 | 113 | compass->readMag(); |
krmreynolds | 1:3272ece36ce1 | 114 | |
krmreynolds | 1:3272ece36ce1 | 115 | magnetom_x = SENSOR_SIGN[6] * compass->m.x; |
krmreynolds | 1:3272ece36ce1 | 116 | magnetom_y = SENSOR_SIGN[7] * compass->m.y; |
krmreynolds | 1:3272ece36ce1 | 117 | magnetom_z = SENSOR_SIGN[8] * compass->m.z; |
krmreynolds | 1:3272ece36ce1 | 118 | } |
krmreynolds | 1:3272ece36ce1 | 119 | |
krmreynolds | 1:3272ece36ce1 | 120 | void minimu9::compass_heading() { |
krmreynolds | 1:3272ece36ce1 | 121 | float MAG_X; |
krmreynolds | 1:3272ece36ce1 | 122 | float MAG_Y; |
krmreynolds | 1:3272ece36ce1 | 123 | float cos_roll; |
krmreynolds | 1:3272ece36ce1 | 124 | float sin_roll; |
krmreynolds | 1:3272ece36ce1 | 125 | float cos_pitch; |
krmreynolds | 1:3272ece36ce1 | 126 | float sin_pitch; |
krmreynolds | 1:3272ece36ce1 | 127 | |
krmreynolds | 1:3272ece36ce1 | 128 | cos_roll = cos(roll); |
krmreynolds | 1:3272ece36ce1 | 129 | sin_roll = sin(roll); |
krmreynolds | 1:3272ece36ce1 | 130 | cos_pitch = cos(pitch); |
krmreynolds | 1:3272ece36ce1 | 131 | sin_pitch = sin(pitch); |
krmreynolds | 1:3272ece36ce1 | 132 | |
krmreynolds | 1:3272ece36ce1 | 133 | // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range |
krmreynolds | 1:3272ece36ce1 | 134 | c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.5; |
krmreynolds | 1:3272ece36ce1 | 135 | c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*0.5; |
krmreynolds | 1:3272ece36ce1 | 136 | c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*0.5; |
krmreynolds | 1:3272ece36ce1 | 137 | |
krmreynolds | 1:3272ece36ce1 | 138 | // Tilt compensated Magnetic filed X: |
krmreynolds | 1:3272ece36ce1 | 139 | MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch; |
krmreynolds | 1:3272ece36ce1 | 140 | // Tilt compensated Magnetic filed Y: |
krmreynolds | 1:3272ece36ce1 | 141 | MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll; |
krmreynolds | 1:3272ece36ce1 | 142 | // Magnetic Heading |
krmreynolds | 1:3272ece36ce1 | 143 | MAG_Heading = atan2(-MAG_Y,MAG_X); |
krmreynolds | 1:3272ece36ce1 | 144 | } |
krmreynolds | 0:dc35364e2291 | 145 | |
krmreynolds | 1:3272ece36ce1 | 146 | void minimu9::normalize(void) { |
krmreynolds | 1:3272ece36ce1 | 147 | float error=0; |
krmreynolds | 1:3272ece36ce1 | 148 | float temporary[3][3]; |
krmreynolds | 1:3272ece36ce1 | 149 | float renorm=0; |
krmreynolds | 1:3272ece36ce1 | 150 | |
krmreynolds | 1:3272ece36ce1 | 151 | error= -vector_dot_product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 |
krmreynolds | 1:3272ece36ce1 | 152 | |
krmreynolds | 1:3272ece36ce1 | 153 | vector_scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 |
krmreynolds | 1:3272ece36ce1 | 154 | vector_scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 |
krmreynolds | 1:3272ece36ce1 | 155 | |
krmreynolds | 1:3272ece36ce1 | 156 | vector_add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 |
krmreynolds | 1:3272ece36ce1 | 157 | vector_add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 |
krmreynolds | 1:3272ece36ce1 | 158 | |
krmreynolds | 1:3272ece36ce1 | 159 | vector_cross_product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 |
krmreynolds | 1:3272ece36ce1 | 160 | |
krmreynolds | 1:3272ece36ce1 | 161 | renorm= .5 *(3 - vector_dot_product(&temporary[0][0],&temporary[0][0])); //eq.21 |
krmreynolds | 1:3272ece36ce1 | 162 | vector_scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); |
krmreynolds | 1:3272ece36ce1 | 163 | |
krmreynolds | 1:3272ece36ce1 | 164 | renorm= .5 *(3 - vector_dot_product(&temporary[1][0],&temporary[1][0])); //eq.21 |
krmreynolds | 1:3272ece36ce1 | 165 | vector_scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); |
krmreynolds | 1:3272ece36ce1 | 166 | |
krmreynolds | 1:3272ece36ce1 | 167 | renorm= .5 *(3 - vector_dot_product(&temporary[2][0],&temporary[2][0])); //eq.21 |
krmreynolds | 1:3272ece36ce1 | 168 | vector_scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); |
krmreynolds | 1:3272ece36ce1 | 169 | } |
krmreynolds | 1:3272ece36ce1 | 170 | |
krmreynolds | 1:3272ece36ce1 | 171 | /**************************************************/ |
krmreynolds | 1:3272ece36ce1 | 172 | void minimu9::drift_correction(void) { |
krmreynolds | 1:3272ece36ce1 | 173 | float mag_heading_x; |
krmreynolds | 1:3272ece36ce1 | 174 | float mag_heading_y; |
krmreynolds | 1:3272ece36ce1 | 175 | float errorCourse; |
krmreynolds | 1:3272ece36ce1 | 176 | //Compensation the Roll, Pitch and Yaw drift. |
krmreynolds | 1:3272ece36ce1 | 177 | static float Scaled_Omega_P[3]; |
krmreynolds | 1:3272ece36ce1 | 178 | static float Scaled_Omega_I[3]; |
krmreynolds | 1:3272ece36ce1 | 179 | float Accel_magnitude; |
krmreynolds | 1:3272ece36ce1 | 180 | float Accel_weight; |
krmreynolds | 1:3272ece36ce1 | 181 | |
krmreynolds | 1:3272ece36ce1 | 182 | |
krmreynolds | 1:3272ece36ce1 | 183 | //*****Roll and Pitch*************** |
krmreynolds | 0:dc35364e2291 | 184 | |
krmreynolds | 1:3272ece36ce1 | 185 | // Calculate the magnitude of the accelerometer vector |
krmreynolds | 1:3272ece36ce1 | 186 | Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); |
krmreynolds | 1:3272ece36ce1 | 187 | Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. |
krmreynolds | 1:3272ece36ce1 | 188 | // Dynamic weighting of accelerometer info (reliability filter) |
krmreynolds | 1:3272ece36ce1 | 189 | // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) |
krmreynolds | 1:3272ece36ce1 | 190 | Accel_weight = 1 - 2*abs(1 - Accel_magnitude),0,1; // |
krmreynolds | 1:3272ece36ce1 | 191 | if( 1 < Accel_weight ) { |
krmreynolds | 1:3272ece36ce1 | 192 | Accel_weight = 1; |
krmreynolds | 1:3272ece36ce1 | 193 | } |
krmreynolds | 1:3272ece36ce1 | 194 | else if( 0 > Accel_weight ) { |
krmreynolds | 1:3272ece36ce1 | 195 | Accel_weight = 0; |
krmreynolds | 1:3272ece36ce1 | 196 | } |
krmreynolds | 1:3272ece36ce1 | 197 | |
krmreynolds | 1:3272ece36ce1 | 198 | vector_cross_product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference |
krmreynolds | 1:3272ece36ce1 | 199 | vector_scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); |
krmreynolds | 1:3272ece36ce1 | 200 | |
krmreynolds | 1:3272ece36ce1 | 201 | vector_scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); |
krmreynolds | 1:3272ece36ce1 | 202 | vector_add(Omega_I,Omega_I,Scaled_Omega_I); |
krmreynolds | 1:3272ece36ce1 | 203 | |
krmreynolds | 1:3272ece36ce1 | 204 | //*****YAW*************** |
krmreynolds | 1:3272ece36ce1 | 205 | // We make the gyro YAW drift correction based on compass magnetic heading |
krmreynolds | 0:dc35364e2291 | 206 | |
krmreynolds | 1:3272ece36ce1 | 207 | mag_heading_x = cos(MAG_Heading); |
krmreynolds | 1:3272ece36ce1 | 208 | mag_heading_y = sin(MAG_Heading); |
krmreynolds | 1:3272ece36ce1 | 209 | errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error |
krmreynolds | 1:3272ece36ce1 | 210 | vector_scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. |
krmreynolds | 1:3272ece36ce1 | 211 | |
krmreynolds | 1:3272ece36ce1 | 212 | vector_scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW. |
krmreynolds | 1:3272ece36ce1 | 213 | vector_add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. |
krmreynolds | 1:3272ece36ce1 | 214 | |
krmreynolds | 1:3272ece36ce1 | 215 | vector_scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator |
krmreynolds | 1:3272ece36ce1 | 216 | vector_add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I |
krmreynolds | 1:3272ece36ce1 | 217 | } |
krmreynolds | 1:3272ece36ce1 | 218 | |
krmreynolds | 1:3272ece36ce1 | 219 | |
krmreynolds | 1:3272ece36ce1 | 220 | void minimu9::matrix_update(void) { |
krmreynolds | 1:3272ece36ce1 | 221 | Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll |
krmreynolds | 1:3272ece36ce1 | 222 | Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch |
krmreynolds | 1:3272ece36ce1 | 223 | Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw |
krmreynolds | 1:3272ece36ce1 | 224 | |
krmreynolds | 1:3272ece36ce1 | 225 | Accel_Vector[0]=accel_x; |
krmreynolds | 1:3272ece36ce1 | 226 | Accel_Vector[1]=accel_y; |
krmreynolds | 1:3272ece36ce1 | 227 | Accel_Vector[2]=accel_z; |
krmreynolds | 1:3272ece36ce1 | 228 | |
krmreynolds | 1:3272ece36ce1 | 229 | vector_add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term |
krmreynolds | 1:3272ece36ce1 | 230 | vector_add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term |
krmreynolds | 0:dc35364e2291 | 231 | |
krmreynolds | 1:3272ece36ce1 | 232 | #if OUTPUTMODE==1 |
krmreynolds | 1:3272ece36ce1 | 233 | Update_Matrix[0][0]=0; |
krmreynolds | 1:3272ece36ce1 | 234 | Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z |
krmreynolds | 1:3272ece36ce1 | 235 | Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y |
krmreynolds | 1:3272ece36ce1 | 236 | Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z |
krmreynolds | 1:3272ece36ce1 | 237 | Update_Matrix[1][1]=0; |
krmreynolds | 1:3272ece36ce1 | 238 | Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x |
krmreynolds | 1:3272ece36ce1 | 239 | Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y |
krmreynolds | 1:3272ece36ce1 | 240 | Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x |
krmreynolds | 1:3272ece36ce1 | 241 | Update_Matrix[2][2]=0; |
krmreynolds | 1:3272ece36ce1 | 242 | #else // Uncorrected data (no drift correction) |
krmreynolds | 1:3272ece36ce1 | 243 | Update_Matrix[0][0]=0; |
krmreynolds | 1:3272ece36ce1 | 244 | Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z |
krmreynolds | 1:3272ece36ce1 | 245 | Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y |
krmreynolds | 1:3272ece36ce1 | 246 | Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z |
krmreynolds | 1:3272ece36ce1 | 247 | Update_Matrix[1][1]=0; |
krmreynolds | 1:3272ece36ce1 | 248 | Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; |
krmreynolds | 1:3272ece36ce1 | 249 | Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; |
krmreynolds | 1:3272ece36ce1 | 250 | Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; |
krmreynolds | 1:3272ece36ce1 | 251 | Update_Matrix[2][2]=0; |
krmreynolds | 0:dc35364e2291 | 252 | #endif |
krmreynolds | 1:3272ece36ce1 | 253 | |
krmreynolds | 1:3272ece36ce1 | 254 | matrix_multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c |
krmreynolds | 1:3272ece36ce1 | 255 | |
krmreynolds | 1:3272ece36ce1 | 256 | for (int x=0; x<3; x++) { //Matrix Addition (update) |
krmreynolds | 1:3272ece36ce1 | 257 | for (int y=0; y<3; y++) { |
krmreynolds | 1:3272ece36ce1 | 258 | DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; |
krmreynolds | 1:3272ece36ce1 | 259 | } |
krmreynolds | 1:3272ece36ce1 | 260 | } |
krmreynolds | 1:3272ece36ce1 | 261 | } |
krmreynolds | 0:dc35364e2291 | 262 | |
krmreynolds | 1:3272ece36ce1 | 263 | void minimu9::euler_angles(void) { |
krmreynolds | 1:3272ece36ce1 | 264 | pitch = -asin(DCM_Matrix[2][0]); |
krmreynolds | 1:3272ece36ce1 | 265 | roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); |
krmreynolds | 1:3272ece36ce1 | 266 | yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); |
krmreynolds | 1:3272ece36ce1 | 267 | } |
krmreynolds | 1:3272ece36ce1 | 268 | |
krmreynolds | 1:3272ece36ce1 | 269 | |
krmreynolds | 1:3272ece36ce1 | 270 | float minimu9::get_roll( void ) { |
krmreynolds | 1:3272ece36ce1 | 271 | return ToDeg( roll ); |
krmreynolds | 1:3272ece36ce1 | 272 | } |
krmreynolds | 1:3272ece36ce1 | 273 | float minimu9::get_pitch( void ) { |
krmreynolds | 1:3272ece36ce1 | 274 | return ToDeg( pitch ); |
krmreynolds | 1:3272ece36ce1 | 275 | } |
krmreynolds | 1:3272ece36ce1 | 276 | float minimu9::get_yaw( void ) { |
krmreynolds | 1:3272ece36ce1 | 277 | return ToDeg( yaw ); |
krmreynolds | 1:3272ece36ce1 | 278 | } |
krmreynolds | 1:3272ece36ce1 | 279 | |
krmreynolds | 1:3272ece36ce1 | 280 | |
krmreynolds | 1:3272ece36ce1 | 281 | void minimu9::initialize_values( void ) { |
krmreynolds | 0:dc35364e2291 | 282 | G_Dt=0.02; |
krmreynolds | 0:dc35364e2291 | 283 | timer=0; |
krmreynolds | 0:dc35364e2291 | 284 | timer24=0; |
krmreynolds | 0:dc35364e2291 | 285 | counter=0; |
krmreynolds | 0:dc35364e2291 | 286 | gyro_sat=0; |
krmreynolds | 0:dc35364e2291 | 287 | |
krmreynolds | 0:dc35364e2291 | 288 | memcpy(SENSOR_SIGN, (int [9]) { |
krmreynolds | 1:3272ece36ce1 | 289 | 1,-1,-1,-1,1,1,1,-1,-1 |
krmreynolds | 1:3272ece36ce1 | 290 | //1,1,1,-1,-1,-1,1,1,1 |
krmreynolds | 0:dc35364e2291 | 291 | }, 9*sizeof(int)); |
krmreynolds | 0:dc35364e2291 | 292 | memcpy(AN_OFFSET, (int [6]) { |
krmreynolds | 0:dc35364e2291 | 293 | 0,0,0,0,0,0 |
krmreynolds | 0:dc35364e2291 | 294 | }, 6*sizeof(int)); |
krmreynolds | 0:dc35364e2291 | 295 | memcpy(Accel_Vector, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 296 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 297 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 298 | memcpy(Gyro_Vector, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 299 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 300 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 301 | memcpy(Omega_Vector, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 302 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 303 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 304 | memcpy(Omega_P, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 305 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 306 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 307 | memcpy(Omega_I, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 308 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 309 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 310 | memcpy(Omega, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 311 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 312 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 313 | memcpy(errorRollPitch, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 314 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 315 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 316 | memcpy(errorYaw, (float [3]) { |
krmreynolds | 0:dc35364e2291 | 317 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 318 | }, 3*sizeof(float)); |
krmreynolds | 0:dc35364e2291 | 319 | memcpy( DCM_Matrix, (float[3][3]) { |
krmreynolds | 0:dc35364e2291 | 320 | { |
krmreynolds | 0:dc35364e2291 | 321 | 1,0,0 |
krmreynolds | 0:dc35364e2291 | 322 | }, { 0,1,0 }, { 0,0,1 } |
krmreynolds | 0:dc35364e2291 | 323 | }, 9*sizeof(float) ); |
krmreynolds | 0:dc35364e2291 | 324 | memcpy( Update_Matrix, (float[3][3]) { |
krmreynolds | 0:dc35364e2291 | 325 | { |
krmreynolds | 0:dc35364e2291 | 326 | 0,1,2 |
krmreynolds | 0:dc35364e2291 | 327 | }, { 3,4,5 }, { 6,7,8 } |
krmreynolds | 0:dc35364e2291 | 328 | }, 9*sizeof(float) ); |
krmreynolds | 0:dc35364e2291 | 329 | memcpy( Temporary_Matrix, (float[3][3]) { |
krmreynolds | 0:dc35364e2291 | 330 | { |
krmreynolds | 0:dc35364e2291 | 331 | 0,0,0 |
krmreynolds | 0:dc35364e2291 | 332 | }, { 0,0,0 }, { 0,0,0 } |
krmreynolds | 0:dc35364e2291 | 333 | }, 9*sizeof(float) ); |
krmreynolds | 0:dc35364e2291 | 334 | } |
krmreynolds | 0:dc35364e2291 | 335 | |
krmreynolds | 0:dc35364e2291 | 336 | void minimu9::print_data(void) { |
krmreynolds | 1:3272ece36ce1 | 337 | #if DEBUG_MODE == 1 |
krmreynolds | 1:3272ece36ce1 | 338 | Serial pc(USBTX, USBRX); |
krmreynolds | 1:3272ece36ce1 | 339 | pc.printf("!"); |
krmreynolds | 0:dc35364e2291 | 340 | |
krmreynolds | 0:dc35364e2291 | 341 | #if PRINT_EULER == 1 |
krmreynolds | 1:3272ece36ce1 | 342 | pc.printf("ANG:%.2f,%.2f,%.2f", ToDeg(roll), ToDeg(pitch), ToDeg(yaw)); |
krmreynolds | 0:dc35364e2291 | 343 | #endif |
krmreynolds | 0:dc35364e2291 | 344 | #if PRINT_ANALOGS==1 |
krmreynolds | 1:3272ece36ce1 | 345 | pc.printf(",AN:%d,%d,%d,%d,%d,%d", AN[0], AN[1], AN[2], AN[3], AN[4], AN[5] ); |
krmreynolds | 1:3272ece36ce1 | 346 | pc.printf(",%.2f,%.2f,%.2f", c_magnetom_x, c_magnetom_y, c_magnetom_z ); |
krmreynolds | 1:3272ece36ce1 | 347 | #endif |
krmreynolds | 1:3272ece36ce1 | 348 | pc.printf("\n"); |
krmreynolds | 0:dc35364e2291 | 349 | #endif |
krmreynolds | 0:dc35364e2291 | 350 | |
krmreynolds | 0:dc35364e2291 | 351 | } |