ACS.cpp

00001 /*------------------------------------------------------------------------------------------------------------------------------------------------------
00002 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
00003 #include <mbed.h>
00004 #include <math.h>
00005 
00006 #include "pni.h" //pni header file
00007 #include "pin_config.h"
00008 #include "ACS.h"
00009 #include "EPS.h"
00010 
00011 //********************************flags******************************************//
00012 extern uint32_t BAE_STATUS;
00013 extern uint32_t BAE_ENABLE;
00014 extern uint8_t ACS_INIT_STATUS;
00015 extern uint8_t ACS_DATA_ACQ_STATUS;
00016 extern uint8_t ACS_ATS_STATUS;
00017 extern uint8_t ACS_MAIN_STATUS;
00018 extern uint8_t ACS_STATUS;
00019 extern uint8_t ACS_DETUMBLING_ALGO_TYPE;
00020 
00021 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
00022 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
00023 
00024 extern uint8_t ACS_ATS_ENABLE;
00025 extern uint8_t ACS_DATA_ACQ_ENABLE;
00026 extern uint8_t ACS_STATE;
00027 
00028 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
00029 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
00030 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
00031 
00032 extern PwmOut PWM1; //x                         //Functions used to generate PWM signal 
00033 extern PwmOut PWM2; //y
00034 extern PwmOut PWM3; //z                         //PWM output comes from pins p6
00035 
00036 int g_err_flag_TR_x=0;       // setting x-flag to zero
00037 int g_err_flag_TR_y=0;       // setting y-flag to zero
00038 int g_err_flag_TR_z=0;       // setting z-flag to zero
00039 
00040 extern float data[6];
00041 extern BAE_HK_actual actual_data;
00042 
00043 
00044 Serial pc_acs(USBTX,USBRX); //for usb communication
00045 //CONTROL_ALGO
00046 
00047 float moment[3]; // Unit: Ampere*Meter^2
00048 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5};  // Unit: Tesla
00049 float db[3];
00050 uint8_t flag_firsttime=1, alarmmode=0;
00051 
00052 
00053 
00054 
00055 void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1);
00056 float max_array(float arr[3]);
00057 void inverse(float mat[3][3],float inv[3][3]);
00058 
00059 //CONTROLALGO PARAMETERS
00060 
00061 
00062 
00063 
00064 void FCTN_ACS_CNTRLALGO ( float b[3] , float omega[3],int nominal)
00065 {
00066 
00067     float normalising_fact;
00068     float b_copy[3], omega_copy[3], db_copy[3];
00069     int i;
00070     if(flag_firsttime==1)
00071         {
00072             for(i=0;i<3;i++)
00073         {
00074                 db[i]=0; // Unit: Tesla/Second
00075         }
00076             flag_firsttime=0;
00077         }
00078     else
00079     {
00080         for(i=0;i<3;i++)
00081         {
00082             db[i]= (b[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
00083         }
00084     }
00085     
00086         if(nominal == 0)
00087         
00088         {
00089     
00090                 if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
00091             {
00092                     alarmmode=0;
00093             }
00094                 else if(max_array(omega)>OmegaMax&& alarmmode==0)
00095             {
00096                     alarmmode=1;
00097             }
00098             
00099         }
00100         
00101     
00102 
00103 
00104     for (i=0;i<3;i++)
00105     {
00106         b_copy[i]=b[i];
00107         db_copy[i]=db[i];
00108         omega_copy[i]=omega[i];
00109     }
00110 
00111     if((alarmmode==0)|| (nominal == 1))
00112         {
00113             controlmodes(b,db,omega,0);
00114         for (i=0;i<3;i++)
00115         {
00116             b[i]=b_copy[i];
00117             db[i]=db_copy[i];
00118             omega[i]=omega_copy[i];
00119         }
00120             if(max_array(moment)>MmntMax)
00121             {
00122                 controlmodes(b,db,omega,1);
00123             for (i=0;i<3;i++)
00124             {
00125                 b[i]=b_copy[i];
00126                 db[i]=db_copy[i];
00127                 omega[i]=omega_copy[i];
00128             }
00129                 if(max_array(moment)>MmntMax)
00130                 {
00131                     normalising_fact=max_array(moment)/MmntMax;
00132                     for(i=0;i<3;i++)
00133                 {
00134                         moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
00135                 }
00136                 }
00137             }
00138             ACS_STATUS = 5;
00139         }
00140         else
00141         {   
00142             controlmodes(b,db,omega,1);
00143         for (i=0;i<3;i++)
00144         {
00145             b[i]=b_copy[i];
00146             db[i]=db_copy[i];
00147             omega[i]=omega_copy[i];
00148         }
00149             if(max_array(moment)>MmntMax)
00150             {
00151                 normalising_fact=max_array(moment)/MmntMax;
00152                 for(i=0;i<3;i++)
00153             {
00154                     moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
00155             }
00156             }
00157         
00158         }
00159     for (i=0;i<3;i++)
00160     {
00161         b_old[i]=b[i];
00162     }
00163 }
00164 
00165 void inverse(float mat[3][3],float inv[3][3],int &singularity_flag)
00166 {
00167     int i,j;
00168     float det=0;
00169     for(i=0;i<3;i++)
00170     { 
00171         for(j=0;j<3;j++)
00172         {
00173             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]);
00174         }
00175     }
00176     det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
00177     if (det==0)
00178     {
00179         singularity_flag=1;
00180     }
00181     else
00182     {
00183         singularity_flag=0;
00184         for(i=0;i<3;i++)
00185         {
00186             for(j=0;j<3;j++)
00187             {
00188                 inv[i][j]/=det;
00189             }
00190         }
00191     }
00192 }
00193 
00194 float max_array(float arr[3])
00195 {
00196     int i;
00197     float temp_max=fabs(arr[0]);
00198     for(i=1;i<3;i++)
00199     {
00200         if(fabs(arr[i])>temp_max)
00201         {
00202             temp_max=fabs(arr[i]);
00203         }
00204     }
00205     return temp_max;
00206 }
00207 
00208 
00209 void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1)
00210 {
00211     float bb[3]={0,0,0};
00212     float d[3]={0,0,0};
00213     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
00214     float den=0,den2;
00215     float bcopy[3];
00216     int i, j;//temporary variables
00217     float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
00218     float invJm[3][3];
00219     float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
00220     int singularity_flag=0;
00221     
00222     if(controlmode1==0)
00223     {
00224         den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
00225         den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
00226         if (den==0)
00227         {
00228             singularity_flag=1;
00229         }
00230         if (singularity_flag==0)
00231         {
00232             for(i=0;i<3;i++)
00233             {
00234                 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
00235             }
00236             for(i=0;i<3;i++)
00237             {
00238                 b[i]/=den; // Mormalized b. Hence no unit.
00239             }
00240             if(b[2]>0.9 || b[2]<-0.9)
00241             {
00242                 kz=kz2;
00243                 kmu=kmu2;
00244                 gamma=gamma2;
00245             }
00246             for(i=0;i<2;i++)
00247             {
00248                 Mu[i]=b[i];
00249                 v[i]=-kmu*Mu[i];
00250                 dv[i]=-kmu*db[i];
00251                 z[i]=db[i]-v[i];
00252                 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
00253             }
00254             inverse(Jm,invJm,singularity_flag);
00255             for(i=0;i<3;i++)
00256             {
00257                 for(j=0;j<3;j++)
00258                 {
00259                     bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
00260                 }
00261             }
00262             for(i=0;i<3;i++)
00263             {
00264                 for(j=0;j<3;j++)
00265                 {
00266                     d[i]+=bb[j]*invJm[i][j];
00267                 }
00268             }
00269             bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
00270             bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
00271             bb[0]=0;
00272             for(i=0;i<3;i++)
00273             {
00274                 d[i]=invJm[2][i];
00275                 invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
00276                 invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
00277                 invJm[0][i]=b[i];
00278             }
00279             inverse(invJm,Jm,singularity_flag);
00280             if (singularity_flag==0)
00281             {
00282                 for(i=0;i<3;i++)
00283                 {
00284                     for(j=0;j<3;j++)
00285                     {
00286                         tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
00287                     }
00288                 }
00289                 for(i=0;i<3;i++)
00290                 {
00291                     bcopy[i]=b[i]*den;
00292                 }
00293                 for(i=0;i<3;i++)
00294                 {
00295                     Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
00296                     Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
00297                 }
00298             }
00299         }
00300         if (singularity_flag==1)
00301         {
00302             for (i=0;i<3;i++)
00303             {
00304                 Mmnt[i]=2*MmntMax;
00305             }
00306         }
00307     }
00308     else if(controlmode1==1)
00309     {
00310         if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo
00311         {
00312             for(i=0;i<3;i++)
00313             {
00314                 Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
00315             }
00316             ACS_STATUS = 6;
00317         }
00318         else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
00319         {
00320             for(i=0;i<3;i++)
00321             {
00322                 Mmnt[i]=-kdetumble*db[i];
00323             }
00324             ACS_STATUS = 4;
00325         }
00326     }
00327     for(i=0;i<3;i++)
00328     {
00329         moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
00330     }
00331 }
00332 
00333 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl  for the attitude sensors and battery gauge
00334 
00335 int FCTN_ACS_INIT(); //initialization of registers happens
00336 int SENSOR_INIT();
00337 int FCTN_ATS_DATA_ACQ(); //data is obtained
00338 int SENSOR_DATA_ACQ();
00339 void T_OUT(); //timeout function to stop infinite loop
00340 
00341 int CONFIG_UPLOAD();
00342 Timeout to; //Timeout variable to
00343 int toFlag; 
00344 
00345 int count =0; // Time for which the BAE uC is running (in seconds)
00346 void T_OUT()
00347 {
00348     toFlag=0; //as T_OUT function gets called the while loop gets terminated
00349 }
00350 
00351 
00352 //DEFINING VARIABLES
00353 char cmd[2];
00354 char raw_gyro[6];
00355 char raw_mag[6];
00356 char reg_data[24];
00357 char store,status;
00358 int16_t bit_data;
00359 uint16_t time_data;
00360 float gyro_data[3], mag_data[3];
00361 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
00362 
00363 int CONFIG_UPLOAD()
00364 {
00365     cmd[0]=RESETREQ;
00366     cmd[1]=BIT_RESREQ;
00367     i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
00368     wait_ms(600);
00369     
00370     //Verify magic number
00371     
00372     cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload
00373     cmd[1]=BIT_HOST_UPLD_ENB;
00374     i2c.write(SLAVE_ADDR,cmd,2);
00375     wait_ms(100);
00376     
00377     cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload
00378     cmd[1]=0x0000;
00379     i2c.write(SLAVE_ADDR,cmd,3);
00380     wait_ms(100);
00381     
00382     
00383     
00384     
00385     cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload
00386     cmd[1]=0x00;
00387     i2c.write(SLAVE_ADDR,cmd,2);
00388     wait_ms(100);
00389     
00390     return 0;
00391     
00392 }
00393 
00394 int SENSOR_INIT()
00395 {
00396     
00397     pc_acs.printf("Entered sensor init\n \r");
00398     cmd[0]=RESETREQ;
00399     cmd[1]=BIT_RESREQ;
00400     i2c.write(SLAVE_ADDR,cmd,2);                                //When 0x01 is written in reset request register Emulates a hard power down/power up
00401     wait_ms(600);                                               //waiting for loading configuration file stored in EEPROM
00402     cmd[0]=SENTRALSTATUS;
00403     i2c.write(SLAVE_ADDR,cmd,1);
00404     i2c.read(SLAVE_ADDR_READ,&store,1);
00405     
00406     pc_acs.printf("Sentral Status is %x\n \r",(int)store);
00407     
00408     //to check whether EEPROM is uploaded properly
00409     switch((int)store) { 
00410         case(3): {
00411             break;
00412         }
00413         case(11): {
00414             break;
00415         }
00416         default: {
00417             cmd[0]=RESETREQ;
00418             cmd[1]=BIT_RESREQ;
00419             i2c.write(SLAVE_ADDR,cmd,2);
00420             wait_ms(600);
00421             
00422             cmd[0]=SENTRALSTATUS;
00423             i2c.write(SLAVE_ADDR,cmd,1);
00424             i2c.read(SLAVE_ADDR_READ,&store,1);
00425             wait_ms(100);
00426             pc_acs.printf("Sentral Status is %x\n \r",(int)store);
00427             
00428         }
00429     }
00430 
00431     
00432     int manual=0;
00433     if( ((int)store != 11 )&&((int)store != 11))
00434     {
00435 
00436             cmd[0]=RESETREQ;
00437             cmd[1]=BIT_RESREQ;
00438             i2c.write(SLAVE_ADDR,cmd,2);
00439             wait_ms(600);
00440             
00441             manual = CONFIG_UPLOAD();
00442             
00443             if(manual == 0)
00444             {
00445                 //MANUAL CONFIGURATION FAILED
00446                 return 0;
00447             }
00448                      
00449     }
00450     
00451         cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
00452         cmd[1]=BIT_RUN_ENB;
00453         i2c.write(SLAVE_ADDR,cmd,2);
00454         
00455         cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
00456         cmd[1]=BIT_MAGODR;
00457         i2c.write(SLAVE_ADDR,cmd,2);
00458         
00459         cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
00460         cmd[1]=BIT_GYROODR;
00461         i2c.write(SLAVE_ADDR,cmd,2);
00462         
00463         cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
00464         cmd[1]=0x00;
00465         
00466         i2c.write(SLAVE_ADDR,cmd,2);
00467         wait_ms(100);
00468         cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
00469         cmd[1]=0x00;
00470         i2c.write(SLAVE_ADDR,cmd,2);
00471         
00472         cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
00473         cmd[1]=BIT_EVT_ENB;
00474         i2c.write(SLAVE_ADDR,cmd,2);
00475         
00476         cmd[0]=SENTRALSTATUS;
00477         i2c.write(SLAVE_ADDR,cmd,1);
00478         i2c.read(SLAVE_ADDR_READ,&store,1);
00479         pc_acs.printf("Sentral Status after initialising is %x\n \r",(int)store);
00480         
00481         if( (int)store == 3)      //Check if initialised properly and not in idle state
00482         {
00483             pc_acs.printf("Exited sensor init successfully\n \r");  
00484             return 1;
00485         }
00486         
00487         else if((int)store == 11)
00488         {
00489             cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
00490             cmd[1]=BIT_RUN_ENB;
00491             i2c.write(SLAVE_ADDR,cmd,2);
00492             wait_ms(100);
00493             
00494             cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
00495             cmd[1]=BIT_MAGODR;
00496             i2c.write(SLAVE_ADDR,cmd,2);
00497             wait_ms(100);
00498             cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
00499             cmd[1]=BIT_GYROODR;
00500             i2c.write(SLAVE_ADDR,cmd,2);
00501             wait_ms(100);
00502             cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
00503             cmd[1]=0x00;
00504             wait_ms(100);
00505             
00506             i2c.write(SLAVE_ADDR,cmd,2);
00507             wait_ms(100);
00508             cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
00509             cmd[1]=0x00;
00510             i2c.write(SLAVE_ADDR,cmd,2);
00511             wait_ms(100);
00512             
00513             cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
00514             cmd[1]=BIT_EVT_ENB;
00515             i2c.write(SLAVE_ADDR,cmd,2);
00516             wait_ms(100);
00517             
00518             cmd[0]=SENTRALSTATUS;
00519             i2c.write(SLAVE_ADDR,cmd,1);
00520             i2c.read(SLAVE_ADDR_READ,&store,1);
00521             wait_ms(100);
00522             pc_acs.printf("Sentral Status after trying again is %x\n \r",(int)store);
00523             
00524             if( (int)store != 3)
00525                 {
00526                     pc_acs.printf("Problem with initialising\n \r");
00527                     return 0;
00528                 }
00529         }
00530         
00531         pc_acs.printf("Sensor init failed \n \r") ;
00532         return 0;
00533 }
00534 
00535 int FCTN_ACS_INIT()
00536 {
00537     ACS_INIT_STATUS = 1;     //set ACS_INIT_STATUS flag
00538 
00539     
00540     int working=0;
00541 
00542     pc_acs.printf("Attitude sensor init called \n \r");
00543     pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
00544     
00545     
00546     if((  (ACS_ATS_STATUS & 0xC0) != 0xC0)&&(  (ACS_ATS_STATUS & 0xC0) != 0x80))                  //Sensor1 status is not 10 or 11
00547     {
00548 
00549         pc_acs.printf("Sensor 1 marked working \n \r");
00550         working = SENSOR_INIT();
00551         if(working ==1)
00552             {
00553                 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
00554                 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);                     //Sensor 1 INIT successful
00555                 pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
00556                 ACS_INIT_STATUS = 0;
00557                 return 1;
00558             }
00559             
00560             
00561             
00562             pc_acs.printf("Sensor 1 not working.Powering off.\n \r");                             //Sensor 1 INIT failure and power off
00563             ATS1_SW_ENABLE = 1;
00564             ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
00565   
00566     }
00567     
00568     pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
00569     
00570     if((  (ACS_ATS_STATUS & 0x0C) != 0x0C)&&(  (ACS_ATS_STATUS & 0x0C) != 0x08))                //Sensor1 status is not 10 or 11
00571     {
00572                         
00573             
00574             ATS2_SW_ENABLE = 0;
00575             wait_ms(5);
00576             working = SENSOR_INIT();
00577             if(working ==1)
00578             {
00579                 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
00580                 pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r");    //Sensor2 INIT successful
00581                 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
00582                 ACS_INIT_STATUS = 0;
00583                 return 2;
00584             }
00585         
00586         
00587     }
00588     
00589     pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
00590     pc_acs.printf("Sensor 2 also not working.Exit init.\n \r");
00591     
00592     
00593     
00594     ATS2_SW_ENABLE = 1;
00595     ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
00596     ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag                                                              //Sensor 2 also not working
00597     return 0;
00598 }
00599 
00600 
00601 int SENSOR_DATA_ACQ()
00602 {
00603         int mag_only=0;
00604         pc_acs.printf("Entering Sensor data acq.\n \r");
00605         char status;
00606         
00607         int sentral;
00608         int event;
00609         int sensor;
00610         int error;
00611         int init;
00612         
00613         int ack1;
00614         int ack2;
00615         
00616         cmd[0]=EVT_STATUS;
00617         i2c.write(SLAVE_ADDR,cmd,1);
00618         ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
00619         //wait_ms(100);
00620         event = (int)status; 
00621         
00622         cmd[0]=SENTRALSTATUS;
00623         i2c.write(SLAVE_ADDR,cmd,1);
00624         ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
00625         
00626         printf("Ack1:  %d   Ack2 : %d\n",ack1,ack2);
00627         
00628         if((ack1!=0)||(ack2!=0))
00629         {
00630                 cmd[0]=EVT_STATUS;
00631                 i2c.write(SLAVE_ADDR,cmd,1);
00632                 ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
00633                 //wait_ms(100);
00634                 cmd[0]=SENTRALSTATUS;
00635                 i2c.write(SLAVE_ADDR,cmd,1);
00636                 ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
00637                 if((ack1!=0)||(ack2!=0))
00638                     {           
00639                         for(int i=0; i<3; i++) {
00640                             actual_data.AngularSpeed_actual[i] = 0;        //acknowledgement failed
00641                             actual_data.Bvalue_actual[i] = 0;
00642                          }
00643                         
00644                         return 1;
00645                     }
00646         }
00647         
00648         sentral = (int) status;
00649         
00650         pc_acs.printf("Event Status is %x\n \r",event);
00651         pc_acs.printf("Sentral Status is %x\n \r",sentral);
00652           
00653         
00654         
00655         if  ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )|| (sentral!= 3))    //check for any error in event status register
00656         {
00657             
00658             
00659             init = SENSOR_INIT();
00660             
00661             
00662             int ack1,ack2;
00663             cmd[0]=EVT_STATUS;
00664             i2c.write(SLAVE_ADDR,cmd,1);
00665             ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
00666             //wait_ms(100);
00667             event = (int)status; 
00668             
00669             cmd[0]=SENTRALSTATUS;
00670             i2c.write(SLAVE_ADDR,cmd,1);
00671             ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
00672             sentral = (int)status;
00673             
00674             if((ack1!=0)||(ack2!=0))
00675             {
00676                     cmd[0]=EVT_STATUS;
00677                     i2c.write(SLAVE_ADDR,cmd,1);
00678                     ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
00679                     event = (int)status; 
00680                     wait_ms(100);
00681                     cmd[0]=SENTRALSTATUS;
00682                     i2c.write(SLAVE_ADDR,cmd,1);
00683                     ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
00684                     sentral = (int)status;
00685                     wait_ms(100);
00686                     
00687                     if((ack1!=0)||(ack2!=0))
00688                         {           
00689                             for(int i=0; i<3; i++) {
00690                                 actual_data.AngularSpeed_actual[i] = 0;        //acknowledgement failed
00691                                 actual_data.Bvalue_actual[i] = 0;
00692                              }
00693                             
00694                             return 1;
00695                         }
00696             }
00697             
00698             pc_acs.printf("Event Status after resetting and init is %x\n \r",event);
00699             
00700             if  ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 ) || (init == 0)||(sentral != 3))    //check for any error in event status     
00701             {
00702                 
00703                     int ack1,ack2;
00704                     char status;
00705                     cmd[0]=ERROR;
00706                     i2c.write(SLAVE_ADDR,cmd,1);
00707                     ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
00708                     error = (int)status; 
00709 
00710                     cmd[0]=SENSORSTATUS;
00711                     i2c.write(SLAVE_ADDR,cmd,1);
00712                     ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
00713                     sensor = (int)status;
00714 
00715                     
00716                     if((ack1!=0)||(ack2!=0))
00717                     {
00718                         cmd[0]=ERROR;
00719                         i2c.write(SLAVE_ADDR,cmd,1);
00720                         ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
00721                         error = (int)status; 
00722                         wait_ms(100);
00723                         
00724                         cmd[0]=SENSORSTATUS;
00725                         i2c.write(SLAVE_ADDR,cmd,1);
00726                         ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
00727                         sensor = (int)status;
00728                         wait_ms(100);
00729                         
00730                         if((ack1!=0)||(ack2!=0))
00731                             {           
00732                                 for(int i=0; i<3; i++) {
00733                                     actual_data.AngularSpeed_actual[i] = 0;        //acknowledgement failed
00734                                     actual_data.Bvalue_actual[i] = 0;
00735                                  }
00736                                 
00737                                 return 1;
00738                             }
00739                     }
00740                     
00741                     
00742                     
00743                     if((error!=0) || (sensor!=0))
00744                      {
00745                                 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16)  )
00746                                      {
00747                                                 
00748                                             if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64)  )
00749                                                   {
00750                                                          
00751                                                         for(int i=0; i<3; i++) {
00752                                                             actual_data.AngularSpeed_actual[i] = 0;        //Set values to 0
00753                                                             actual_data.Bvalue_actual[i] = 0;
00754                                                          }
00755                                                          
00756                                                          pc_acs.printf("error in both sensors.Exiting.\n \r");
00757                                                          return 1;
00758                                                   }
00759                                              pc_acs.printf("error in gyro alone.Exiting.\n \r");
00760                             
00761                                             for(int i=0; i<3; i++) {
00762                                                 actual_data.AngularSpeed_actual[i] = 0;        //Set values to 0
00763                                                 }
00764                                             
00765                                             mag_only = 1; 
00766                                             //return 2;
00767                                       }  
00768                                       
00769                                       if(mag_only!= 1){
00770                                         pc_acs.printf("error in something else.Exiting.\n \r");  
00771                                         for(int i=0; i<3; i++) {
00772                                                 actual_data.AngularSpeed_actual[i] = 0;        //Set values to 0
00773                                                 actual_data.Bvalue_actual[i] = 0;
00774                                             }
00775                                         return 1;  
00776                                         }                     
00777                      }
00778                      
00779                      if((event & 1 == 1 ))
00780                      {
00781                          pc_acs.printf("error in CPU Reset.\n \r");
00782                         for(int i=0; i<3; i++) {
00783                             actual_data.AngularSpeed_actual[i] = 0;        //Set values to 0
00784                             actual_data.Bvalue_actual[i] = 0;
00785                             }
00786                             return 1;
00787                          
00788                       }
00789                       
00790                     if((event & 8 != 8 )||(event & 32 != 32 ))
00791                         {
00792                             pc_acs.printf("Data not ready waiting...\n \r");
00793                             //POLL
00794                             wait_ms(1000);
00795                             cmd[0]=EVT_STATUS;
00796                             i2c.write(SLAVE_ADDR,cmd,1);
00797                             i2c.read(SLAVE_ADDR_READ,&status,1);
00798                             wait_ms(100);
00799                             if((event & 8 != 8 )||(event & 32 != 32 ))
00800                             {
00801 
00802                                 if(event & 32 != 32 )
00803                                 {
00804                                     if(event & 8 != 8 )
00805                                     {
00806                                         pc_acs.printf("Both data still not ready.Exiting..\n \r");
00807                                         
00808                                         for(int i=0; i<3; i++) {
00809                                         actual_data.AngularSpeed_actual[i] = 0;        //Set values to 0
00810                                         actual_data.Bvalue_actual[i] = 0;
00811                                          }
00812                                         return 1;
00813                                     }
00814                                     
00815                                     pc_acs.printf("Mag data only ready.Read..\n \r");
00816                                     mag_only = 1;
00817                                     //return 2;
00818                                     
00819                                 }
00820                                 
00821                                 
00822                             }
00823                             
00824                                              
00825                         } 
00826                         
00827      
00828             }
00829             
00830                 if(mag_only !=1)
00831                 {
00832                         if(mag_only!= 1){
00833                         pc_acs.printf("Error in something else.Exiting.\n \r");  
00834                         for(int i=0; i<3; i++) {
00835                                 actual_data.AngularSpeed_actual[i] = 0;        //Set values to 0
00836                                 actual_data.Bvalue_actual[i] = 0;
00837                             }
00838                         return 1;  
00839                 } 
00840                 
00841             }
00842             
00843             
00844             
00845                     
00846             
00847         }
00848         
00849             
00850             
00851             cmd[0]=MAG_XOUT_H; //LSB of x
00852             i2c.write(SLAVE_ADDR,cmd,1);                        //Read gryo and mag registers together
00853             ack1 = i2c.read(SLAVE_ADDR_READ,reg_data,24);
00854             
00855             if(ack1 != 0)
00856             {
00857                 wait_ms(100);
00858                 cmd[0]=MAG_XOUT_H; //LSB of x
00859                 i2c.write(SLAVE_ADDR,cmd,1);                        //Read gryo and mag registers together
00860                 ack1 = i2c.read(SLAVE_ADDR_READ,reg_data,24);
00861                 wait_ms(100);
00862                 if(ack1 !=1)
00863                 {
00864                         for(int i=0;i<3;i++)
00865                         {
00866                             actual_data.Bvalue_actual[i] = 0;
00867                             actual_data.AngularSpeed_actual[i] = 0;
00868                         }
00869                         return 1;
00870                 }
00871                 
00872             }
00873             
00874             
00875         //    pc_acs.printf("\nGyro Values:\n");
00876             for(int i=0; i<3; i++) {
00877                 //concatenating gyro LSB and MSB to get 16 bit signed data values
00878                 bit_data= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i]; 
00879                 gyro_data[i]=(float)bit_data;
00880                 gyro_data[i]=gyro_data[i]/senstivity_gyro;
00881                 gyro_data[i]+=gyro_error[i];
00882             }
00883             
00884             for(int i=0; i<3; i++) {
00885                 //concatenating mag LSB and MSB to get 16 bit signed data values                      Extract data
00886                 bit_data= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i]; 
00887                 mag_data[i]=(float)bit_data;
00888                 mag_data[i]=mag_data[i]/senstivity_mag;
00889                 mag_data[i]+=mag_error[i];
00890             }
00891             for(int i=0; i<3; i++) {
00892                // data[i]=gyro_data[i];
00893                 actual_data.AngularSpeed_actual[i] = gyro_data[i];
00894                 actual_data.Bvalue_actual[i] = mag_data[i];
00895             }
00896                 
00897 
00898        
00899         if(mag_only == 0)
00900         {
00901  
00902           pc_acs.printf("Reading data successful.\n \r");
00903           return 0;
00904         }
00905         else if(mag_only == 1)
00906         {
00907                 
00908                 for(int i=0;i<3;i++)
00909                 {
00910                     actual_data.AngularSpeed_actual[i] = 0;
00911                 }
00912                 
00913                 
00914                 pc_acs.printf("Reading data partial success.\n \r");
00915                 return 2;
00916         }
00917         
00918             pc_acs.printf("Reading data success.\n \r");
00919             return 0;
00920         
00921 }
00922 
00923 
00924 
00925 int FCTN_ATS_DATA_ACQ()
00926 {
00927     
00928     int acq;
00929     
00930     pc_acs.printf("DATA_ACQ  called \n \r");
00931     pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
00932     
00933     // 0 success //1 full failure //2 partial failure
00934 
00935     
00936     
00937     if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
00938     {
00939 
00940         acq = SENSOR_DATA_ACQ();
00941         if(acq == 0)
00942             {
00943                 
00944                  ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
00945                  
00946                  ACS_DATA_ACQ_STATUS = 0;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
00947                  pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
00948                  pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
00949                  return 0;
00950             }
00951         else if(acq == 2)
00952             {
00953                     ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40;
00954                     pc_acs.printf(" Sensor 1 data partial success.Exiting.\n \r");
00955                     return 2;
00956                     
00957                     /*if((ACS_ATS_STATUS & 0x0F == 0x00))
00958                         {
00959                             pc_acs.printf(" Sensor 1 data acq partial success.Trying Sensor 2\n \r");
00960                             ATS1_SW_ENABLE = 1;
00961                             ATS2_SW_ENABLE = 0;
00962                             wait_ms(5);
00963                             ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
00964                             
00965                             int acq;
00966                             acq = SENSOR_DATA_ACQ();
00967                             
00968                             if(acq == 0)
00969                                 {
00970                                 ACS_DATA_ACQ_STATUS = 0;
00971                                  pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r");
00972                                 return 0;
00973                                 
00974                                 }
00975                             else if(acq == 2)
00976                                 {
00977                                 ACS_DATA_ACQ_STATUS = 2;
00978                                 pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r");
00979                                 return 2;
00980                                 }
00981                             
00982                             else if(acq == 1)
00983                                 {
00984                                     
00985                                         int acq;
00986                                         pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
00987                                         ATS2_SW_ENABLE = 1;
00988                                         ATS1_SW_ENABLE = 0;
00989                                         wait_ms(5);
00990                                         acq = SENSOR_DATA_ACQ();
00991                                         if(acq == 0)
00992                                             {   
00993                                                 pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r");
00994                                                 ACS_DATA_ACQ_STATUS = 0;
00995                                                 return 0;
00996                                             }
00997                                         else if(acq == 2)
00998                                             {
00999                                                 pc_acs.printf(" Sensor 1 data acq partial success.Exiting.\n \r");
01000                                                 ACS_DATA_ACQ_STATUS = 2;
01001                                                 return 2;
01002                                             }
01003                                         else
01004                                             {
01005                                                 pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r");
01006                                                 ATS1_SW_ENABLE = 0;
01007                                                 ACS_DATA_ACQ_STATUS = 1;
01008                                                 return 1;
01009                                             }
01010                                             
01011                                                 pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r");
01012                                                 ATS1_SW_ENABLE = 0;
01013                                                 ACS_DATA_ACQ_STATUS = 1;
01014                                                 return 1;
01015         
01016                                 }
01017                         
01018                         }
01019                         
01020                         else
01021                         {
01022                             ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40;
01023                             pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
01024                             return 2;
01025                             
01026                             
01027                         }*/
01028 
01029             }
01030             
01031         else if(acq == 1)
01032             {
01033                  pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r");
01034                  ATS1_SW_ENABLE = 1;
01035                  ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
01036             }
01037             
01038 
01039   
01040     }
01041     
01042         
01043         
01044                                
01045         ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
01046     
01047       
01048     if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
01049     {
01050         ATS2_SW_ENABLE = 0;
01051         wait_ms(5); 
01052         
01053         acq = SENSOR_DATA_ACQ();
01054         if(acq == 0)
01055             {
01056                  pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r");
01057                  ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
01058                  ACS_DATA_ACQ_STATUS = 0;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
01059                  return 0;
01060             }
01061         else if(acq == 2)
01062             {
01063                  
01064                  pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r");
01065                  
01066                  ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x04;
01067                  ACS_DATA_ACQ_STATUS = 2;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
01068                  return 2;
01069 
01070             }
01071             
01072             else if(acq == 1)
01073             {
01074                 pc_acs.printf(" Sensor 2 data acq failure.Exiting.\n \r");
01075                 ATS2_SW_ENABLE = 1;
01076                  ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
01077                 //Sensor 2 also not working
01078             }
01079             
01080             
01081             
01082 
01083        
01084     }
01085     
01086 
01087     
01088          ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
01089          
01090     pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
01091     pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r");
01092     
01093     ACS_DATA_ACQ_STATUS = 1;        //set ACS_DATA_ACQ_STATUS flag for att sens 2
01094     return 1;
01095 }
01096 
01097 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
01098 {
01099     printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
01100     
01101     float l_duty_cycle_x=0;    //Duty cycle of Moment in x direction
01102     float l_current_x=0;       //Current sent in x TR's
01103     float l_duty_cycle_y=0;    //Duty cycle of Moment in y direction
01104     float l_current_y=0;       //Current sent in y TR's
01105     float l_duty_cycle_z=0;    //Duty cycle of Moment in z direction
01106     float l_current_z=0;       //Current sent in z TR's
01107  
01108     
01109     printf("\r\r");
01110     
01111     //-----------------------------  x-direction TR  --------------------------------------------//
01112     
01113     
01114     float l_moment_x = Moment[0];         //Moment in x direction
01115     
01116     phase_TR_x = 1;  // setting the default current direction
01117     if (l_moment_x <0)
01118     {
01119         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 
01120         l_moment_x = abs(l_moment_x);
01121     }
01122     
01123     l_current_x = l_moment_x * TR_CONSTANT ;        //Moment and Current always have the linear relationship
01124     printf("current in trx is %f \r \n",l_current_x);
01125     if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
01126     {
01127         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 
01128         printf("DC for trx is %f \r \n",l_duty_cycle_x);
01129         PWM1.period(TIME_PERIOD);
01130         PWM1 = l_duty_cycle_x/100 ;
01131     }
01132     else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
01133     {
01134         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
01135         printf("DC for trx is %f \r \n",l_duty_cycle_x);
01136         PWM1.period(TIME_PERIOD);
01137         PWM1 = l_duty_cycle_x/100 ;            
01138     }
01139     else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
01140     {
01141         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
01142         printf("DC for trx is %f \r \n",l_duty_cycle_x);
01143         PWM1.period(TIME_PERIOD);
01144         PWM1 = l_duty_cycle_x/100 ;            
01145     }
01146     else if(l_current_x==0)
01147     {
01148         printf("\n \r l_current_x====0");
01149         l_duty_cycle_x = 0;      // default value of duty cycle
01150         printf("DC for trx is %f \r \n",l_duty_cycle_x);
01151         PWM1.period(TIME_PERIOD);
01152         PWM1 = l_duty_cycle_x/100 ;            
01153     }
01154     else                                           //not necessary
01155     {
01156         g_err_flag_TR_x = 1;
01157     } 
01158          
01159     //------------------------------------- y-direction TR--------------------------------------//
01160     
01161      
01162     float l_moment_y = Moment[1];         //Moment in y direction
01163     
01164     phase_TR_y = 1;  // setting the default current direction
01165     if (l_moment_y <0)
01166     {
01167         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  
01168         l_moment_y = abs(l_moment_y);
01169     }
01170     
01171     
01172     l_current_y = l_moment_y * TR_CONSTANT ;        //Moment and Current always have the linear relationship
01173     printf("current in try is %f \r \n",l_current_y);
01174     if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
01175     {
01176         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 
01177         printf("DC for try is %f \r \n",l_duty_cycle_y);
01178         PWM2.period(TIME_PERIOD);
01179         PWM2 = l_duty_cycle_y/100 ;
01180     }
01181     else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
01182     {
01183         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
01184         printf("DC for try is %f \r \n",l_duty_cycle_y);
01185         PWM2.period(TIME_PERIOD);
01186         PWM2 = l_duty_cycle_y/100 ;            
01187     }
01188     else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
01189     {
01190         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
01191         printf("DC for try is %f \r \n",l_duty_cycle_y);
01192         PWM2.period(TIME_PERIOD);
01193         PWM2 = l_duty_cycle_y/100 ;            
01194     }        
01195     else if(l_current_y==0)
01196     {
01197         printf("\n \r l_current_y====0");
01198         l_duty_cycle_y = 0; // default value of duty cycle
01199         printf("DC for try is %f \r \n",l_duty_cycle_y);
01200         PWM2.period(TIME_PERIOD);
01201         PWM2 = l_duty_cycle_y/100 ;            
01202     }
01203     else                               // not necessary
01204     {
01205       g_err_flag_TR_y = 1;
01206     } 
01207              
01208     //----------------------------------------------- z-direction TR -------------------------//  
01209     
01210     
01211      
01212     float l_moment_z = Moment[2];         //Moment in z direction
01213     
01214     phase_TR_z = 1;  // setting the default current direction
01215     if (l_moment_z <0)
01216     {
01217         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  
01218         l_moment_z = abs(l_moment_z);
01219     }
01220     
01221     
01222     l_current_z = l_moment_z * TR_CONSTANT ;        //Moment and Current always have the linear relationship
01223     printf("current in trz is %f \r \n",l_current_z);
01224     if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
01225     {
01226         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 
01227         printf("DC for trz is %f \r \n",l_duty_cycle_z);
01228         PWM3.period(TIME_PERIOD);
01229         PWM3 = l_duty_cycle_z/100 ;
01230     }
01231     else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
01232     {
01233         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
01234         printf("DC for trz is %f \r \n",l_duty_cycle_z);
01235         PWM3.period(TIME_PERIOD);
01236         PWM3 = l_duty_cycle_z/100 ;            
01237     }
01238     else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
01239     {
01240         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
01241         printf("DC for trz is %f \r \n",l_duty_cycle_z);
01242         PWM3.period(TIME_PERIOD);
01243         PWM3 = l_duty_cycle_z/100 ;            
01244     }        
01245     else if(l_current_z==0)
01246     {
01247         printf("\n \r l_current_z====0");
01248         l_duty_cycle_z = 0; // default value of duty cycle
01249         printf("DC for trz is %f \r \n",l_duty_cycle_z);
01250         PWM3.period(TIME_PERIOD);
01251         PWM3 = l_duty_cycle_z/100 ;            
01252     }
01253     else                               // not necessary
01254     {
01255       g_err_flag_TR_z = 1;
01256     } 
01257     
01258     //-----------------------------------------exiting the function-----------------------------------//
01259     
01260     printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
01261  
01262 }
01263 
01264 
01265 
01266 
01267