4-10-2015 BAE_RTOS_TEST ACS_DATA_ACQ and I2C working
Fork of BAE_RTOS_TEST1 by
Diff: ACS.cpp
- Revision:
- 1:b8c71afbe6e5
diff -r f417d854dc29 -r b8c71afbe6e5 ACS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ACS.cpp Sun Oct 04 07:06:22 2015 +0000 @@ -0,0 +1,133 @@ +#include "ACS.h" +#include "pin_config.h" +//....................................ATS......................................................// + +I2C i2c (PIN85,PIN84); //PTC2-sda,PTC1-scl + +Timeout g_to; //Timeout variable to +int g_toflag; +char g_cmd[2]; +float g_gyro_error[3]= {0,0,0}, g_mag_error[3]= {0,0,0}; + +/*------------------------------------------------------------------------------------------------------------------------------------------------------ +------------------------------------------- ATS data acquisition------------------------------------------------------------------------------------------*/ +void FCTN_T_OUT() +{ + g_toflag=0; //as T_OUT function gets called the while loop gets terminated +} + +void FCTN_ACS_INIT() +{ + char store; + g_cmd[0]=RESETREQ; + g_cmd[1]=BIT_RESREQ; + i2c.write(SLAVE_ADDR,g_cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up + wait_ms(2000); //waiting for loading configuration file stored in EEPROM + g_cmd[0]=SENTRALSTATUS; + i2c.write(SLAVE_ADDR,g_cmd,1); + i2c.read(SLAVE_ADDR_READ,&store,1); + wait_ms(100); + //to check whether EEPROM is uploaded + switch((int)store) { + case(3): { + break; + } + case(11): { + break; + } + default: { + g_cmd[0]=RESETREQ; + g_cmd[1]=BIT_RESREQ; + i2c.write(SLAVE_ADDR,g_cmd,2); + wait_ms(2000); + } + } + //pc.printf("Sentral Status is %x\n",(int)store); + g_cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors + g_cmd[1]=BIT_RUN_ENB; + i2c.write(SLAVE_ADDR,g_cmd,2); + wait_ms(100); + g_cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer + g_cmd[1]=BIT_MAGODR; + i2c.write(SLAVE_ADDR,g_cmd,2); + wait_ms(100); + g_cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope + g_cmd[1]=BIT_GYROODR; + i2c.write(SLAVE_ADDR,g_cmd,2); + wait_ms(100); + g_cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values + g_cmd[1]=0x00; + i2c.write(SLAVE_ADDR,g_cmd,2); + wait_ms(100); + g_cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values + g_cmd[1]=BIT_EVT_ENB; + i2c.write(SLAVE_ADDR,g_cmd,2); + wait_ms(100); +} + +void FCTN_ATS_DATA_ACQ(float g_gyro_data[3],float g_mag_data[3]) +{ + char status; + g_toflag=1; //toFlag is set to 1 so that it enters while loop + g_to.attach(&FCTN_T_OUT,2); //after 2 seconds the while loop gets terminated + + g_cmd[0]=EVT_STATUS; + i2c.write(SLAVE_ADDR,g_cmd,1); + i2c.read(SLAVE_ADDR_READ,&status,1); + wait_ms(100); + //pc.printf("\nEvent Status is %x\n",(int)status); + //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take + if(((int)status&40)==40) + { + FCTN_GET_DATA(g_gyro_data,g_mag_data); + printf("\n\r data received \n"); + for(int i=0; i<3; i++) + { + printf("%f\t",g_gyro_data[i]); + } + for(int i=0; i<3; i++) + { + printf("%f\t",g_mag_data[i]); + } + } + //checking for the error + else if (((int)status&2)==2) + { + FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error + } + +} + +void FCTN_GET_DATA(float g_gyro_data[3],float g_mag_data[3]) +{ + char raw_gyro[6]; + char raw_mag[6]; + int16_t bit_data; + + float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps + float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla + g_cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x + i2c.write(SLAVE_ADDR,g_cmd,1); + i2c.read(SLAVE_ADDR_READ,raw_gyro,6); + g_cmd[0]=MAG_XOUT_H; //LSB of x + i2c.write(SLAVE_ADDR,g_cmd,1); + i2c.read(SLAVE_ADDR_READ,raw_mag,6); + //pc.printf("\nGyro Values:\n"); + for(int i=0; i<3; i++) { + //concatenating gyro LSB and MSB to get 16 bit signed data values + bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; + g_gyro_data[i]=(float)bit_data; + g_gyro_data[i]=g_gyro_data[i]/senstivity_gyro; + g_gyro_data[i]+=g_gyro_error[i]; + //pc.printf("%f\t",gyro_data[i]); + } + for(int i=0; i<3; i++) { + //concatenating mag LSB and MSB to get 16 bit signed data values + bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i]; + g_mag_data[i]=(float)bit_data; + g_mag_data[i]=g_mag_data[i]/senstivity_mag; + g_mag_data[i]+=g_mag_error[i]; + //pc.printf("%f\t",mag_data[i]); + } + +}