ACS.cpp

00001 #include "ACS.h"
00002 #include "pin_config.h"
00003 //....................................ATS......................................................//
00004 
00005 I2C i2c (PIN85,PIN84); //PTC2-sda,PTC1-scl
00006 
00007 Timeout g_to; //Timeout variable to
00008 int g_toflag; 
00009 char g_cmd[2];
00010 float g_gyro_error[3]= {0,0,0}, g_mag_error[3]= {0,0,0};
00011 
00012 /*------------------------------------------------------------------------------------------------------------------------------------------------------
00013 ------------------------------------------- ATS data acquisition------------------------------------------------------------------------------------------*/
00014 void FCTN_T_OUT()
00015 {
00016     g_toflag=0; //as T_OUT function gets called the while loop gets terminated
00017 }
00018 
00019 void  FCTN_ACS_INIT()
00020 {
00021     char store;
00022     g_cmd[0]=RESETREQ;
00023     g_cmd[1]=BIT_RESREQ;
00024     i2c.write(SLAVE_ADDR,g_cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
00025     wait_ms(2000); //waiting for loading configuration file stored in EEPROM
00026     g_cmd[0]=SENTRALSTATUS;
00027     i2c.write(SLAVE_ADDR,g_cmd,1);
00028     i2c.read(SLAVE_ADDR_READ,&store,1);
00029     wait_ms(100);
00030     //to check whether EEPROM is uploaded
00031     switch((int)store) { 
00032         case(3): {
00033             break;
00034         }
00035         case(11): {
00036             break;
00037         }
00038         default: {
00039             g_cmd[0]=RESETREQ;
00040             g_cmd[1]=BIT_RESREQ;
00041             i2c.write(SLAVE_ADDR,g_cmd,2);
00042             wait_ms(2000);
00043         }
00044     }
00045     //pc.printf("Sentral Status is %x\n",(int)store);
00046     g_cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
00047     g_cmd[1]=BIT_RUN_ENB;
00048     i2c.write(SLAVE_ADDR,g_cmd,2);
00049     wait_ms(100);
00050     g_cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
00051     g_cmd[1]=BIT_MAGODR;
00052     i2c.write(SLAVE_ADDR,g_cmd,2);
00053     wait_ms(100);
00054     g_cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
00055     g_cmd[1]=BIT_GYROODR;
00056     i2c.write(SLAVE_ADDR,g_cmd,2);
00057     wait_ms(100);
00058     g_cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
00059     g_cmd[1]=0x00;
00060     i2c.write(SLAVE_ADDR,g_cmd,2);
00061     wait_ms(100);
00062     g_cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
00063     g_cmd[1]=BIT_EVT_ENB;
00064     i2c.write(SLAVE_ADDR,g_cmd,2);
00065     wait_ms(100);
00066 }
00067 
00068 void FCTN_ATS_DATA_ACQ(float g_gyro_data[3],float g_mag_data[3])
00069 {
00070     char status;
00071     g_toflag=1; //toFlag is set to 1 so that it enters while loop
00072     g_to.attach(&FCTN_T_OUT,2); //after 2 seconds the while loop gets terminated 
00073     
00074         g_cmd[0]=EVT_STATUS;
00075         i2c.write(SLAVE_ADDR,g_cmd,1);
00076         i2c.read(SLAVE_ADDR_READ,&status,1);
00077         wait_ms(100);
00078         //pc.printf("\nEvent Status is %x\n",(int)status);
00079         //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
00080         if(((int)status&40)==40) 
00081         {
00082             FCTN_GET_DATA(g_gyro_data,g_mag_data);
00083             printf("\n\r data received \n");
00084              for(int i=0; i<3; i++) 
00085             {
00086             printf("%f\t",g_gyro_data[i]);
00087             }
00088             for(int i=0; i<3; i++) 
00089             {
00090             printf("%f\t",g_mag_data[i]);
00091             }
00092         }
00093        //checking for the error
00094         else if (((int)status&2)==2) 
00095         {
00096             FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
00097         }
00098     
00099 }
00100 
00101 void FCTN_GET_DATA(float g_gyro_data[3],float g_mag_data[3])
00102 {
00103     char raw_gyro[6];
00104     char raw_mag[6];
00105     int16_t bit_data;
00106     
00107     float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
00108     float senstivity_mag  =32.768; //senstivity is obtained from 2^15/1000microtesla
00109     g_cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x 
00110     i2c.write(SLAVE_ADDR,g_cmd,1);
00111     i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
00112     g_cmd[0]=MAG_XOUT_H; //LSB of x
00113     i2c.write(SLAVE_ADDR,g_cmd,1);
00114     i2c.read(SLAVE_ADDR_READ,raw_mag,6);
00115             //pc.printf("\nGyro Values:\n");
00116             for(int i=0; i<3; i++) {
00117                 //concatenating gyro LSB and MSB to get 16 bit signed data values
00118                 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; 
00119                 g_gyro_data[i]=(float)bit_data;
00120                 g_gyro_data[i]=g_gyro_data[i]/senstivity_gyro;
00121                 g_gyro_data[i]+=g_gyro_error[i];
00122                 //pc.printf("%f\t",gyro_data[i]);
00123             }
00124             for(int i=0; i<3; i++) {
00125                 //concatenating mag LSB and MSB to get 16 bit signed data values
00126                 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
00127                 g_mag_data[i]=(float)bit_data;
00128                 g_mag_data[i]=g_mag_data[i]/senstivity_mag;
00129                 g_mag_data[i]+=g_mag_error[i];
00130                 //pc.printf("%f\t",mag_data[i]);
00131             }
00132             
00133 }