sakthi priya amirtharaj
acs bea hk together
Fork of
BAE_vr3honeycomb1_christmas
by 
green rosh
Embed:
(wiki syntax)
Show/hide line numbers
ACS.cpp
00001 #include "ACS.h" 00002 #include "MPU3300.h" 00003 #include "pin_config.h" 00004 00005 //PwmOut PWM1(PTD4); //Functions used to generate PWM signal 00006 //PWM output comes from pins p6 00007 Serial pc1(USBTX, USBRX); 00008 SPI spi_acs (PIN16, PIN17, PIN15); // mosi, miso, sclk PTE18,19,17 00009 DigitalOut SSN_MAG (PTC16); // ssn for magnetometer PTB11 00010 DigitalInOut DRDY (PTE3); // drdy for magnetometer PTA17 00011 DigitalOut ssn_gyr (PTE2); //Slave Select pin of gyroscope PTB16 00012 InterruptIn dr(PTC6); //Interrupt pin for gyro PTC5 00013 PwmOut PWM1(D2); //Functions used to generate PWM signal 00014 PwmOut PWM2(D3); 00015 PwmOut PWM3(D4); //PWM output comes from pins p6 00016 Ticker tr; //Ticker function to give values for limited amount of time for gyro 00017 Timeout tr_mag; 00018 uint8_t trflag_mag; 00019 uint8_t trFlag; //ticker Flag for gyro 00020 uint8_t drFlag; //data-ready interrupt flag for gyro 00021 00022 //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------// 00023 00024 void FUNC_ACS_GENPWM(float M[3]) 00025 { 00026 00027 00028 printf("\nEnterd PWMGEN function\n"); 00029 float DCx = 0; //Duty cycle of Moment in x, y, z directions 00030 float ix = 0; //Current sent in x, y, z TR's 00031 float timep = 0.02 ; 00032 float Mx=M[0]; //Time period is set to 0.02s 00033 //Moment in x, y, z directions 00034 00035 00036 00037 ix = Mx * 0.3 ; //Moment and Current always have the linear relationship 00038 00039 if( ix>0&& ix < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% 00040 { 00041 DCx = 6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008; 00042 PWM1.period(timep); 00043 PWM1 = DCx/100 ; 00044 } 00045 else if( ix >= 0.006&& ix < 0.0116) 00046 { 00047 DCx = 1*100000000*pow(ix,4) - 5*1000000*pow(ix,3) + 62603*pow(ix,2) - 199.29*ix + 0.7648; 00048 PWM1.period(timep); 00049 PWM1 = DCx/100 ; 00050 } 00051 else if (ix >= 0.0116&& ix < 0.0624) 00052 { 00053 00054 DCx = 212444*pow(ix,4) - 33244*pow(ix,3) + 1778.4*pow(ix,2) + 120.91*ix + 0.3878; 00055 PWM1.period(timep); 00056 PWM1 = DCx/100 ; 00057 } 00058 else if(ix >= 0.0624&& ix < 0.555) 00059 { 00060 printf("\nACS entered if\n"); 00061 DCx = 331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338; 00062 PWM1.period(timep); 00063 PWM1 = DCx/100 ; 00064 } 00065 else if(ix==0) 00066 { 00067 DCx = 50; 00068 PWM1.period(timep); 00069 PWM1 = DCx/100 ; 00070 } 00071 else 00072 { 00073 // printf("!!!!!!!!!!Error!!!!!!!!!"); 00074 } 00075 00076 printf("\n moment :%f\n",DCx); 00077 float DCy = 0; //Duty cycle of Moment in x, y, z directions 00078 float iy = 0; //Current sent in x, y, z TR's 00079 00080 float My=M[1]; //Time period is set to 0.2s 00081 //Moment in x, y, z directions 00082 00083 00084 iy = My * 0.3 ; //Moment and Current always have the linear relationship 00085 00086 if( iy>0&& iy < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% 00087 { 00088 DCy = 6*1000000*pow(iy,4) - 377291*pow(iy,3) + 4689.6*pow(iy,2) + 149.19*iy - 0.0008; 00089 PWM2.period(timep); 00090 PWM2 = DCy/100 ; 00091 } 00092 else if( iy >= 0.006&& iy < 0.0116) 00093 { 00094 DCy = 1*100000000*pow(iy,4) - 5*1000000*pow(iy,3) + 62603*pow(iy,2) - 199.29*iy + 0.7648; 00095 PWM2.period(timep); 00096 PWM2 = DCy/100 ; 00097 } 00098 else if (iy >= 0.0116&& iy < 0.0624) 00099 { 00100 00101 DCy = 212444*pow(iy,4) - 33244*pow(iy,3) + 1778.4*pow(iy,2) + 120.91*iy + 0.3878; 00102 PWM2.period(timep); 00103 PWM2 = DCy/100 ; 00104 } 00105 else if(iy >= 0.0624&& iy < 0.555) 00106 { 00107 printf("\nACS entered if\n"); 00108 DCy = 331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338; 00109 PWM2.period(timep); 00110 PWM2 = DCy/100 ; 00111 } 00112 else if(iy==0) 00113 { 00114 DCy = 0; 00115 PWM2.period(timep); 00116 PWM2 = DCy/100 ; 00117 } 00118 else 00119 { 00120 // printf("!!!!!!!!!!Error!!!!!!!!!"); 00121 } 00122 float DCz = 0; //Duty cycle of Moment in x, y, z directions 00123 float iz = 0; //Current sent in x, y, z TR's 00124 00125 float Mz=M[2]; //Time period is set to 0.2s 00126 //Moment in x, y, z directions 00127 00128 00129 iz = Mz * 0.3 ; //Moment and Current always have the linear relationship 00130 00131 if( iz>0&& iz < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% 00132 { 00133 DCz = 6*1000000*pow(iz,4) - 377291*pow(iz,3) + 4689.6*pow(iz,2) + 149.19*iz - 0.0008; 00134 PWM3.period(timep); 00135 PWM3 = DCz/100 ; 00136 } 00137 else if( iz >= 0.006&& iz < 0.0116) 00138 { 00139 DCz = 1*100000000*pow(iz,4) - 5*1000000*pow(iz,3) + 62603*pow(iz,2) - 199.29*iz + 0.7648; 00140 PWM3.period(timep); 00141 PWM3 = DCz/100 ; 00142 } 00143 else if (iz >= 0.0116&& iz < 0.0624) 00144 { 00145 00146 DCz = 212444*pow(iz,4) - 33244*pow(iz,3) + 1778.4*pow(iz,2) + 120.91*iz + 0.3878; 00147 PWM3.period(timep); 00148 PWM3 = DCz/100 ; 00149 } 00150 else if(iz >= 0.0624&& iz < 0.555) 00151 { 00152 printf("\nACS entered if\n"); 00153 DCz = 331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338; 00154 PWM3.period(timep); 00155 PWM3 = DCz/100 ; 00156 } 00157 else if(iz==0) 00158 { 00159 DCz = 0; 00160 PWM3.period(timep); 00161 PWM3 = DCz/100 ; 00162 } 00163 else 00164 { 00165 // printf("!!!!!!!!!!Error!!!!!!!!!"); 00166 } 00167 00168 printf("\nExited PWMGEN function\n"); 00169 } 00170 /*------------------------------------------------------------------------------------------------------------------------------------------------------- 00171 -------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/ 00172 00173 void trsub_mag() 00174 { 00175 trflag_mag=0; 00176 } 00177 00178 void FUNC_ACS_MAG_INIT() 00179 { 00180 //DRDY.output(); 00181 DRDY = 0; 00182 int a ; 00183 a=DRDY; 00184 printf("\n DRDY is %d\n",a); 00185 SSN_MAG=1; //pin is disabled 00186 spi_acs.format(8,0); // 8bits,Mode 0 00187 spi_acs.frequency(100000); //clock frequency 00188 00189 SSN_MAG=0; // Selecting pin 00190 wait_ms(10); //accounts for delay.can be minimised. 00191 00192 spi_acs.write(0x83); // 00193 00194 wait_ms(10); 00195 00196 unsigned char i; 00197 for(i=0;i<3;i++)//initialising values. 00198 { 00199 spi_acs.write(0x00); //MSB of X,y,Z 00200 spi_acs.write(0xc8); //LSB of X,Y,z;pointer increases automatically. 00201 } 00202 SSN_MAG=1; 00203 00204 } 00205 00206 float* FUNC_ACS_MAG_EXEC() 00207 { 00208 //printf("\nEntered magnetometer function\n"); 00209 //DRDY.output(); 00210 DRDY.write(0); 00211 int a; 00212 a = DRDY; 00213 printf("\n DRDY is %d\n",a); 00214 SSN_MAG=0; //enabling slave to measure the values 00215 wait_ms(10); 00216 spi_acs.write(0x82); //initiates measurement 00217 wait_ms(10); 00218 spi_acs.write(0x01); //selecting x,y and z axes, measurement starts now 00219 SSN_MAG=1; 00220 wait_ms(10); 00221 00222 trflag_mag=1; 00223 tr_mag.attach(&trsub_mag,1); //runs in background,makes trflag_mag=0 after 1s 00224 DRDY.input(); 00225 while(trflag_mag) /*initially flag is 1,so loop is executed,if DRDY is high,then data is retrieved and programme ends,else 00226 loop runs for at the max 1s and if still DRDY is zero,the flag becomes 0 and loop is not executed and 00227 programme is terminated*/ 00228 { 00229 wait_ms(5); 00230 if(DRDY==1) 00231 { 00232 printf("\nwth\n"); 00233 SSN_MAG=0; 00234 spi_acs.write(0xc9); //command byte for retrieving data 00235 00236 unsigned char axis; 00237 float Bnewvalue[3]={0.0,0.0,0.0}; 00238 int32_t Bvalue[3]={0,0,0}; 00239 int32_t a= pow(2.0,24.0); 00240 int32_t b= pow(2.0,23.0); 00241 00242 for(axis=0;axis<3;axis++) 00243 { 00244 Bvalue[axis]=spi_acs.write(0x00)<<16; //MSB 1 is send first 00245 wait_ms(10); 00246 Bvalue[axis]|=spi_acs.write(0x00)<<8; //MSB 2 is send next 00247 wait_ms(10); 00248 Bvalue[axis]|=spi_acs.write(0x00); //LSB is send.....total length is 24 bits(3*8bits)...which are appended to get actual bit configuration 00249 00250 00251 if((Bvalue[axis]&b)==b) 00252 { 00253 Bvalue[axis]=Bvalue[axis]-a; //converting 2s complement to signed decimal 00254 00255 } 00256 Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0); //1 LSB=(22nT)...final value of field obtained in micro tesla 00257 00258 wait_ms(10); 00259 printf("\t%lf\n",Bnewvalue[axis]); 00260 00261 } 00262 SSN_MAG=1; 00263 /* for test only to removed */ 00264 Bnewvalue[0]=Bnewvalue[1]=Bnewvalue[2]=100; 00265 return Bnewvalue; //return here? doubt.. 00266 break; 00267 } 00268 00269 } 00270 00271 } 00272 /*------------------------------------------------------------------------------------------------------------------------------------------------------ 00273 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/ 00274 00275 float * FUNC_ACS_CNTRLALGO(float b[3],float omega[3]) 00276 { 00277 float db[3]; /// inputs 00278 //initialization 00279 float bb[3] = {0, 0, 0}; 00280 float d[3] = {0, 0, 0}; 00281 float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}}; 00282 float den = 0; 00283 float den2; 00284 int i, j; //temporary variables 00285 float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs 00286 float invJm[3][3]; 00287 float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4; 00288 printf("Entered cntrl algo\n"); 00289 //structure parameters 00290 00291 void inverse (float mat[3][3], float inv[3][3]); 00292 void getInput (float x[9]); 00293 //functions 00294 00295 ////////// Input from Matlab ////////////// 00296 while(1) 00297 { 00298 00299 /*getInput(inputs); 00300 //while(1) 00301 b[0] = inputs[0]; 00302 b[1] = inputs[1]; 00303 b[2] = inputs[2]; 00304 db[0] = inputs[3]; 00305 db[1] = inputs[4]; 00306 db[2] = inputs[5]; 00307 omega[0] = inputs[6]; 00308 omega[1] = inputs[7]; 00309 omega[2] = inputs[8];*/ 00310 /////////// Control Algorithm ////////////////////// 00311 // calculate norm b, norm db 00312 den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2])); 00313 den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]); 00314 00315 for(i=0;i<3;i++) 00316 { 00317 db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3)); 00318 //db[i]/=den*den*den; 00319 } 00320 00321 for(i=0;i<3;i++) 00322 { 00323 printf("\nreached here\n"); 00324 if(den!=0) 00325 //b[i]=b[i]/den; //there is a problem here. The code gets stuck here. Maf value is required 00326 ; 00327 } 00328 00329 // select kz, kmu, gamma 00330 if(b[0]>0.9 || b[0]<-0.9) 00331 { 00332 kz = kz2; 00333 kmu = kmu2; 00334 gamma = gamma2; 00335 } 00336 // calculate Mu, v, dv, z, u 00337 for(i=0;i<2;i++) 00338 { 00339 Mu[i] = b[i+1]; 00340 v[i] = -kmu*Mu[i]; 00341 dv[i] = -kmu*db[i+1]; 00342 z[i] = db[i+1] - v[i]; 00343 u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma); 00344 } 00345 inverse(Jm, invJm); 00346 // calculate cross(omega,J*omega)for(i=0;i<3;i++) 00347 00348 for(j=0;j<3;j++) 00349 bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]); 00350 00351 // calculate invJm*cross(omega,J*omega) store in d 00352 for(i=0;i<3;i++) 00353 { 00354 for(j=0;j<3;j++) 00355 d[i] += bb[j]*invJm[i][j]; 00356 } 00357 // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db) 00358 // bb =[0;u-d(2:3)] 00359 // store in bb 00360 bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]); 00361 bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]); 00362 bb[0] = 0; 00363 // calculate N 00364 // reusing invJm as N 00365 00366 for(i=0;i<3;i++) 00367 { 00368 d[i] = invJm[1][i]; 00369 invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i]; 00370 invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i]; 00371 invJm[0][i] = b[i]; 00372 } 00373 // calculate inv(N) store in Jm 00374 inverse(invJm, Jm); 00375 // calculate tauc 00376 for(i=0;i<3;i++) 00377 { 00378 for(j=0;j<3;j++) 00379 tauc[i] += Jm[i][j]*bb[j]; 00380 } 00381 00382 return(tauc); 00383 } 00384 } 00385 /////////// Output to Matlab ////////////////// 00386 /* for(i=0;i<3;i++) { 00387 printf("%f\n",tauc[i]*10000000); 00388 wait_ms(10); 00389 } 00390 } 00391 00392 }*/ 00393 void inverse(float mat[3][3], float inv[3][3]) 00394 { 00395 int i, j; 00396 float det = 0; 00397 for(i=0;i<3;i++) 00398 { for(j=0;j<3;j++) 00399 inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3] 00400 [(j+1)%3]*mat[(i+1)%3][(j+2)%3]); 00401 } 00402 det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]); 00403 for(i=0;i<3;i++) 00404 { for(j=0;j<3;j++) 00405 inv[i][j] /= det; 00406 } 00407 }/* 00408 void getInput (float x[9]) { 00409 //Functions used to generate PWM signal 00410 //PWM output comes from pins p6 00411 Serial pc1(USBTX, USBRX); 00412 char c[10]; 00413 char tempchar[8]; 00414 int i, j; 00415 //float f[9]; 00416 long n = 0; 00417 float flval = 0; 00418 for(j=0;j<9;j++) { 00419 for(i=0;i<9;i++) { 00420 c[i] = pc1.getc(); if(i<8) { 00421 tempchar[i] = c[i]; 00422 } 00423 } 00424 sscanf (tempchar, "%8x", &n); 00425 memcpy(&flval, &n, sizeof(long)); 00426 printf("%f\n", flval); 00427 x[j] = flval; 00428 } 00429 }*/ 00430 00431 void trSub(); 00432 void drSub(); 00433 void init_gyro(); 00434 float * FUNC_ACS_EXEC_GYR(); 00435 00436 void drSub() //In this function we setting data-ready flag to 1 00437 { 00438 drFlag=1; 00439 } 00440 void trSub() //In this function we are setting ticker flag to 0 00441 { 00442 trFlag=0; 00443 } 00444 void FUNC_ACS_INIT_GYR() 00445 { 00446 uint8_t response; 00447 ssn_gyr=1; //Deselecting the chip 00448 spi_acs.format(8,0); // Spi format is 8 bits, and clock mode 3 00449 spi_acs.frequency(1000000); //frequency to be set as 1MHz 00450 drFlag=0; //Intially defining data-ready flag to be 0 00451 dr.mode(PullDown); 00452 dr.rise(&drSub); 00453 __disable_irq(); 00454 00455 /*Following the above mentioned algorithm for initializing the register and changing its configuration*/ 00456 ssn_gyr=0; //Selecting chip(Mpu-3300) 00457 spi_acs.write(USER_CTRL|READFLAG); //sending USER_CTRL address with read bit 00458 response=spi_acs.write(DUMMYBIT); //sending dummy bit to get default values of the register 00459 00460 ssn_gyr=1; //Deselecting the chip 00461 wait(0.1); //waiting according the product specifications 00462 00463 ssn_gyr=0; //again selecting the chip 00464 spi_acs.write(USER_CTRL); //sending USER_CTRL address without read bit 00465 spi_acs.write(response|BIT_I2C_IF_DIS); //disabling the I2C mode in the register 00466 ssn_gyr=1; //deselecting the chip 00467 wait(0.1); // waiting for 100ms before going for another register 00468 00469 ssn_gyr=0; 00470 spi_acs.write(PWR_MGMT_1|READFLAG); //Power Management register-1 00471 response=spi_acs.write(DUMMYBIT); 00472 ssn_gyr=1; 00473 wait(0.1); 00474 00475 ssn_gyr=0; 00476 spi_acs.write(PWR_MGMT_1); 00477 response=spi_acs.write(response|BIT_CLKSEL_X); //Selecting the X axis gyroscope as clock as mentioned above 00478 ssn_gyr=1; 00479 wait(0.1); 00480 00481 ssn_gyr=0; 00482 spi_acs.write(GYRO_CONFIG|READFLAG); //sending GYRO_CONFIG address with read bit 00483 response=spi_acs.write(DUMMYBIT); 00484 ssn_gyr=1; 00485 wait(0.1); 00486 00487 ssn_gyr=0; 00488 spi_acs.write(GYRO_CONFIG); //sending GYRO_CONFIG address to write to register 00489 spi_acs.write(response&(~(BITS_FS_SEL_3|BITS_FS_SEL_4))); //selecting a full scale mode of +/=225 deg/sec 00490 ssn_gyr=1; 00491 wait(0.1); 00492 00493 ssn_gyr=0; 00494 spi_acs.write(CONFIG|READFLAG); //sending CONFIG address with read bit 00495 response=spi_acs.write(DUMMYBIT); 00496 ssn_gyr=1; 00497 wait(0.1); 00498 00499 ssn_gyr=0; 00500 spi_acs.write(CONFIG); //sending CONFIG address to write to register 00501 spi_acs.write(response|BITS_DLPF_CFG); //selecting a bandwidth of 42 hz and delay of 4.8ms 00502 ssn_gyr=1; 00503 wait(0.1); 00504 00505 ssn_gyr=0; 00506 spi_acs.write(SMPLRT_DIV|READFLAG); //sending SMPLRT_DIV address with read bit 00507 response=spi_acs.write(DUMMYBIT); 00508 ssn_gyr=1; 00509 wait(0.1); 00510 00511 ssn_gyr=0; 00512 spi_acs.write(SMPLRT_DIV); //sending SMPLRT_DIV address to write to register 00513 spi_acs.write(response&BITS_SMPLRT_DIV); //setting the sampling rate division to be 0 to make sample rate = gyroscopic output rate 00514 ssn_gyr=1; 00515 wait(0.1); 00516 00517 ssn_gyr=0; 00518 spi_acs.write(INT_ENABLE|READFLAG); //sending address of INT_ENABLE with readflag 00519 response=spi_acs.write(DUMMYBIT); //sending dummy byte to get the default values of the 00520 // regiser 00521 ssn_gyr=1; 00522 wait(0.1); 00523 00524 ssn_gyr=0; 00525 spi_acs.write(INT_ENABLE); //sending INT_ENABLE address to write to register 00526 spi_acs.write(response|BIT_DATA_RDY_ENABLE); //Enbling data ready interrupt 00527 ssn_gyr=1; 00528 wait(0.1); 00529 00530 __enable_irq(); 00531 } 00532 00533 float * FUNC_ACS_EXEC_GYR() 00534 { 00535 printf("\nEntered gyro\n"); 00536 uint8_t response; 00537 uint8_t MSB,LSB; 00538 int16_t bit_data; 00539 float data[3],error[3]={0,0,0}; //declaring error array to add to the values when required 00540 float senstivity = 145.6; //senstivity is 145.6 for full scale mode of +/-225 deg/sec 00541 ssn_gyr=0; 00542 spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of INT_ENABLE with readflag 00543 response=spi_acs.write(DUMMYBIT); // 00544 ssn_gyr=1; 00545 wait(0.1); 00546 00547 ssn_gyr=0; 00548 spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register 00549 response=spi_acs.write(response&(~(BIT_SLEEP))); //waking up the gyroscope from sleep 00550 ssn_gyr=1; 00551 wait(0.1); 00552 00553 trFlag=1; 00554 tr.attach(&trSub,1); //executes the function trSub afer 1sec 00555 00556 while(trFlag) 00557 { 00558 wait_ms(5); //This is required for this while loop to exit. I don't know why. 00559 if(drFlag==1) 00560 { 00561 ssn_gyr=0; 00562 spi_acs.write(GYRO_XOUT_H|READFLAG); //sending address of PWR_MGMT_1 with readflag 00563 for(int i=0;i<3;i++) 00564 { 00565 MSB = spi_acs.write(DUMMYBIT); //reading the MSB values of x,y and z respectively 00566 LSB = spi_acs.write(DUMMYBIT); //reading the LSB values of x,y and z respectively 00567 bit_data= ((int16_t)MSB<<8)|LSB; //concatenating to get 16 bit 2's complement of the required gyroscope values 00568 data[i]=(float)bit_data; 00569 data[i]=data[i]/senstivity; //dividing with senstivity to get the readings in deg/sec 00570 data[i]+=error[i]; //adding with error to remove offset errors 00571 } 00572 ssn_gyr=1; 00573 for (int i=0;i<3;i++) 00574 { 00575 printf("%f\t",data[i]); //printing the angular velocity values 00576 } 00577 printf("\n"); 00578 break; 00579 } 00580 drFlag=0; 00581 } 00582 ssn_gyr=0; 00583 spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of PWR_MGMT_1 with readflag 00584 response=spi_acs.write(DUMMYBIT); 00585 ssn_gyr=1; 00586 wait(0.1); 00587 00588 ssn_gyr=0; 00589 spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register 00590 response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode 00591 ssn_gyr=1; 00592 wait(0.1); 00593 printf("\nExited gyro\n"); 00594 return data; 00595 } 00596 00597 00598 00599 00600 00601
Generated on Wed Jul 13 2022 21:19:31 by
1.7.2