latest BAE code 10 dec

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of RAJANGAM_REVIEW_BAE_CODE by Team Fox

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers ACS.cpp Source File

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