A Team / DCM_AHRS

DCM Code ported from Arduino for FRDM-KL25Z

Dependents:   minimu_data_capture minimu_data_capture

Fork of DCM_AHRS by Kris Reynolds

minimu9.cpp@2:85214374e094, 2015-01-20 (annotated)

Committer:
ogarai
Date:
Tue Jan 20 02:04:07 2015 +0000
Revision:
2:85214374e094
Parent:
1:3272ece36ce1 
First Commit

Who changed what in which revision?

UserRevisionLine numberNew 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();
ogarai 2:85214374e094 12 gyro = new L3G4200D( PTE0, PTE1 );
ogarai 2:85214374e094 13 compass = new LSM303( PTE0, PTE1 );
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
ogarai 2:85214374e094 338 // Serial pc(USBTX, USBRX);
ogarai 2:85214374e094 339 printf("!");
krmreynolds 0:dc35364e2291 340
krmreynolds 0:dc35364e2291 341 #if PRINT_EULER == 1
ogarai 2:85214374e094 342 printf("ANG:%.2f,%.2f,%.2f", ToDeg(roll), ToDeg(pitch), ToDeg(yaw));
krmreynolds 0:dc35364e2291 343 #endif
krmreynolds 0:dc35364e2291 344 #if PRINT_ANALOGS==1
ogarai 2:85214374e094 345 printf(",AN:%d,%d,%d,%d,%d,%d", AN[0], AN[1], AN[2], AN[3], AN[4], AN[5] );
ogarai 2:85214374e094 346 printf(",%.2f,%.2f,%.2f", c_magnetom_x, c_magnetom_y, c_magnetom_z );
krmreynolds 1:3272ece36ce1 347 #endif
ogarai 2:85214374e094 348 printf("\n");
krmreynolds 0:dc35364e2291 349 #endif
krmreynolds 0:dc35364e2291 350
krmreynolds 0:dc35364e2291 351 }