GOPA KUMAR K C / Mbed 2 deprecated BAE_RTOS_test_1

4-10-2015 BAE_RTOS_TEST ACS_DATA_ACQ and I2C working

Dependencies:   mbed-rtos mbed

Fork of BAE_RTOS_TEST1 by GOPA KUMAR K C

ACS.cpp@1:b8c71afbe6e5, 2015-10-04 (annotated)

Committer:
gkumar
Date:
Sun Oct 04 07:06:22 2015 +0000
Revision:
1:b8c71afbe6e5
4-10-2015 BAE_RTOS_TEST1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gkumar 1:b8c71afbe6e5 1 #include "ACS.h"
gkumar 1:b8c71afbe6e5 2 #include "pin_config.h"
gkumar 1:b8c71afbe6e5 3 //....................................ATS......................................................//
gkumar 1:b8c71afbe6e5 4
gkumar 1:b8c71afbe6e5 5 I2C i2c (PIN85,PIN84); //PTC2-sda,PTC1-scl
gkumar 1:b8c71afbe6e5 6
gkumar 1:b8c71afbe6e5 7 Timeout g_to; //Timeout variable to
gkumar 1:b8c71afbe6e5 8 int g_toflag;
gkumar 1:b8c71afbe6e5 9 char g_cmd[2];
gkumar 1:b8c71afbe6e5 10 float g_gyro_error[3]= {0,0,0}, g_mag_error[3]= {0,0,0};
gkumar 1:b8c71afbe6e5 11
gkumar 1:b8c71afbe6e5 12 /*------------------------------------------------------------------------------------------------------------------------------------------------------
gkumar 1:b8c71afbe6e5 13 ------------------------------------------- ATS data acquisition------------------------------------------------------------------------------------------*/
gkumar 1:b8c71afbe6e5 14 void FCTN_T_OUT()
gkumar 1:b8c71afbe6e5 15 {
gkumar 1:b8c71afbe6e5 16 g_toflag=0; //as T_OUT function gets called the while loop gets terminated
gkumar 1:b8c71afbe6e5 17 }
gkumar 1:b8c71afbe6e5 18
gkumar 1:b8c71afbe6e5 19 void FCTN_ACS_INIT()
gkumar 1:b8c71afbe6e5 20 {
gkumar 1:b8c71afbe6e5 21 char store;
gkumar 1:b8c71afbe6e5 22 g_cmd[0]=RESETREQ;
gkumar 1:b8c71afbe6e5 23 g_cmd[1]=BIT_RESREQ;
gkumar 1:b8c71afbe6e5 24 i2c.write(SLAVE_ADDR,g_cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
gkumar 1:b8c71afbe6e5 25 wait_ms(2000); //waiting for loading configuration file stored in EEPROM
gkumar 1:b8c71afbe6e5 26 g_cmd[0]=SENTRALSTATUS;
gkumar 1:b8c71afbe6e5 27 i2c.write(SLAVE_ADDR,g_cmd,1);
gkumar 1:b8c71afbe6e5 28 i2c.read(SLAVE_ADDR_READ,&store,1);
gkumar 1:b8c71afbe6e5 29 wait_ms(100);
gkumar 1:b8c71afbe6e5 30 //to check whether EEPROM is uploaded
gkumar 1:b8c71afbe6e5 31 switch((int)store) {
gkumar 1:b8c71afbe6e5 32 case(3): {
gkumar 1:b8c71afbe6e5 33 break;
gkumar 1:b8c71afbe6e5 34 }
gkumar 1:b8c71afbe6e5 35 case(11): {
gkumar 1:b8c71afbe6e5 36 break;
gkumar 1:b8c71afbe6e5 37 }
gkumar 1:b8c71afbe6e5 38 default: {
gkumar 1:b8c71afbe6e5 39 g_cmd[0]=RESETREQ;
gkumar 1:b8c71afbe6e5 40 g_cmd[1]=BIT_RESREQ;
gkumar 1:b8c71afbe6e5 41 i2c.write(SLAVE_ADDR,g_cmd,2);
gkumar 1:b8c71afbe6e5 42 wait_ms(2000);
gkumar 1:b8c71afbe6e5 43 }
gkumar 1:b8c71afbe6e5 44 }
gkumar 1:b8c71afbe6e5 45 //pc.printf("Sentral Status is %x\n",(int)store);
gkumar 1:b8c71afbe6e5 46 g_cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
gkumar 1:b8c71afbe6e5 47 g_cmd[1]=BIT_RUN_ENB;
gkumar 1:b8c71afbe6e5 48 i2c.write(SLAVE_ADDR,g_cmd,2);
gkumar 1:b8c71afbe6e5 49 wait_ms(100);
gkumar 1:b8c71afbe6e5 50 g_cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
gkumar 1:b8c71afbe6e5 51 g_cmd[1]=BIT_MAGODR;
gkumar 1:b8c71afbe6e5 52 i2c.write(SLAVE_ADDR,g_cmd,2);
gkumar 1:b8c71afbe6e5 53 wait_ms(100);
gkumar 1:b8c71afbe6e5 54 g_cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
gkumar 1:b8c71afbe6e5 55 g_cmd[1]=BIT_GYROODR;
gkumar 1:b8c71afbe6e5 56 i2c.write(SLAVE_ADDR,g_cmd,2);
gkumar 1:b8c71afbe6e5 57 wait_ms(100);
gkumar 1:b8c71afbe6e5 58 g_cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
gkumar 1:b8c71afbe6e5 59 g_cmd[1]=0x00;
gkumar 1:b8c71afbe6e5 60 i2c.write(SLAVE_ADDR,g_cmd,2);
gkumar 1:b8c71afbe6e5 61 wait_ms(100);
gkumar 1:b8c71afbe6e5 62 g_cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
gkumar 1:b8c71afbe6e5 63 g_cmd[1]=BIT_EVT_ENB;
gkumar 1:b8c71afbe6e5 64 i2c.write(SLAVE_ADDR,g_cmd,2);
gkumar 1:b8c71afbe6e5 65 wait_ms(100);
gkumar 1:b8c71afbe6e5 66 }
gkumar 1:b8c71afbe6e5 67
gkumar 1:b8c71afbe6e5 68 void FCTN_ATS_DATA_ACQ(float g_gyro_data[3],float g_mag_data[3])
gkumar 1:b8c71afbe6e5 69 {
gkumar 1:b8c71afbe6e5 70 char status;
gkumar 1:b8c71afbe6e5 71 g_toflag=1; //toFlag is set to 1 so that it enters while loop
gkumar 1:b8c71afbe6e5 72 g_to.attach(&FCTN_T_OUT,2); //after 2 seconds the while loop gets terminated
gkumar 1:b8c71afbe6e5 73
gkumar 1:b8c71afbe6e5 74 g_cmd[0]=EVT_STATUS;
gkumar 1:b8c71afbe6e5 75 i2c.write(SLAVE_ADDR,g_cmd,1);
gkumar 1:b8c71afbe6e5 76 i2c.read(SLAVE_ADDR_READ,&status,1);
gkumar 1:b8c71afbe6e5 77 wait_ms(100);
gkumar 1:b8c71afbe6e5 78 //pc.printf("\nEvent Status is %x\n",(int)status);
gkumar 1:b8c71afbe6e5 79 //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
gkumar 1:b8c71afbe6e5 80 if(((int)status&40)==40)
gkumar 1:b8c71afbe6e5 81 {
gkumar 1:b8c71afbe6e5 82 FCTN_GET_DATA(g_gyro_data,g_mag_data);
gkumar 1:b8c71afbe6e5 83 printf("\n\r data received \n");
gkumar 1:b8c71afbe6e5 84 for(int i=0; i<3; i++)
gkumar 1:b8c71afbe6e5 85 {
gkumar 1:b8c71afbe6e5 86 printf("%f\t",g_gyro_data[i]);
gkumar 1:b8c71afbe6e5 87 }
gkumar 1:b8c71afbe6e5 88 for(int i=0; i<3; i++)
gkumar 1:b8c71afbe6e5 89 {
gkumar 1:b8c71afbe6e5 90 printf("%f\t",g_mag_data[i]);
gkumar 1:b8c71afbe6e5 91 }
gkumar 1:b8c71afbe6e5 92 }
gkumar 1:b8c71afbe6e5 93 //checking for the error
gkumar 1:b8c71afbe6e5 94 else if (((int)status&2)==2)
gkumar 1:b8c71afbe6e5 95 {
gkumar 1:b8c71afbe6e5 96 FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
gkumar 1:b8c71afbe6e5 97 }
gkumar 1:b8c71afbe6e5 98
gkumar 1:b8c71afbe6e5 99 }
gkumar 1:b8c71afbe6e5 100
gkumar 1:b8c71afbe6e5 101 void FCTN_GET_DATA(float g_gyro_data[3],float g_mag_data[3])
gkumar 1:b8c71afbe6e5 102 {
gkumar 1:b8c71afbe6e5 103 char raw_gyro[6];
gkumar 1:b8c71afbe6e5 104 char raw_mag[6];
gkumar 1:b8c71afbe6e5 105 int16_t bit_data;
gkumar 1:b8c71afbe6e5 106
gkumar 1:b8c71afbe6e5 107 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
gkumar 1:b8c71afbe6e5 108 float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
gkumar 1:b8c71afbe6e5 109 g_cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
gkumar 1:b8c71afbe6e5 110 i2c.write(SLAVE_ADDR,g_cmd,1);
gkumar 1:b8c71afbe6e5 111 i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
gkumar 1:b8c71afbe6e5 112 g_cmd[0]=MAG_XOUT_H; //LSB of x
gkumar 1:b8c71afbe6e5 113 i2c.write(SLAVE_ADDR,g_cmd,1);
gkumar 1:b8c71afbe6e5 114 i2c.read(SLAVE_ADDR_READ,raw_mag,6);
gkumar 1:b8c71afbe6e5 115 //pc.printf("\nGyro Values:\n");
gkumar 1:b8c71afbe6e5 116 for(int i=0; i<3; i++) {
gkumar 1:b8c71afbe6e5 117 //concatenating gyro LSB and MSB to get 16 bit signed data values
gkumar 1:b8c71afbe6e5 118 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
gkumar 1:b8c71afbe6e5 119 g_gyro_data[i]=(float)bit_data;
gkumar 1:b8c71afbe6e5 120 g_gyro_data[i]=g_gyro_data[i]/senstivity_gyro;
gkumar 1:b8c71afbe6e5 121 g_gyro_data[i]+=g_gyro_error[i];
gkumar 1:b8c71afbe6e5 122 //pc.printf("%f\t",gyro_data[i]);
gkumar 1:b8c71afbe6e5 123 }
gkumar 1:b8c71afbe6e5 124 for(int i=0; i<3; i++) {
gkumar 1:b8c71afbe6e5 125 //concatenating mag LSB and MSB to get 16 bit signed data values
gkumar 1:b8c71afbe6e5 126 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
gkumar 1:b8c71afbe6e5 127 g_mag_data[i]=(float)bit_data;
gkumar 1:b8c71afbe6e5 128 g_mag_data[i]=g_mag_data[i]/senstivity_mag;
gkumar 1:b8c71afbe6e5 129 g_mag_data[i]+=g_mag_error[i];
gkumar 1:b8c71afbe6e5 130 //pc.printf("%f\t",mag_data[i]);
gkumar 1:b8c71afbe6e5 131 }
gkumar 1:b8c71afbe6e5 132
gkumar 1:b8c71afbe6e5 133 }