latest BAE code 10 dec
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of RAJANGAM_REVIEW_BAE_CODE by
ACS.cpp
00001 /*------------------------------------------------------------------------------------------------------------------------------------------------------ 00002 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/ 00003 #include <mbed.h> 00004 #include <math.h> 00005 Timer timer_SENSOR_DATA_ACQ; 00006 Timer timer_controlmodes; 00007 Timer timer_SENSOR_INIT; 00008 Timer timer_CONFIG_UPLOAD; 00009 00010 Serial acs_pc(USBTX,USBRX); 00011 00012 #include "pni.h" //pni header file 00013 #include "pin_config.h" 00014 #include "configuration.h" 00015 #include "ACS.h" 00016 #include "EPS.h" 00017 /*variables will get get updated value from FLash 00018 in case flash cups while testing i.e initial defaul values are kept as of now 00019 */ 00020 //********************************flags******************************************// 00021 extern uint32_t BAE_STATUS; 00022 extern uint32_t BAE_ENABLE; 00023 extern uint8_t ACS_INIT_STATUS; 00024 extern uint8_t ACS_DATA_ACQ_STATUS; 00025 extern uint8_t ACS_ATS_STATUS; 00026 extern uint8_t ACS_MAIN_STATUS; 00027 extern uint8_t ACS_STATUS; 00028 extern uint8_t ACS_DETUMBLING_ALGO_TYPE;////// 00029 00030 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch 00031 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch 00032 00033 extern uint8_t ACS_ATS_ENABLE; 00034 extern uint8_t ACS_DATA_ACQ_ENABLE; 00035 extern uint8_t ACS_STATE; 00036 00037 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod 00038 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod 00039 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod 00040 00041 extern PwmOut PWM1; //x //Functions used to generate PWM signal 00042 extern PwmOut PWM2; //y 00043 extern PwmOut PWM3; //z //PWM output comes from pins p6 00044 00045 int g_err_flag_TR_x=0; // setting x-flag to zero 00046 int g_err_flag_TR_y=0; // setting y-flag to zero 00047 int g_err_flag_TR_z=0; // setting z-flag to zero 00048 00049 extern float data[6]; 00050 extern BAE_HK_actual actual_data; 00051 00052 //global para 00053 //FUNCTION 00054 float max_invjm [9]= {1.0000,1.0000,1.0000,0.0471,4.6159,4.1582,4.4047,0.0755,4.1582}; 00055 float min_invjm[9] = {-1.0000,-1.0000,-1.0000,-0.0471,-4.6159,-4.1582,-4.4047,-0.0755,-4.1582}; 00056 float max_jm[9] = {0.3755,0.0176,0.2672,0.4895,0.2174,0.0452,1.0000,0.1209,0.0572}; 00057 float min_jm[9] = {-0.2491,-0.0457,-0.2271,-0.1556,-0.2222,-0.0175,-0.9998,-0.0361,-0.0922}; 00058 //se some other better way 00059 /* 00060 float max_bb[3] = {0,1.0*e-04*0.1633,1.0*e-04*0.1528}; 00061 float min_bb[3] = {0,1.0*e-04*(-0.1736),1.0*e-04*(-0.1419)}; 00062 */ 00063 float max_bb[3] = {0,1.0*0.0001*0.1633,1.0*0.0001*0.1528}; 00064 float min_bb[3] = {0,1.0*0.0001*(-0.1736),1.0*0.0001*(-0.1419)}; 00065 00066 //ACS 00067 uint8_t controlmode_mms = 0; 00068 uint8_t ATS1_EVENT_STATUS_RGTR=0x00; 00069 uint8_t ATS1_SENTRAL_STATUS_RGTR=0x00; 00070 uint8_t ATS1_ERROR_RGTR=0x00; 00071 uint8_t ATS2_EVENT_STATUS_RGTR=0x00; 00072 uint8_t ATS2_SENTRAL_STATUS_RGTR=0x00; 00073 uint8_t ATS2_ERROR_RGTR=0x00; 00074 uint8_t invjm_mms[9]; 00075 uint8_t jm_mms[9]; 00076 uint8_t bb_mms[3]; 00077 uint8_t singularity_flag_mms=0; 00078 uint8_t B_SCZ_ANGLE = 0x00; 00079 uint8_t ACS_MAG_TIME_DELAY;// = 65; 00080 uint8_t ACS_DEMAG_TIME_DELAY;// = 65; 00081 uint16_t ACS_Z_FIXED_MOMENT;// = 1.3; 00082 uint8_t ACS_TR_Z_SW_STATUS;//=1; 00083 uint8_t ACS_TR_XY_SW_STATUS;//=1; 00084 //GLOBAL PARA 00085 uint8_t ACS_TR_X_PWM; //* 00086 uint8_t ACS_TR_Y_PWM; //* 00087 uint8_t ACS_TR_Z_PWM; //* 00088 //change 00089 uint16_t ACS_MM_X_COMSN = 40; 00090 uint16_t ACS_MM_Y_COMSN = 50; 00091 uint16_t ACS_MG_X_COMSN = 40; 00092 uint16_t ACS_MG_Y_COMSN = 50; 00093 uint16_t ACS_MM_Z_COMSN = 60; 00094 uint16_t ACS_MG_Z_COMSN = 60; 00095 00096 uint8_t float_to_uint8(float min,float max,float val) 00097 { 00098 if(val>max) 00099 {return 0xff; 00100 } 00101 if(val<min) 00102 {return 0x00; 00103 } 00104 float div=max-min;div=(255.0/div);val=((val-min)*div); 00105 // printf("\n\n\n\rthe algo value is %d",val); 00106 return (uint8_t)val; 00107 } 00108 00109 00110 void float_to_uint8_ARRAY(int d1,int d2, float *arr,float max[], float min[], uint8_t *valarr) 00111 { 00112 for(int i=0;i<d1;i++) 00113 for(int j=0;j<d2;j++) 00114 { 00115 acs_pc.printf("\n\r%f",*((arr+(i*d1))+j)); 00116 valarr[i*d1+j] = (uint8_t)float_to_uint8(min[i*d1+j],max[i*d1+j],*((arr+(i*d1))+j)); 00117 acs_pc.printf("\n\r%d",valarr[i*d1+j]); 00118 } 00119 } 00120 00121 00122 00123 //Serial pc(USBTX,USBRX); //for usb communication 00124 00125 //CONTROL_ALGO 00126 float moment[3]; // Unit: Ampere*Meter^2//* 00127 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla 00128 //float db[3];//* 00129 uint8_t flag_firsttime=1, alarmmode=0; 00130 00131 //void controllermodes(float moment[3],float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t detumblingalgo); 00132 void inversec(float mat[3][3],float inv[3][3],int *singularity_flag); 00133 float max_array(float arr[3]); 00134 00135 void FCTN_ACS_CNTRLALGO (float moment[3], float b1[3], float omega1[3], uint8_t nominal , uint8_t detumbling , uint8_t ACS_DETUMBLING_ALGO_TYPE ) 00136 { 00137 float db1[3]; // Unit: Tesla/Second 00138 float normalising_fact; 00139 float b1_copy[3], omega1_copy[3], db1_copy[3]; 00140 int i, j; 00141 if(flag_firsttime==1) 00142 { 00143 for(i=0;i<3;i++) 00144 { 00145 db1[i]=0; // Unit: Tesla/Second 00146 } 00147 flag_firsttime=0; 00148 } 00149 else 00150 { 00151 for(i=0;i<3;i++) 00152 { 00153 db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second 00154 } 00155 } 00156 00157 if(nominal == 0) 00158 { 00159 if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1) 00160 { 00161 alarmmode=0; 00162 } 00163 else if(max_array(omega1)>OmegaMax && alarmmode==0) 00164 { 00165 alarmmode=1; 00166 } 00167 00168 } 00169 00170 for (i=0;i<3;i++) 00171 { 00172 b1_copy[i]=b1[i]; 00173 db1_copy[i]=db1[i]; 00174 omega1_copy[i]=omega1[i]; 00175 } 00176 00177 if(((alarmmode==0)|| (nominal == 1))&&(detumbling==0)) 00178 { 00179 //*controlmode=0; 00180 controlmode_mms =0; 00181 controllermodes(moment,b1,db1,omega1,controlmode_mms,ACS_DETUMBLING_ALGO_TYPE); 00182 for (i=0;i<3;i++) 00183 { 00184 b1[i]=b1_copy[i]; 00185 db1[i]=db1_copy[i]; 00186 omega1[i]=omega1_copy[i]; 00187 } 00188 if(max_array(moment)>MmntMax) 00189 { 00190 //*controlmode=1; 00191 controlmode_mms = 1; 00192 controllermodes(moment,b1,db1,omega1,controlmode_mms,ACS_DETUMBLING_ALGO_TYPE); 00193 for (i=0;i<3;i++) 00194 { 00195 b1[i]=b1_copy[i]; 00196 db1[i]=db1_copy[i]; 00197 omega1[i]=omega1_copy[i]; 00198 } 00199 if(max_array(moment)>MmntMax) 00200 { 00201 normalising_fact=max_array(moment)/MmntMax; 00202 for(i=0;i<3;i++) 00203 { 00204 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 00205 } 00206 } 00207 } 00208 00209 ACS_STATUS = 5; 00210 } 00211 else 00212 { 00213 //*controlmode=1; 00214 controlmode_mms = 1; 00215 controllermodes(moment,b1,db1,omega1,controlmode_mms,ACS_DETUMBLING_ALGO_TYPE); 00216 for (i=0;i<3;i++) 00217 { 00218 b1[i]=b1_copy[i]; 00219 db1[i]=db1_copy[i]; 00220 omega1[i]=omega1_copy[i]; 00221 } 00222 if(max_array(moment)>MmntMax) 00223 { 00224 normalising_fact=max_array(moment)/MmntMax; 00225 for(i=0;i<3;i++) 00226 { 00227 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 00228 } 00229 } 00230 00231 } 00232 for (i=0;i<3;i++) 00233 { 00234 b_old[i]=b1[i]; 00235 } 00236 } 00237 00238 void controllermodes(float moment[3], float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t ACS_DETUMBLING_ALGO_TYPE) 00239 { 00240 00241 float bb[3]={0,0,0}; 00242 float d[3]={0,0,0}; 00243 float den=0,den2; 00244 float bcopy[3]; 00245 int i, j;//temporary variables 00246 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs 00247 float invJm[3][3]; 00248 00249 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4; 00250 float Jm[3][3]={{0.2271,0.0014,-0.0026},{0.0014,0.2167,-0.004},{-0.0026,-0.004,0.2406}}; // Unit: Kilogram*Meter^2. Jm may change depending on the final satellite structure 00251 00252 int singularity_flag = 0; 00253 singularity_flag_mms=0; 00254 00255 if(controlmode1==0) 00256 { 00257 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2])); 00258 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]); 00259 if (den==0) 00260 { 00261 singularity_flag = 1; 00262 singularity_flag_mms=1; 00263 } 00264 if (singularity_flag==0) 00265 { 00266 for(i=0;i<3;i++) 00267 { 00268 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1) 00269 } 00270 for(i=0;i<3;i++) 00271 { 00272 b[i]/=den; // Mormalized b. Hence no unit. 00273 } 00274 if(b[2]>0.9 || b[2]<-0.9) 00275 { 00276 kz=kz2; 00277 kmu=kmu2; 00278 gamma=gamma2; 00279 } 00280 for(i=0;i<2;i++) 00281 { 00282 Mu[i]=b[i]; 00283 v[i]=-kmu*Mu[i]; 00284 dv[i]=-kmu*db[i]; 00285 z[i]=db[i]-v[i]; 00286 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma); 00287 } 00288 inversec(Jm,invJm,&singularity_flag); 00289 for(i=0;i<3;i++) 00290 { 00291 for(j=0;j<3;j++) 00292 { 00293 bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]); 00294 } 00295 } 00296 for(i=0;i<3;i++) 00297 { 00298 for(j=0;j<3;j++) 00299 { 00300 d[i]+=bb[j]*invJm[i][j]; 00301 } 00302 } 00303 bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])+(omega[1]*db[2])-(omega[2]*db[1]); 00304 bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])+(omega[2]*db[0])-(omega[0]*db[2]); 00305 bb[0]=0; 00306 for(i=0;i<3;i++) 00307 { 00308 d[i]=invJm[2][i]; 00309 invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i]; 00310 invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i]; 00311 invJm[0][i]=b[i]; 00312 } 00313 inversec(invJm,Jm,&singularity_flag); 00314 00315 float_to_uint8_ARRAY(3,3, (float*)invJm,max_invjm, min_invjm, invjm_mms); 00316 float_to_uint8_ARRAY(3,3, (float*)Jm,max_jm, min_jm, jm_mms); 00317 float_to_uint8_ARRAY(1,3, (float*)bb,max_bb, min_bb, bb_mms); 00318 singularity_flag_mms = singularity_flag; 00319 if (singularity_flag==0) 00320 { 00321 for(i=0;i<3;i++) 00322 { 00323 for(j=0;j<3;j++) 00324 { 00325 tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2 00326 } 00327 } 00328 for(i=0;i<3;i++) 00329 { 00330 bcopy[i]=b[i]*den; 00331 } 00332 for(i=0;i<3;i++) 00333 { 00334 Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3]; 00335 Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2 00336 } 00337 } 00338 } 00339 if (singularity_flag==1) 00340 { 00341 for (i=0;i<3;i++) 00342 { 00343 Mmnt[i]=2*MmntMax; 00344 } 00345 } 00346 ACS_STATUS =5; 00347 } 00348 else if(controlmode1==1) 00349 { 00350 if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo 00351 { 00352 for(i=0;i<3;i++) 00353 { 00354 Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2 00355 } 00356 ACS_STATUS = 6; 00357 } 00358 else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo 00359 { 00360 for(i=0;i<3;i++) 00361 { 00362 Mmnt[i]=-kdetumble*db[i]; 00363 } 00364 ACS_STATUS = 4; 00365 } 00366 } 00367 for(i=0;i<3;i++) 00368 { 00369 moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2 00370 } 00371 } 00372 00373 void inversec(float mat[3][3],float inv[3][3],int *singularity_flag) 00374 { 00375 int i,j; 00376 float det=0; 00377 for(i=0;i<3;i++) 00378 { 00379 for(j=0;j<3;j++) 00380 { 00381 inv[j][i]=(mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3])-(mat[(i+2)%3][(j+1)%3]*mat[(i+1)%3][(j+2)%3]); 00382 } 00383 } 00384 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]); 00385 if (det==0) 00386 { 00387 *singularity_flag=1; 00388 } 00389 else 00390 { 00391 *singularity_flag=0; 00392 for(i=0;i<3;i++) 00393 { 00394 for(j=0;j<3;j++) 00395 { 00396 inv[i][j]/=det; 00397 } 00398 } 00399 } 00400 } 00401 00402 float max_array(float arr[3]) 00403 { 00404 int i; 00405 float temp_max=fabs(arr[0]); 00406 for(i=1;i<3;i++) 00407 { 00408 if(fabs(arr[i])>temp_max) 00409 { 00410 temp_max=fabs(arr[i]); 00411 } 00412 } 00413 return temp_max; 00414 } 00415 00416 00417 00418 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge 00419 00420 int FCTN_ACS_INIT(); //initialization of registers happens 00421 int SENSOR_INIT(); 00422 int FCTN_ATS_DATA_ACQ(); //data is obtained 00423 int SENSOR_DATA_ACQ(); 00424 //void T_OUT(); //timeout function to stop infinite loop 00425 00426 int CONFIG_UPLOAD(); 00427 //Timeout to; //Timeout variable to 00428 int toFlag; 00429 00430 int count =0; // Time for which the BAE uC is running (in seconds) 00431 //void T_OUT() 00432 //{ 00433 // toFlag=0; //as T_OUT function gets called the while loop gets terminated 00434 //} 00435 00436 00437 //DEFINING VARIABLES 00438 char cmd[2]; 00439 char raw_gyro[6]; 00440 char raw_mag[6]; 00441 char reg_data[24]; 00442 char store,status; 00443 //int16_t bit_data done in actual_data structure itself; 00444 00445 uint16_t time_data; 00446 float gyro_data[3], mag_data[3]; 00447 //float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0}; 00448 00449 int ack; 00450 int CONFIG_UPLOAD() 00451 { 00452 uint8_t value; 00453 00454 cmd[0]=RESETREQ; 00455 cmd[1]=BIT_RESREQ; 00456 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up 00457 wait_ms(275); 00458 //kick_WDOG(); 00459 acs_pc.printf("\n\r lvl5"); 00460 wait_ms(300); 00461 //Verify magic number 00462 00463 00464 cmd[0]=SENTRALSTATUS; 00465 i2c.write(SLAVE_ADDR,cmd,1); 00466 i2c.read(SLAVE_ADDR_READ,&store,1); 00467 value = (uint8_t)store; 00468 00469 if(value & 0x02) 00470 { 00471 acs_pc.printf("Sentral already has eeprom firmware loaded.\n"); 00472 } 00473 /* Write value 0x01 to the ResetReq register, address 0x9B. This will result 00474 in a hard reset of the Sentral. This is unnecessary if the prior event was 00475 a Reset. */ 00476 if(!(value & 0x08)) 00477 { 00478 acs_pc.printf("CPU is not in standby, issuing a shutdown request.\n"); 00479 //i2c_write(I2C_SLAVE_ADDR, 0x34, data, 1); 00480 cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to shut down 00481 cmd[1]=0x00; 00482 i2c.write(SLAVE_ADDR,cmd,2); 00483 00484 int cnt=0; 00485 do { 00486 cmd[0]=SENTRALSTATUS; 00487 i2c.write(SLAVE_ADDR,cmd,1); 00488 i2c.read(SLAVE_ADDR_READ,&store,1); 00489 value = (uint8_t)store; 00490 wait_ms(100); 00491 cnt++; 00492 } while((!(value & 0x08))&&(cnt<4)); 00493 00494 if(cnt==4) 00495 { 00496 return 0; 00497 } 00498 } 00499 00500 cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload 00501 cmd[1]=BIT_HOST_UPLD_ENB; 00502 i2c.write(SLAVE_ADDR,cmd,2); 00503 wait_ms(20); 00504 00505 //kick_WDOG(); 00506 acs_pc.printf("\n\r lvl6"); 00507 00508 cmd[0]=UPLOAD_ADDR; //0x0000 is written in RAM register to enable upload 00509 cmd[1]=0x0000; 00510 i2c.write(SLAVE_ADDR,cmd,3); 00511 wait_ms(100); 00512 00513 acs_pc.printf("Uploading data...\n"); 00514 00515 #define TRASACTION_SIZE 3 00516 00517 00518 for(int i = 0; i < EEPROMTextLength; i += TRASACTION_SIZE * 4) 00519 { 00520 00521 char* data = new char[TRASACTION_SIZE * 4]; 00522 data[0]=0x96; 00523 for(int j = 0; j < TRASACTION_SIZE; j++) 00524 { 00525 data[j * 4 + 1] = configdata[i + j * 4 + 3]; 00526 data[j * 4 + 2] = configdata[i + j * 4 + 2]; 00527 data[j * 4 + 3] = configdata[i + j * 4 + 1]; 00528 data[j * 4 + 4] = configdata[i + j * 4 + 0]; 00529 } 00530 00531 if(EEPROMTextLength < (i + (TRASACTION_SIZE * 4))) 00532 { 00533 uint32_t bytes = EEPROMTextLength - i; 00534 i2c.write(SLAVE_ADDR,data,bytes+1); 00535 } 00536 00537 else 00538 { 00539 /* Write the Configuration File to Sentral’s program RAM. The file is sent 00540 one byte at a time, using the UploadData register, register address 0x96. */ 00541 i2c.write(SLAVE_ADDR,data,13); 00542 } 00543 delete data; 00544 } 00545 00546 char crc[4]; 00547 cmd[0]=0x97; 00548 i2c.write(SLAVE_ADDR,cmd,1); 00549 i2c.read(SLAVE_ADDR_READ,crc,4); 00550 value = (uint8_t)store; 00551 00552 uint32_t actualCRC = ((uint32_t)crc[0] << 0) | ((uint32_t)crc[1] << 8) | ((uint32_t)crc[2] << 16) | ((uint32_t)crc[3] << 24); 00553 00554 if(actualCRC != EEPROMTextCRC) 00555 { 00556 acs_pc.printf("Program crc (0x%.8X) does not match CRC reported by Sentral (0x%0.8X)\n", EEPROMTextCRC, actualCRC); 00557 return 0; 00558 } 00559 else 00560 { 00561 acs_pc.printf("Firmware Upload Complete.\n"); 00562 return 1; 00563 } 00564 00565 cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload 00566 cmd[1]=0x00; 00567 i2c.write(SLAVE_ADDR,cmd,2); 00568 wait_ms(20); 00569 00570 return 0; 00571 } 00572 00573 00574 int PARAMETER_TRANSFER_GYRO(int gyro_rng) 00575 { 00576 00577 //READ_PARAMETER(); 00578 if(gyro_rng ==0) 00579 return 0; 00580 char range[5]; 00581 00582 00583 range[0] = 0x64; 00584 range[1] = 0x4B; 00585 i2c.write(SLAVE_ADDR,range,5); 00586 i2c.write(SLAVE_ADDR,range,2); 00587 00588 range[0] = 0x54; 00589 range[1] = 0x80; 00590 i2c.write(SLAVE_ADDR,range,2); 00591 00592 range[0]=0x3A; 00593 i2c.write(SLAVE_ADDR,range,1); 00594 i2c.read(SLAVE_ADDR_READ,range,1); 00595 00596 printf("ACK_PARA : %.02X",range[0]); 00597 00598 if(range[0] != ((char)(0x4B))) 00599 { 00600 printf("Parameter transfer read failed \r\n"); 00601 return 0; 00602 } 00603 00604 range[0]=0x3B; 00605 i2c.write(SLAVE_ADDR,range,1); 00606 i2c.read(SLAVE_ADDR_READ,range,4); 00607 00608 printf("range[0] range[1] range[2] range[3] : %.02X %.02X %.02X %.02X \r\n",range[0] ,range[1] , range[2] ,range[3]); 00609 00610 //range[0] = 0x64; 00611 //range[1] = 0x00; 00612 //i2c.write(SLAVE_ADDR,range,2); 00613 00614 //range[0] = 0x54; 00615 //range[1] = 0x00; 00616 //i2c.write(SLAVE_ADDR,range,2); 00617 //wait_ms(4000); 00618 00619 00620 00621 range[0] = 0x60; 00622 00623 00624 range[1] = (int)(gyro_rng%256); //1000 00625 range[2] = (int)(gyro_rng/256); 00626 00627 //range[1]= 0x88; 00628 //range[2] = 0x13; //5000 00629 00630 range[3] = 0x00; 00631 range[4] = 0x00; 00632 i2c.write(SLAVE_ADDR,range,5); 00633 00634 range[0] = 0x64; 00635 range[1] = 0xCB; 00636 i2c.write(SLAVE_ADDR,range,2); 00637 00638 00639 //range[0] = 0x54; 00640 //range[1] = 0x80; 00641 //i2c.write(SLAVE_ADDR,range,2); 00642 00643 range[0]=0x3A; 00644 i2c.write(SLAVE_ADDR,range,1); 00645 i2c.read(SLAVE_ADDR_READ,range,1); 00646 00647 printf("ACK_PARA : %.02X",range[0]); 00648 00649 if(range[0] != ((char)(0xCB))) 00650 { 00651 printf("Parameter transfer failed \r\n"); 00652 return 0; 00653 } 00654 00655 printf("Parameter transfer success\r\n"); 00656 //wait_ms(4000); 00657 //range[0] = 0x64; 00658 //range[1] = 0x00; 00659 //i2c.write(SLAVE_ADDR,range,5); 00660 //i2c.write(SLAVE_ADDR,range,2); 00661 00662 // range[0] = 0x54; 00663 // range[1] = 0x00; 00664 //i2c.write(SLAVE_ADDR,range,2); 00665 00666 00667 printf("Reading parameter\r\n"); 00668 //READ_PARAMETER(); 00669 00670 range[0] = 0x64; 00671 range[1] = 0x4B; 00672 i2c.write(SLAVE_ADDR,range,5); 00673 i2c.write(SLAVE_ADDR,range,2); 00674 00675 //range[0] = 0x54; 00676 //range[1] = 0x80; 00677 //i2c.write(SLAVE_ADDR,range,2); 00678 00679 range[0]=0x3A; 00680 i2c.write(SLAVE_ADDR,range,1); 00681 i2c.read(SLAVE_ADDR_READ,range,1); 00682 00683 printf("ACK_PARA : %.02X",range[0]); 00684 00685 if(range[0] != ((char)(0x4B))) 00686 { 00687 printf("Parameter transfer read failed \r\n"); 00688 return 0; 00689 } 00690 00691 range[0]=0x3B; 00692 i2c.write(SLAVE_ADDR,range,1); 00693 i2c.read(SLAVE_ADDR_READ,range,4); 00694 00695 printf("range[0] range[1] range[2] range[3] : %.02X %.02X %.02X %.02X \r\n",range[0] ,range[1] , range[2] ,range[3]); 00696 00697 range[0] = 0x64; 00698 range[1] = 0x00; 00699 i2c.write(SLAVE_ADDR,range,2); 00700 00701 range[0] = 0x54; 00702 range[1] = 0x00; 00703 i2c.write(SLAVE_ADDR,range,2); 00704 00705 00706 return 1; 00707 00708 } 00709 00710 int PARAMETER_TRANSFER_MAG(int mag_rng) 00711 { 00712 00713 //READ_PARAMETER(); 00714 if(mag_rng ==0) 00715 return 0; 00716 char range[5]; 00717 00718 00719 range[0] = 0x64; 00720 range[1] = 0x4A; 00721 i2c.write(SLAVE_ADDR,range,5); 00722 i2c.write(SLAVE_ADDR,range,2); 00723 00724 range[0] = 0x54; 00725 range[1] = 0x80; 00726 i2c.write(SLAVE_ADDR,range,2); 00727 00728 range[0]=0x3A; 00729 i2c.write(SLAVE_ADDR,range,1); 00730 i2c.read(SLAVE_ADDR_READ,range,1); 00731 00732 printf("ACK_PARA : %.02X",range[0]); 00733 00734 if(range[0] != ((char)(0x4B))) 00735 { 00736 printf("Parameter transfer read failed \r\n"); 00737 return 0; 00738 } 00739 00740 range[0]=0x3B; 00741 i2c.write(SLAVE_ADDR,range,1); 00742 i2c.read(SLAVE_ADDR_READ,range,4); 00743 00744 printf("range[0] range[1] range[2] range[3] : %.02X %.02X %.02X %.02X \r\n",range[0] ,range[1] , range[2] ,range[3]); 00745 00746 //range[0] = 0x64; 00747 //range[1] = 0x00; 00748 //i2c.write(SLAVE_ADDR,range,2); 00749 00750 //range[0] = 0x54; 00751 //range[1] = 0x00; 00752 //i2c.write(SLAVE_ADDR,range,2); 00753 //wait_ms(4000); 00754 00755 00756 00757 range[0] = 0x60; 00758 00759 00760 range[1] = (int)(mag_rng%256); //1000 00761 range[2] = (int)(mag_rng/256); 00762 00763 //range[1]= 0x88; 00764 //range[2] = 0x13; //5000 00765 00766 range[3] = 0x00; 00767 range[4] = 0x00; 00768 i2c.write(SLAVE_ADDR,range,5); 00769 00770 range[0] = 0x64; 00771 range[1] = 0xCA; 00772 i2c.write(SLAVE_ADDR,range,2); 00773 00774 00775 //range[0] = 0x54; 00776 //range[1] = 0x80; 00777 //i2c.write(SLAVE_ADDR,range,2); 00778 00779 range[0]=0x3A; 00780 i2c.write(SLAVE_ADDR,range,1); 00781 i2c.read(SLAVE_ADDR_READ,range,1); 00782 00783 printf("ACK_PARA : %.02X",range[0]); 00784 00785 if(range[0] != ((char)(0xCB))) 00786 { 00787 printf("Parameter transfer failed \r\n"); 00788 return 0; 00789 } 00790 00791 printf("Parameter transfer success\r\n"); 00792 //wait_ms(4000); 00793 //range[0] = 0x64; 00794 //range[1] = 0x00; 00795 //i2c.write(SLAVE_ADDR,range,5); 00796 //i2c.write(SLAVE_ADDR,range,2); 00797 00798 // range[0] = 0x54; 00799 // range[1] = 0x00; 00800 //i2c.write(SLAVE_ADDR,range,2); 00801 00802 00803 printf("Reading parameter\r\n"); 00804 //READ_PARAMETER(); 00805 00806 range[0] = 0x64; 00807 range[1] = 0x4A; 00808 i2c.write(SLAVE_ADDR,range,5); 00809 i2c.write(SLAVE_ADDR,range,2); 00810 00811 //range[0] = 0x54; 00812 //range[1] = 0x80; 00813 //i2c.write(SLAVE_ADDR,range,2); 00814 00815 range[0]=0x3A; 00816 i2c.write(SLAVE_ADDR,range,1); 00817 i2c.read(SLAVE_ADDR_READ,range,1); 00818 00819 printf("ACK_PARA : %.02X",range[0]); 00820 00821 if(range[0] != ((char)(0x4B))) 00822 { 00823 printf("Parameter transfer read failed \r\n"); 00824 return 0; 00825 } 00826 00827 range[0]=0x3B; 00828 i2c.write(SLAVE_ADDR,range,1); 00829 i2c.read(SLAVE_ADDR_READ,range,4); 00830 00831 printf("range[0] range[1] range[2] range[3] : %.02X %.02X %.02X %.02X \r\n",range[0] ,range[1] , range[2] ,range[3]); 00832 00833 range[0] = 0x64; 00834 range[1] = 0x00; 00835 i2c.write(SLAVE_ADDR,range,2); 00836 00837 range[0] = 0x54; 00838 range[1] = 0x00; 00839 i2c.write(SLAVE_ADDR,range,2); 00840 00841 00842 return 1; 00843 00844 } 00845 00846 00847 int SENSOR_INIT() 00848 { 00849 /// acs_pc.printf("Entered sensor init\n \r"); 00850 cmd[0]=RESETREQ; 00851 cmd[1]=BIT_RESREQ; 00852 ack = i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up 00853 //wait_ms(575); //waiting for loading configuration file stored in EEPROM 00854 00855 /// acs_pc.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM 00856 00857 if( ack!=0) 00858 { 00859 cmd[0]=RESETREQ; 00860 cmd[1]=BIT_RESREQ; 00861 ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat 00862 if(ack !=0) 00863 return 0; 00864 } 00865 00866 00867 wait_ms(275); 00868 //kick_WDOG(); 00869 acs_pc.printf("\n\r lvl2"); 00870 wait_ms(300); 00871 00872 cmd[0]=SENTRALSTATUS; 00873 ack = i2c.write(SLAVE_ADDR,cmd,1); 00874 00875 if( ack!=0) 00876 { 00877 ack = i2c.write(SLAVE_ADDR,cmd,1); 00878 if(ack!=0) 00879 return 0; 00880 } 00881 00882 ack = i2c.read(SLAVE_ADDR_READ,&store,1); 00883 00884 if( ack!=0) 00885 { 00886 ack = i2c.read(SLAVE_ADDR_READ,&store,1); 00887 if(ack!=0) 00888 return 0; 00889 } 00890 00891 /// acs_pc.printf("Sentral Status is %x\n \r",(int)store); 00892 acs_pc.printf("\n\r stuck here maaan nupe passed it!! inside ACS"); 00893 //to check whether EEPROM is uploaded properly 00894 switch((int)store) { 00895 case(3): { 00896 break; 00897 } 00898 case(11): { 00899 break; 00900 } 00901 default: { 00902 cmd[0]=RESETREQ; 00903 cmd[1]=BIT_RESREQ; 00904 ack = i2c.write(SLAVE_ADDR,cmd,2); 00905 if( ack!=0) 00906 { 00907 ack = i2c.write(SLAVE_ADDR,cmd,2); 00908 if(ack!=0) 00909 return 0; 00910 } 00911 wait_ms(275);//should be 600 00912 //kick_WDOG(); 00913 acs_pc.printf("\n\r lvl3"); 00914 wait_ms(300); 00915 cmd[0]=SENTRALSTATUS; 00916 ack = i2c.write(SLAVE_ADDR,cmd,1); 00917 if( ack!=0) 00918 { 00919 ack = i2c.write(SLAVE_ADDR,cmd,1); 00920 if(ack!=0) 00921 return 0; 00922 } 00923 ack = i2c.read(SLAVE_ADDR_READ,&store,1); 00924 if( ack!=0) 00925 { 00926 ack = i2c.read(SLAVE_ADDR_READ,&store,1); 00927 if(ack!=0) 00928 return 0; 00929 } 00930 /// acs_pc.printf("Sentral Status is %x\n \r",(int)store); 00931 00932 } 00933 } 00934 00935 int manual=0; 00936 if( ((int)store != 11 )&&((int)store != 3)) 00937 { 00938 00939 cmd[0]=RESETREQ; 00940 cmd[1]=BIT_RESREQ; 00941 ack = i2c.write(SLAVE_ADDR,cmd,2); 00942 if( ack!=0) 00943 { 00944 ack = i2c.write(SLAVE_ADDR,cmd,2); 00945 if(ack!=0) 00946 return 0; 00947 } 00948 wait_ms(275); 00949 //kick_WDOG(); 00950 acs_pc.printf("\n\r lvl4"); 00951 wait_ms(300); 00952 00953 timer_SENSOR_INIT.start(); 00954 manual = CONFIG_UPLOAD(); 00955 timer_SENSOR_INIT.stop(); 00956 00957 if(manual == 0) 00958 { 00959 //MANUAL CONFIGURATION FAILED 00960 return 0; 00961 } 00962 00963 } 00964 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors 00965 cmd[1]=BIT_RUN_ENB; 00966 ack = i2c.write(SLAVE_ADDR,cmd,2); 00967 if( ack!=0) 00968 { 00969 ack = i2c.write(SLAVE_ADDR,cmd,2); 00970 if(ack!=0) 00971 return 0; 00972 } 00973 00974 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer 00975 cmd[1]=BIT_MAGODR; 00976 ack = i2c.write(SLAVE_ADDR,cmd,2); 00977 if( ack!=0) 00978 { 00979 ack = i2c.write(SLAVE_ADDR,cmd,2); 00980 if(ack!=0) 00981 return 0; 00982 } 00983 00984 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope 00985 cmd[1]=BIT_GYROODR; 00986 ack = i2c.write(SLAVE_ADDR,cmd,2); 00987 if( ack!=0) 00988 { 00989 ack = i2c.write(SLAVE_ADDR,cmd,2); 00990 if(ack!=0) 00991 return 0; 00992 } 00993 00994 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer 00995 cmd[1]=0x00; 00996 ack = i2c.write(SLAVE_ADDR,cmd,2); 00997 if( ack!=0) 00998 { 00999 ack = i2c.write(SLAVE_ADDR,cmd,2); 01000 if(ack!=0) 01001 return 0; 01002 } 01003 //wait_ms(20); 01004 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values 01005 cmd[1]=0x00; 01006 ack = i2c.write(SLAVE_ADDR,cmd,2); 01007 if( ack!=0) 01008 { 01009 ack = i2c.write(SLAVE_ADDR,cmd,2); 01010 if(ack!=0) 01011 return 0; 01012 } 01013 01014 cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values 01015 cmd[1]=BIT_EVT_ENB; 01016 ack = i2c.write(SLAVE_ADDR,cmd,2); 01017 if( ack!=0) 01018 { 01019 ack = i2c.write(SLAVE_ADDR,cmd,2); 01020 if(ack!=0) 01021 return 0; 01022 } 01023 01024 PARAMETER_TRANSFER_GYRO(0); // dont set range //PARAMETER of function sets range of gyroscope 01025 PARAMETER_TRANSFER_MAG(0); // dont set range //PARAMETER of function sets range of magnetometer 01026 01027 cmd[0]=SENTRALSTATUS; 01028 ack = i2c.write(SLAVE_ADDR,cmd,1); 01029 if( ack!=0) 01030 { 01031 ack = i2c.write(SLAVE_ADDR,cmd,1); 01032 if(ack!=0) 01033 return 0; 01034 } 01035 01036 ack = i2c.read(SLAVE_ADDR_READ,&store,1); 01037 if( ack!=0) 01038 { 01039 ack= i2c.read(SLAVE_ADDR_READ,&store,1); 01040 if(ack!=0) 01041 return 0; 01042 } 01043 01044 /// acs_pc.printf("Sentral Status after initialising is %x\n \r",(int)store); 01045 01046 if( (int)store == 3) //Check if initialised properly and not in idle state 01047 { 01048 /// acs_pc.printf("Exited sensor init successfully\n \r"); 01049 return 1; 01050 } 01051 01052 01053 //// acs_pc.printf("Sensor init failed \n \r") ; 01054 return 0; 01055 } 01056 01057 int FCTN_ACS_INIT() 01058 { 01059 ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag 01060 01061 int working=0; 01062 01063 /// acs_pc.printf("Attitude sensor init called \n \r"); 01064 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 01065 01066 01067 if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11 01068 { 01069 01070 /// acs_pc.printf("Sensor 1 marked working \n \r"); 01071 timer_SENSOR_INIT.start(); 01072 working = SENSOR_INIT(); 01073 timer_SENSOR_INIT.stop(); 01074 if(working ==1) 01075 { 01076 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 01077 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful 01078 /// acs_pc.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r"); 01079 ACS_INIT_STATUS = 0; 01080 return 1; 01081 } 01082 01083 01084 01085 /// acs_pc.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off 01086 ATS1_SW_ENABLE = 0; 01087 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; 01088 01089 } 01090 01091 /// acs_pc.printf("Sensor 1 not working. Trying Sensor 2\n \r"); 01092 01093 if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11 01094 { 01095 01096 01097 ATS2_SW_ENABLE = 0; 01098 wait_ms(5); 01099 timer_SENSOR_INIT.reset(); 01100 timer_SENSOR_INIT.start(); 01101 working = SENSOR_INIT(); 01102 timer_SENSOR_INIT.stop(); 01103 01104 if(working ==1) 01105 { 01106 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 01107 /// acs_pc.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); //Sensor2 INIT successful 01108 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 01109 ACS_INIT_STATUS = 0; 01110 return 2; 01111 } 01112 01113 ATS2_SW_ENABLE = 1; 01114 wait_ms(5); 01115 01116 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; 01117 01118 01119 } 01120 01121 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 01122 /// acs_pc.printf("Sensor 2 also not working.Exit init.\n \r"); 01123 01124 ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working 01125 return 0; 01126 } 01127 01128 01129 int SENSOR_DATA_ACQ() 01130 { 01131 //int mag_only=0; 01132 /// acs_pc.printf("Entering Sensor data acq.\n \r"); 01133 char status; 01134 int sentral; 01135 int event; 01136 int sensor; 01137 int error; 01138 int init; 01139 01140 uint8_t gyro_error=0; 01141 uint8_t mag_error=0; 01142 01143 //int ack1; 01144 //int ack2; 01145 01146 cmd[0]=EVT_STATUS; 01147 ack = i2c.write(SLAVE_ADDR,cmd,1); 01148 if(ack!=0) 01149 { 01150 ack = i2c.write(SLAVE_ADDR,cmd,1); 01151 if(ack!=0) 01152 return 0; 01153 } 01154 01155 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01156 if(ack!=0) 01157 { 01158 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01159 if(ack!=0) 01160 return 0; 01161 } 01162 01163 event = (int)status; 01164 01165 //if(ACS_ATS_STATUS&0xC0 == 0x40) 01166 if(1) 01167 { 01168 ATS1_EVENT_STATUS_RGTR = (uint8_t)event; 01169 } 01170 else if(ACS_ATS_STATUS&0x0C == 0x04) 01171 { 01172 ATS2_EVENT_STATUS_RGTR = (uint8_t)event; 01173 } 01174 01175 cmd[0]=SENTRALSTATUS; 01176 ack = i2c.write(SLAVE_ADDR,cmd,1); 01177 if(ack!=0) 01178 { 01179 ack = i2c.write(SLAVE_ADDR,cmd,1); 01180 if(ack!=0) 01181 return 0; 01182 } 01183 01184 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01185 if(ack!=0) 01186 { 01187 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01188 if(ack!=0) 01189 return 0; 01190 } 01191 01192 01193 sentral = (int) status; 01194 01195 //if(ACS_ATS_STATUS&0xC0 == 0x40) 01196 if(1) 01197 { 01198 ATS1_SENTRAL_STATUS_RGTR = (uint8_t)sentral; 01199 } 01200 else if(ACS_ATS_STATUS&0x0C == 0x04) 01201 { 01202 ATS2_SENTRAL_STATUS_RGTR = (uint8_t)sentral; 01203 } 01204 01205 /// acs_pc.printf("Event Status is %x\n \r",event); 01206 /// acs_pc.printf("Sentral Status is %x\n \r",sentral); 01207 01208 01209 01210 if ( ((event & 0x20 )!= 0x20 ) || ((event & 0x08) != 0x08 ) || ((event & 0x01) == 0x01 )|| ((event & 0x02) == 0x02 )|| (sentral!= 3)) //check for any error in event status register 01211 { 01212 01213 timer_SENSOR_INIT.reset(); 01214 timer_SENSOR_INIT.start(); 01215 init = SENSOR_INIT(); 01216 timer_SENSOR_INIT.stop(); 01217 01218 cmd[0]=EVT_STATUS; 01219 ack = i2c.write(SLAVE_ADDR,cmd,1); 01220 if(ack!=0) 01221 { 01222 ack = i2c.write(SLAVE_ADDR,cmd,1); 01223 if(ack!=0) 01224 return 0; 01225 } 01226 01227 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01228 if(ack!=0) 01229 { 01230 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01231 if(ack!=0) 01232 return 0; 01233 } 01234 01235 event = (int)status; 01236 01237 cmd[0]=SENTRALSTATUS; 01238 ack = i2c.write(SLAVE_ADDR,cmd,1); 01239 if(ack!=0) 01240 { 01241 ack = i2c.write(SLAVE_ADDR,cmd,1); 01242 if(ack!=0) 01243 return 0; 01244 } 01245 01246 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01247 if(ack!=0) 01248 { 01249 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01250 if(ack!=0) 01251 return 0; 01252 } 01253 01254 sentral = (int)status; 01255 int poll_status; 01256 /// acs_pc.printf("Event Status after resetting and init is %x\n \r",event); 01257 01258 if ( ((event & 0x20) != 0x20 ) || ((event & 0x08) != 0x08) || ((event & 0x01) == 0x01 )|| ((event & 0x02) == 0x02 ) || (init == 0)||(sentral != 3)) //check for any error in event status 01259 { 01260 01261 cmd[0]=ERROR; 01262 ack = i2c.write(SLAVE_ADDR,cmd,1); 01263 if(ack!=0) 01264 { 01265 ack = i2c.write(SLAVE_ADDR,cmd,1); 01266 if(ack!=0) 01267 return 0; 01268 } 01269 01270 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01271 if(ack!=0) 01272 { 01273 01274 if(ACS_ATS_STATUS&0xC0 == 0x40) 01275 { 01276 ATS1_ERROR_RGTR = 0x01; 01277 } 01278 else if(ACS_ATS_STATUS&0x0C == 0x04) 01279 { 01280 ATS2_ERROR_RGTR = 0x01; 01281 } 01282 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01283 if(ack!=0) 01284 return 0; 01285 } 01286 01287 error = (int)status; 01288 01289 //if(ACS_ATS_STATUS&0xC0 == 0x40) 01290 if(1) 01291 { 01292 ATS1_ERROR_RGTR = (uint8_t)error; 01293 } 01294 else if(ACS_ATS_STATUS&0x0C == 0x04) 01295 { 01296 ATS2_ERROR_RGTR = (uint8_t)error; 01297 } 01298 01299 cmd[0]=SENSORSTATUS; 01300 ack = i2c.write(SLAVE_ADDR,cmd,1); 01301 if(ack!=0) 01302 { 01303 ack = i2c.write(SLAVE_ADDR,cmd,1); 01304 if(ack!=0) 01305 return 0; 01306 } 01307 01308 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01309 if(ack!=0) 01310 { 01311 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01312 if(ack!=0) 01313 return 0; 01314 } 01315 01316 sensor = (int)status; 01317 01318 01319 if((error!=0) || (sensor!=0)) 01320 { 01321 if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) ) 01322 { 01323 acs_pc.printf("error in gyro.\n \r"); 01324 gyro_error = 1; 01325 } 01326 01327 01328 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) ) 01329 { 01330 01331 acs_pc.printf("error in mag.Exiting.\n \r"); 01332 mag_error = 1; 01333 } 01334 if( (gyro_error!=1)&&(mag_error!=1)) 01335 { 01336 acs_pc.printf("error in something else.Exiting.\n \r"); 01337 return 0; 01338 01339 } 01340 } 01341 01342 01343 if(((event & 1) == 1 )) 01344 { 01345 /// acs_pc.printf("error in CPU Reset.\n \r"); 01346 return 0; 01347 01348 } 01349 int poll =0; 01350 poll_status =1; 01351 while((poll<15)&&(((event & 8) != 8 )||((event & 32) != 32 ))) 01352 //if(((event & 8) != 8 )||((event & 32) != 32 )) 01353 { 01354 poll++; 01355 acs_pc.printf("Data not ready waiting...POLL#%d\n \r",poll); 01356 //POLL 01357 wait_ms(200); 01358 01359 cmd[0]=EVT_STATUS; 01360 01361 ack = i2c.write(SLAVE_ADDR,cmd,1); 01362 if(ack!=0) 01363 { 01364 ack = i2c.write(SLAVE_ADDR,cmd,1); 01365 if(ack!=0) 01366 return 0; 01367 } 01368 01369 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01370 if(ack!=0) 01371 { 01372 ack = i2c.read(SLAVE_ADDR_READ,&status,1); 01373 if(ack!=0) 01374 return 0; 01375 } 01376 01377 event = (int)status; 01378 if((event & 32) != 32 ) 01379 { 01380 01381 acs_pc.printf("Gyro not ready..\n \r"); 01382 gyro_error = 1; 01383 01384 } 01385 else 01386 { 01387 gyro_error = 0; 01388 } 01389 01390 if((event & 8) != 8 ) 01391 { 01392 acs_pc.printf("Mag not ready..\n \r"); 01393 mag_error=1; 01394 } 01395 else 01396 { 01397 mag_error=0; 01398 } 01399 01400 01401 } 01402 01403 if(poll!=15) 01404 { 01405 poll_status = 0; 01406 } 01407 01408 01409 } 01410 01411 if(((mag_error !=1)&&(gyro_error!=1))&&(poll_status!=0)) 01412 { 01413 acs_pc.printf("Error in something else.Exiting.\n \r"); 01414 return 0; 01415 } 01416 01417 if(((mag_error ==1)&&(gyro_error==1))&&(poll_status!=0)) 01418 { 01419 acs_pc.printf("Error in both gyro and mag.Exiting.\n \r"); 01420 return 0; 01421 } 01422 01423 } 01424 01425 01426 cmd[0]=MAG_XOUT_H; //LSB of x 01427 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together 01428 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24); 01429 if(ack != 0) 01430 { 01431 cmd[0]=MAG_XOUT_H; //LSB of x 01432 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together 01433 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24); 01434 if(ack !=1) 01435 return 0; 01436 01437 } 01438 01439 01440 // acs_pc.printf("\nGyro Values:\n"); 01441 if (gyro_error!=1) 01442 { 01443 for(int i=0; i<3; i++) { 01444 //concatenating gyro LSB and MSB to get 16 bit signed data values 01445 actual_data.bit_data_acs_mg[i]= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i]; 01446 gyro_data[i]=(float)actual_data.bit_data_acs_mg[i]; 01447 gyro_data[i]=gyro_data[i]/senstivity_gyro; 01448 actual_data.AngularSpeed_actual[i] = gyro_data[i]; 01449 } 01450 } 01451 01452 if(mag_error!=1) 01453 { 01454 for(int i=0; i<3; i++) { 01455 //concatenating mag LSB and MSB to get 16 bit signed data values Extract data 01456 actual_data.bit_data_acs_mm[i]= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i]; 01457 01458 mag_data[i]=(float)actual_data.bit_data_acs_mm[i]; 01459 mag_data[i]=mag_data[i]/senstivity_mag; 01460 actual_data.Bvalue_actual[i] = mag_data[i]; 01461 } 01462 } 01463 01464 01465 if(mag_error == 1) 01466 { 01467 01468 acs_pc.printf("Gyro only successful.\n \r"); 01469 return 1; 01470 } 01471 if(gyro_error == 1) 01472 { 01473 acs_pc.printf("Mag only successful.\n \r"); 01474 return 2; 01475 } 01476 01477 //acs_pc.printf("Reading data success.\n \r"); 01478 return 3; 01479 } 01480 01481 01482 int FCTN_ATS_DATA_ACQ() 01483 { 01484 for(int i=0; i<3; i++) { 01485 actual_data.AngularSpeed_actual[i] = 0; 01486 actual_data.Bvalue_actual[i] = 0; 01487 } 01488 01489 int acq; 01490 int init; 01491 01492 //// acs_pc.printf("DATA_ACQ called \n \r"); 01493 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 01494 01495 01496 if(( (ACS_ATS_STATUS & 0xC0) == 0x40)) 01497 { 01498 timer_SENSOR_DATA_ACQ.start(); 01499 acq = SENSOR_DATA_ACQ(); 01500 timer_SENSOR_DATA_ACQ.stop(); 01501 //pc.pritnf("\n\r timer_SENSOR_DATA_ACQ is %f",timer_SENSOR_DATA_ACQ.read()); 01502 01503 01504 if(acq == 3) 01505 { 01506 01507 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 01508 01509 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 01510 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 01511 //// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r"); 01512 return 3; 01513 } 01514 else if((acq == 2)||(acq==1)) 01515 { 01516 acs_pc.printf(" Sensor 1 data partial success.Try other sensor.\n \r"); 01517 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) ) 01518 { 01519 //other sensor both working, off or 01520 //other sensor gyro working, this sensor not working , off 01521 //other sensor mag working, this sensor not working,off 01522 01523 ATS1_SW_ENABLE = 01; //switch off sensor 1 01524 wait_ms(5); 01525 if(acq == 1) 01526 { 01527 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status 01528 } 01529 if(acq==2) 01530 { 01531 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20; 01532 } 01533 01534 ATS2_SW_ENABLE = 0; //switch on sensor 2 01535 wait_ms(5); 01536 01537 timer_SENSOR_INIT.reset(); 01538 timer_SENSOR_INIT.start(); 01539 init = SENSOR_INIT(); 01540 timer_SENSOR_INIT.stop(); 01541 //sensor 2 init 01542 if( init == 0) 01543 { 01544 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); 01545 ATS2_SW_ENABLE = 1; 01546 wait_ms(5); 01547 ATS1_SW_ENABLE = 1; 01548 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1 01549 if(acq == 1) 01550 { 01551 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status 01552 } 01553 if(acq==2) 01554 { 01555 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01556 } 01557 return acq; 01558 } 01559 01560 int acq2; 01561 acq2 = SENSOR_DATA_ACQ(); 01562 if(acq2 == 3) 01563 { 01564 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 01565 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit 01566 return 3; 01567 } 01568 else if(acq2 == 1) 01569 { 01570 if(acq==2) 01571 { 01572 ATS2_SW_ENABLE = 1; 01573 wait_ms(5); 01574 ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only 01575 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; 01576 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01577 return 3; 01578 } 01579 else 01580 { 01581 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only 01582 return 1; 01583 } 01584 } 01585 01586 else if(acq2==2) //Sensor 2 mag only, exit in both cases 01587 { 01588 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01589 return 2; 01590 } 01591 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1 01592 { 01593 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); 01594 ATS2_SW_ENABLE = 1; 01595 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same 01596 ATS1_SW_ENABLE = 0; 01597 wait_ms(5); 01598 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40; 01599 return acq; 01600 } 01601 01602 } 01603 else //Sensor 2 not working or both sensors gyro/mag ONLY 01604 { 01605 if(acq == 1) 01606 { 01607 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq 01608 return 1; 01609 } 01610 if(acq==2) 01611 { 01612 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01613 return 2; 01614 } 01615 acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r"); 01616 return acq; 01617 01618 } 01619 } 01620 01621 else if(acq == 0) 01622 { 01623 acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all 01624 ATS1_SW_ENABLE = 0; 01625 wait_ms(5); //Switch ON sensor 2 01626 ATS2_SW_ENABLE = 0; 01627 wait_ms(5); 01628 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; 01629 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX 01630 { 01631 init = SENSOR_INIT(); 01632 if( init == 0) 01633 { 01634 acs_pc.printf(" Sensor 2 also data acq failure.\n \r"); 01635 ATS2_SW_ENABLE = 1; 01636 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit 01637 return 0; 01638 } 01639 01640 int acq2; 01641 acq2 = SENSOR_DATA_ACQ(); 01642 if(acq2 == 3) 01643 { 01644 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 01645 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working 01646 return 3; 01647 } 01648 else if(acq2 == 1) 01649 { 01650 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; 01651 return 1; 01652 } 01653 else if(acq2 == 2) 01654 { 01655 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01656 return 2; 01657 } 01658 else if(acq2 == 0) 01659 { 01660 acs_pc.printf(" Sensor 2 data acq failure..\n \r"); 01661 ATS2_SW_ENABLE = 1; 01662 01663 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; 01664 return 0; 01665 } 01666 01667 } 01668 01669 } 01670 01671 01672 } 01673 01674 if(( (ACS_ATS_STATUS & 0x0C) == 0x04)) 01675 { 01676 acq = SENSOR_DATA_ACQ(); //ATS2 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only 01677 if(acq == 3) //Both available read and exit 01678 { 01679 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 01680 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 01681 acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r"); 01682 return 3; 01683 } 01684 else if((acq == 2)||(acq==1)) //Only mag or only gyro 01685 { 01686 acs_pc.printf(" Sensor 2 data partial success.Try other sensor.\n \r"); 01687 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) ) 01688 { 01689 //other sensor both working, off or 01690 //other sensor gyro working, this sensor not working , off 01691 //other sensor mag working, this sensor not working,off 01692 ATS2_SW_ENABLE = 1; //switch off sensor 2 01693 wait_ms(5); 01694 if(acq == 1) 01695 { 01696 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status 01697 } 01698 if(acq==2) 01699 { 01700 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02; 01701 } 01702 01703 ATS1_SW_ENABLE = 1; //switch on sensor 1 01704 wait_ms(5); 01705 init = SENSOR_INIT(); //sensor 2 init 01706 01707 if( init == 0) 01708 { 01709 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r"); 01710 ATS1_SW_ENABLE = 0; 01711 wait_ms(5); 01712 ATS2_SW_ENABLE = 0; 01713 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2 01714 if(acq == 1) 01715 { 01716 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status 01717 } 01718 if(acq==2) 01719 { 01720 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01721 } 01722 return acq; 01723 } 01724 01725 int acq2; 01726 acq2 = SENSOR_DATA_ACQ(); 01727 01728 if(acq2 == 3) 01729 { 01730 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 01731 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit 01732 return 3; 01733 } 01734 01735 else if(acq2 == 1) 01736 { 01737 if(acq==2) 01738 { 01739 ATS1_SW_ENABLE = 0; 01740 wait_ms(5); 01741 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only 01742 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; 01743 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01744 return 3; 01745 } 01746 else 01747 { 01748 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only 01749 return 1; 01750 } 01751 } 01752 01753 else if(acq2==2) //Sensor 1 mag only, exit in both cases 01754 { 01755 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01756 return 2; 01757 } 01758 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2 01759 { 01760 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r"); 01761 ATS1_SW_ENABLE = 0; 01762 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same 01763 ATS2_SW_ENABLE = 0; 01764 wait_ms(5); 01765 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04; 01766 return acq; 01767 } 01768 01769 } 01770 else //Sensor 1 not working or both sensors gyro/mag ONLY 01771 { 01772 if(acq == 1) 01773 { 01774 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq 01775 return 1; 01776 } 01777 if(acq==2) 01778 { 01779 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01780 return 2; 01781 } 01782 acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r"); 01783 return acq; 01784 01785 } 01786 } 01787 else if(acq == 0) 01788 { 01789 acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all 01790 ATS2_SW_ENABLE = 1; 01791 wait_ms(5); //Switch ON sensor 1 01792 ATS1_SW_ENABLE = 1; 01793 wait_ms(5); 01794 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; 01795 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX 01796 { 01797 init = SENSOR_INIT(); 01798 if( init == 0) 01799 { 01800 acs_pc.printf(" Sensor 1 also data acq failure.\n \r"); 01801 ATS2_SW_ENABLE = 1; 01802 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit 01803 return 0; 01804 } 01805 01806 int acq2; 01807 acq2 = SENSOR_DATA_ACQ(); 01808 if(acq2 == 3) 01809 { 01810 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 01811 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working 01812 return 3; 01813 } 01814 else if(acq2 == 1) 01815 { 01816 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; 01817 return 1; 01818 } 01819 else if(acq2 == 2) 01820 { 01821 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01822 return 2; 01823 } 01824 else if(acq2 == 0) 01825 { 01826 acs_pc.printf(" Sensor 1 data acq failure..\n \r"); 01827 ATS1_SW_ENABLE = 0; 01828 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; 01829 return 0; 01830 } 01831 } 01832 } 01833 } 01834 01835 01836 01837 if(( (ACS_ATS_STATUS & 0xC0) == 0x00)) 01838 { 01839 ATS2_SW_ENABLE = 1; 01840 ATS1_SW_ENABLE = 0; 01841 wait_ms(10); 01842 acq = SENSOR_DATA_ACQ(); 01843 if(acq == 3) 01844 { 01845 01846 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 01847 01848 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 01849 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 01850 //// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r"); 01851 return 3; 01852 } 01853 else if((acq == 2)||(acq==1)) 01854 { 01855 acs_pc.printf(" Sensor 1 data partial success.Try other sensor.\n \r"); 01856 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) ) 01857 { 01858 //other sensor both working, off or 01859 //other sensor gyro working, this sensor not working , off 01860 //other sensor mag working, this sensor not working,off 01861 01862 ATS1_SW_ENABLE = 0; //switch off sensor 1 01863 wait_ms(5); 01864 if(acq == 1) 01865 { 01866 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status 01867 } 01868 if(acq==2) 01869 { 01870 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20; 01871 } 01872 01873 ATS2_SW_ENABLE = 0; //switch on sensor 2 01874 wait_ms(5); 01875 01876 init = SENSOR_INIT(); //sensor 2 init 01877 if( init == 0) 01878 { 01879 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); 01880 ATS2_SW_ENABLE = 1; 01881 wait_ms(5); 01882 ATS1_SW_ENABLE = 0; 01883 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1 01884 if(acq == 1) 01885 { 01886 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status 01887 } 01888 if(acq==2) 01889 { 01890 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01891 } 01892 return acq; 01893 } 01894 01895 int acq2; 01896 acq2 = SENSOR_DATA_ACQ(); 01897 if(acq2 == 3) 01898 { 01899 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 01900 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit 01901 return 3; 01902 } 01903 else if(acq2 == 1) 01904 { 01905 if(acq==2) 01906 { 01907 ATS2_SW_ENABLE = 1; 01908 wait_ms(5); 01909 ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only 01910 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; 01911 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01912 return 3; 01913 } 01914 else 01915 { 01916 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only 01917 return 1; 01918 } 01919 } 01920 01921 else if(acq2==2) //Sensor 2 mag only, exit in both cases 01922 { 01923 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01924 return 2; 01925 } 01926 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1 01927 { 01928 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); 01929 ATS2_SW_ENABLE = 1; 01930 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same 01931 ATS1_SW_ENABLE = 1; 01932 wait_ms(5); 01933 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40; 01934 return acq; 01935 } 01936 01937 } 01938 else //Sensor 2 not working or both sensors gyro/mag ONLY 01939 { 01940 if(acq == 1) 01941 { 01942 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq 01943 return 1; 01944 } 01945 if(acq==2) 01946 { 01947 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 01948 return 2; 01949 } 01950 acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r"); 01951 return acq; 01952 01953 } 01954 } 01955 01956 else if(acq == 0) 01957 { 01958 acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all 01959 ATS1_SW_ENABLE = 0; 01960 wait_ms(5); //Switch ON sensor 2 01961 ATS2_SW_ENABLE = 0; 01962 wait_ms(5); 01963 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; 01964 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX 01965 { 01966 init = SENSOR_INIT(); 01967 if( init == 0) 01968 { 01969 acs_pc.printf(" Sensor 2 also data acq failure.\n \r"); 01970 ATS2_SW_ENABLE = 1; 01971 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit 01972 return 0; 01973 } 01974 01975 int acq2; 01976 acq2 = SENSOR_DATA_ACQ(); 01977 if(acq2 == 3) 01978 { 01979 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 01980 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working 01981 return 3; 01982 } 01983 else if(acq2 == 1) 01984 { 01985 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; 01986 return 1; 01987 } 01988 else if(acq2 == 2) 01989 { 01990 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 01991 return 2; 01992 } 01993 else if(acq2 == 0) 01994 { 01995 acs_pc.printf(" Sensor 2 data acq failure..\n \r"); 01996 ATS2_SW_ENABLE = 1; 01997 01998 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; 01999 return 0; 02000 } 02001 02002 } 02003 02004 } 02005 02006 02007 } 02008 02009 if(( (ACS_ATS_STATUS & 0x0C) == 0x00)) 02010 { 02011 ATS1_SW_ENABLE = 1; 02012 ATS2_SW_ENABLE = 0; 02013 wait_ms(10); 02014 acq = SENSOR_DATA_ACQ(); //make ATS2 on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only 02015 if(acq == 3) //Both available read and exit 02016 { 02017 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; 02018 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 02019 acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r"); 02020 return 3; 02021 } 02022 else if((acq == 2)||(acq==1)) //Only mag or only gyro 02023 { 02024 acs_pc.printf(" Sensor 2 data partial success.Try other sensor.\n \r"); 02025 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) ) 02026 { 02027 //other sensor both working, off or 02028 //other sensor gyro working, this sensor not working , off 02029 //other sensor mag working, this sensor not working,off 02030 ATS2_SW_ENABLE = 1; //switch off sensor 2 02031 wait_ms(5); 02032 if(acq == 1) 02033 { 02034 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status 02035 } 02036 if(acq==2) 02037 { 02038 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02; 02039 } 02040 02041 ATS1_SW_ENABLE = 1; //switch on sensor 1 02042 wait_ms(5); 02043 init = SENSOR_INIT(); //sensor 2 init 02044 02045 if( init == 0) 02046 { 02047 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r"); 02048 ATS1_SW_ENABLE = 0; 02049 wait_ms(5); 02050 ATS2_SW_ENABLE = 0; 02051 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2 02052 if(acq == 1) 02053 { 02054 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status 02055 } 02056 if(acq==2) 02057 { 02058 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 02059 } 02060 return acq; 02061 } 02062 02063 int acq2; 02064 acq2 = SENSOR_DATA_ACQ(); 02065 02066 if(acq2 == 3) 02067 { 02068 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 02069 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit 02070 return 3; 02071 } 02072 02073 else if(acq2 == 1) 02074 { 02075 if(acq==2) 02076 { 02077 ATS1_SW_ENABLE = 0; 02078 wait_ms(5); 02079 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only 02080 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; 02081 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 02082 return 3; 02083 } 02084 else 02085 { 02086 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only 02087 return 1; 02088 } 02089 } 02090 02091 else if(acq2==2) //Sensor 1 mag only, exit in both cases 02092 { 02093 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 02094 return 2; 02095 } 02096 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2 02097 { 02098 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r"); 02099 ATS1_SW_ENABLE = 0; 02100 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same 02101 ATS2_SW_ENABLE = 0; 02102 wait_ms(5); 02103 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04; 02104 return acq; 02105 } 02106 02107 } 02108 else //Sensor 1 not working or both sensors gyro/mag ONLY 02109 { 02110 if(acq == 1) 02111 { 02112 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq 02113 return 1; 02114 } 02115 if(acq==2) 02116 { 02117 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; 02118 return 2; 02119 } 02120 acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r"); 02121 return acq; 02122 02123 } 02124 } 02125 else if(acq == 0) 02126 { 02127 acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all 02128 ATS2_SW_ENABLE = 1; 02129 wait_ms(5); //Switch ON sensor 1 02130 ATS1_SW_ENABLE = 1; 02131 wait_ms(5); 02132 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; 02133 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX 02134 { 02135 init = SENSOR_INIT(); 02136 if( init == 0) 02137 { 02138 acs_pc.printf(" Sensor 1 also data acq failure.\n \r"); 02139 ATS2_SW_ENABLE = 1; 02140 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit 02141 return 0; 02142 } 02143 02144 int acq2; 02145 acq2 = SENSOR_DATA_ACQ(); 02146 if(acq2 == 3) 02147 { 02148 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; 02149 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working 02150 return 3; 02151 } 02152 else if(acq2 == 1) 02153 { 02154 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; 02155 return 1; 02156 } 02157 else if(acq2 == 2) 02158 { 02159 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; 02160 return 2; 02161 } 02162 else if(acq2 == 0) 02163 { 02164 acs_pc.printf(" Sensor 1 data acq failure..\n \r"); 02165 ATS1_SW_ENABLE = 0; 02166 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; 02167 return 0; 02168 } 02169 } 02170 } 02171 } 02172 02173 02174 02175 02176 02177 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); 02178 acs_pc.printf(" Both sensors data acq failure.Exiting.\n \r"); 02179 return 0; 02180 } 02181 02182 void FCTN_ACS_GENPWM_MAIN(float Moment[3]) 02183 { 02184 //// printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function 02185 02186 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction 02187 float l_current_x=0; //Current sent in x TR's 02188 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction 02189 float l_current_y=0; //Current sent in y TR's 02190 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction 02191 float l_current_z=0; //Current sent in z TR's 02192 02193 02194 //// printf("\r\r"); 02195 02196 //----------------------------- x-direction TR --------------------------------------------// 02197 02198 02199 float l_moment_x = Moment[0]; //Moment in x direction 02200 02201 phase_TR_x = 1; // setting the default current direction 02202 if (l_moment_x <0) 02203 { 02204 phase_TR_x = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 02205 l_moment_x = abs(l_moment_x); 02206 } 02207 02208 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship 02209 //// printf("current in trx is %f \r \n",l_current_x); 02210 if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% 02211 { 02212 l_duty_cycle_x = 3*10000000*pow(l_current_x,3)- 90216*pow(l_current_x,2) + 697.78*l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation 02213 //// printf("DC for trx is %f \r \n",l_duty_cycle_x); 02214 PWM1.period(TIME_PERIOD); 02215 PWM1 = l_duty_cycle_x/100 ; 02216 } 02217 else if (l_current_x >= 0.0016 && l_current_x < 0.0171) 02218 { 02219 l_duty_cycle_x = - 76880*pow(l_current_x,3) + 1280.8*pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation 02220 //// printf("DC for trx is %f \r \n",l_duty_cycle_x); 02221 PWM1.period(TIME_PERIOD); 02222 PWM1 = l_duty_cycle_x/100 ; 02223 } 02224 else if(l_current_x >= 0.0171 && l_current_x < 0.1678) 02225 { 02226 l_duty_cycle_x = 275.92*pow(l_current_x,2) + 546.13*l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation 02227 //// printf("DC for trx is %f \r \n",l_duty_cycle_x); 02228 PWM1.period(TIME_PERIOD); 02229 PWM1 = l_duty_cycle_x/100 ; 02230 } 02231 else if(l_current_x==0) 02232 { 02233 //// printf("\n \r l_current_x====0"); 02234 l_duty_cycle_x = 0; // default value of duty cycle 02235 //// printf("DC for trx is %f \r \n",l_duty_cycle_x); 02236 PWM1.period(TIME_PERIOD); 02237 PWM1 = l_duty_cycle_x/100 ; 02238 } 02239 else //not necessary 02240 { 02241 g_err_flag_TR_x = 1; 02242 } 02243 acs_pc.printf("DC for trx is %f \r \n",l_duty_cycle_x); 02244 02245 02246 //----------------------------- y-direction TR --------------------------------------------// 02247 02248 02249 float l_moment_y = Moment[1]; //Moment in y direction 02250 02251 phase_TR_y = 1; // setting the default current direction 02252 if (l_moment_y <0) 02253 { 02254 phase_TR_y = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 02255 l_moment_y = abs(l_moment_y); 02256 } 02257 02258 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship 02259 //// printf("current in try is %f \r \n",l_current_y); 02260 if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% 02261 { 02262 l_duty_cycle_y = 3*10000000*pow(l_current_y,3)- 90216*pow(l_current_y,2) + 697.78*l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation 02263 //// printf("DC for try is %f \r \n",l_duty_cycle_y); 02264 PWM2.period(TIME_PERIOD); 02265 PWM2 = l_duty_cycle_y/100 ; 02266 } 02267 else if (l_current_y >= 0.0016 && l_current_y < 0.0171) 02268 { 02269 l_duty_cycle_y = - 76880*pow(l_current_y,3) + 1280.8*pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation 02270 //// printf("DC for try is %f \r \n",l_duty_cycle_y); 02271 PWM2.period(TIME_PERIOD); 02272 PWM2 = l_duty_cycle_y/100 ; 02273 } 02274 else if(l_current_y >= 0.0171 && l_current_y < 0.1678) 02275 { 02276 l_duty_cycle_y = 275.92*pow(l_current_y,2) + 546.13*l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation 02277 //// printf("DC for try is %f \r \n",l_duty_cycle_y); 02278 PWM2.period(TIME_PERIOD); 02279 PWM2 = l_duty_cycle_y/100 ; 02280 } 02281 else if(l_current_y==0) 02282 { 02283 //// printf("\n \r l_current_y====0"); 02284 l_duty_cycle_y = 0; // default value of duty cycle 02285 //// printf("DC for try is %f \r \n",l_duty_cycle_y); 02286 PWM2.period(TIME_PERIOD); 02287 PWM2 = l_duty_cycle_y/100 ; 02288 } 02289 else //not necessary 02290 { 02291 g_err_flag_TR_y = 1; 02292 } 02293 acs_pc.printf("DC for try is %f \r \n",l_duty_cycle_y); 02294 //----------------------------------------------- z-direction TR -------------------------// 02295 02296 02297 02298 02299 float l_moment_z = Moment[2]; //Moment in z direction 02300 02301 phase_TR_z = 1; // setting the default current direction 02302 if (l_moment_z <0) 02303 { 02304 phase_TR_z = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 02305 l_moment_z = abs(l_moment_z); 02306 } 02307 02308 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship 02309 //// printf("current in trz is %f \r \n",l_current_z); 02310 if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% 02311 { 02312 l_duty_cycle_z = 3*10000000*pow(l_current_z,3)- 90216*pow(l_current_z,2) + 697.78*l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation 02313 //// printf("DC for trz is %f \r \n",l_duty_cycle_z); 02314 PWM3.period(TIME_PERIOD); 02315 PWM3 = l_duty_cycle_z/100 ; 02316 } 02317 else if (l_current_z >= 0.0016 && l_current_z < 0.0171) 02318 { 02319 l_duty_cycle_z = - 76880*pow(l_current_z,3) + 1280.8*pow(l_current_z,2) + 583.78*l_current_z + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation 02320 //// printf("DC for trz is %f \r \n",l_duty_cycle_z); 02321 PWM3.period(TIME_PERIOD); 02322 PWM3 = l_duty_cycle_z/100 ; 02323 } 02324 else if(l_current_z >= 0.0171 && l_current_z < 0.1678) 02325 { 02326 l_duty_cycle_z = 275.92*pow(l_current_z,2) + 546.13*l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation 02327 //// printf("DC for trz is %f \r \n",l_duty_cycle_z); 02328 PWM3.period(TIME_PERIOD); 02329 PWM3 = l_duty_cycle_z/100 ; 02330 } 02331 else if(l_current_z==0) 02332 { 02333 //// printf("\n \r l_current_z====0"); 02334 l_duty_cycle_z = 0; // default value of duty cycle 02335 //// printf("DC for trz is %f \r \n",l_duty_cycle_z); 02336 PWM3.period(TIME_PERIOD); 02337 PWM3 = l_duty_cycle_z/100 ; 02338 } 02339 else //not necessary 02340 { 02341 g_err_flag_TR_z = 1; 02342 } 02343 acs_pc.printf("DC for trz is %f \r \n",l_duty_cycle_z); 02344 02345 //changed 02346 if(phase_TR_x) 02347 ACS_TR_X_PWM = float_to_uint8(-1,1,PWM1); 02348 else 02349 ACS_TR_X_PWM = float_to_uint8(-1,1,-PWM1); 02350 if(phase_TR_y) 02351 ACS_TR_Y_PWM = float_to_uint8(-1,1,PWM2); 02352 else 02353 ACS_TR_Y_PWM = float_to_uint8(-1,1,-PWM2); 02354 if(phase_TR_z) 02355 ACS_TR_Z_PWM = float_to_uint8(-1,1,PWM3); 02356 else 02357 ACS_TR_Z_PWM = float_to_uint8(-1,1,-PWM3); 02358 02359 //-----------------------------------------exiting the function-----------------------------------// 02360 02361 //// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function 02362 02363 } 02364
Generated on Tue Jul 19 2022 10:57:30 by
1.7.2
