Seeker of Truth ,
/
baeaftrq1
hahalvl1
Fork of BAE_b4hw_test by
mnm.cpp@6:1cdbda747f99, 2015-02-06 (annotated)
- Committer:
- sakthipriya
- Date:
- Fri Feb 06 18:26:46 2015 +0000
- Revision:
- 6:1cdbda747f99
- Parent:
- 5:4199c0dfed33
BAE without beacon
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sakthipriya | 5:4199c0dfed33 | 1 | #include<mbed.h> |
sakthipriya | 5:4199c0dfed33 | 2 | #include "mnm.h" |
sakthipriya | 5:4199c0dfed33 | 3 | #include "pni.h" //pni header file |
sakthipriya | 5:4199c0dfed33 | 4 | Serial mnm(USBTX,USBRX); //for usb communication |
sakthipriya | 5:4199c0dfed33 | 5 | I2C i2c (PTC9,PTC8); //PTC2-sda,PTC1-scl |
sakthipriya | 5:4199c0dfed33 | 6 | /*void INIT_PNI(void); //initialization of registers happens |
sakthipriya | 5:4199c0dfed33 | 7 | float *EXECUTE_PNI(); //data is obtained |
sakthipriya | 5:4199c0dfed33 | 8 | void T_OUT(); //timeout function to stop infinite loop*/ |
sakthipriya | 5:4199c0dfed33 | 9 | Timeout to; //Timeout variable to |
sakthipriya | 5:4199c0dfed33 | 10 | int toFlag; |
sakthipriya | 5:4199c0dfed33 | 11 | void T_OUT() |
sakthipriya | 5:4199c0dfed33 | 12 | { |
sakthipriya | 5:4199c0dfed33 | 13 | toFlag=0; //as T_OUT function gets called the while loop gets terminated |
sakthipriya | 5:4199c0dfed33 | 14 | } |
sakthipriya | 5:4199c0dfed33 | 15 | |
sakthipriya | 5:4199c0dfed33 | 16 | //DEFINING VARIABLES |
sakthipriya | 5:4199c0dfed33 | 17 | char cmd[2]; |
sakthipriya | 5:4199c0dfed33 | 18 | char raw_gyro[6]; |
sakthipriya | 5:4199c0dfed33 | 19 | char raw_mag[6]; |
sakthipriya | 5:4199c0dfed33 | 20 | char store,status; |
sakthipriya | 5:4199c0dfed33 | 21 | int16_t bit_data; |
sakthipriya | 5:4199c0dfed33 | 22 | float gyro_data[3], mag_data[3],combined_values[6],*data; |
sakthipriya | 5:4199c0dfed33 | 23 | float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps |
sakthipriya | 5:4199c0dfed33 | 24 | float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla |
sakthipriya | 5:4199c0dfed33 | 25 | float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0}; |
sakthipriya | 5:4199c0dfed33 | 26 | /* |
sakthipriya | 5:4199c0dfed33 | 27 | int main(void) |
sakthipriya | 5:4199c0dfed33 | 28 | { |
sakthipriya | 5:4199c0dfed33 | 29 | |
sakthipriya | 5:4199c0dfed33 | 30 | INIT_PNI(); |
sakthipriya | 5:4199c0dfed33 | 31 | data=EXECUTE_PNI(); //the angular velocity is stored in the first 3 values and magnetic field values in next 3 |
sakthipriya | 5:4199c0dfed33 | 32 | mnm.printf("gyro values\n"); //printing the angular velocity and magnetic field values |
sakthipriya | 5:4199c0dfed33 | 33 | for(int i=0; i<3; i++) { |
sakthipriya | 5:4199c0dfed33 | 34 | mnm.printf("%f\t",data[i]); |
sakthipriya | 5:4199c0dfed33 | 35 | } |
sakthipriya | 5:4199c0dfed33 | 36 | mnm.printf("mag values\n"); |
sakthipriya | 5:4199c0dfed33 | 37 | for(int i=3; i<6; i++) { |
sakthipriya | 5:4199c0dfed33 | 38 | mnm.printf("%f\t",data[i]); |
sakthipriya | 5:4199c0dfed33 | 39 | } |
sakthipriya | 5:4199c0dfed33 | 40 | }*/ |
sakthipriya | 5:4199c0dfed33 | 41 | |
sakthipriya | 5:4199c0dfed33 | 42 | void INIT_PNI() |
sakthipriya | 5:4199c0dfed33 | 43 | { |
sakthipriya | 5:4199c0dfed33 | 44 | cmd[0]=RESETREQ; |
sakthipriya | 5:4199c0dfed33 | 45 | cmd[1]=BIT_RESREQ; |
sakthipriya | 5:4199c0dfed33 | 46 | i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up |
sakthipriya | 5:4199c0dfed33 | 47 | wait_ms(2000); //waiting for loading configuration file stored in EEPROM |
sakthipriya | 5:4199c0dfed33 | 48 | cmd[0]=SENTRALSTATUS; |
sakthipriya | 5:4199c0dfed33 | 49 | i2c.write(SLAVE_ADDR,cmd,1); |
sakthipriya | 5:4199c0dfed33 | 50 | i2c.read(SLAVE_ADDR_READ,&store,1); |
sakthipriya | 5:4199c0dfed33 | 51 | wait_ms(100); |
sakthipriya | 5:4199c0dfed33 | 52 | //to check whether EEPROM is uploaded |
sakthipriya | 5:4199c0dfed33 | 53 | switch((int)store) { |
sakthipriya | 5:4199c0dfed33 | 54 | case(3): { |
sakthipriya | 5:4199c0dfed33 | 55 | break; |
sakthipriya | 5:4199c0dfed33 | 56 | } |
sakthipriya | 5:4199c0dfed33 | 57 | case(11): { |
sakthipriya | 5:4199c0dfed33 | 58 | break; |
sakthipriya | 5:4199c0dfed33 | 59 | } |
sakthipriya | 5:4199c0dfed33 | 60 | default: { |
sakthipriya | 5:4199c0dfed33 | 61 | cmd[0]=RESETREQ; |
sakthipriya | 5:4199c0dfed33 | 62 | cmd[1]=BIT_RESREQ; |
sakthipriya | 5:4199c0dfed33 | 63 | i2c.write(SLAVE_ADDR,cmd,2); |
sakthipriya | 5:4199c0dfed33 | 64 | wait_ms(2000); |
sakthipriya | 5:4199c0dfed33 | 65 | } |
sakthipriya | 5:4199c0dfed33 | 66 | } |
sakthipriya | 5:4199c0dfed33 | 67 | //mnm.printf("Sentral Status is %x\n",(int)store); |
sakthipriya | 5:4199c0dfed33 | 68 | cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors |
sakthipriya | 5:4199c0dfed33 | 69 | cmd[1]=BIT_RUN_ENB; |
sakthipriya | 5:4199c0dfed33 | 70 | i2c.write(SLAVE_ADDR,cmd,2); |
sakthipriya | 5:4199c0dfed33 | 71 | wait_ms(100); |
sakthipriya | 5:4199c0dfed33 | 72 | cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer |
sakthipriya | 5:4199c0dfed33 | 73 | cmd[1]=BIT_MAGODR; |
sakthipriya | 5:4199c0dfed33 | 74 | i2c.write(SLAVE_ADDR,cmd,2); |
sakthipriya | 5:4199c0dfed33 | 75 | wait_ms(100); |
sakthipriya | 5:4199c0dfed33 | 76 | cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope |
sakthipriya | 5:4199c0dfed33 | 77 | cmd[1]=BIT_GYROODR; |
sakthipriya | 5:4199c0dfed33 | 78 | i2c.write(SLAVE_ADDR,cmd,2); |
sakthipriya | 5:4199c0dfed33 | 79 | wait_ms(100); |
sakthipriya | 5:4199c0dfed33 | 80 | cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values |
sakthipriya | 5:4199c0dfed33 | 81 | cmd[1]=0x00; |
sakthipriya | 5:4199c0dfed33 | 82 | i2c.write(SLAVE_ADDR,cmd,2); |
sakthipriya | 5:4199c0dfed33 | 83 | wait_ms(100); |
sakthipriya | 5:4199c0dfed33 | 84 | cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values |
sakthipriya | 5:4199c0dfed33 | 85 | cmd[1]=BIT_EVT_ENB; |
sakthipriya | 5:4199c0dfed33 | 86 | i2c.write(SLAVE_ADDR,cmd,2); |
sakthipriya | 5:4199c0dfed33 | 87 | wait_ms(100); |
sakthipriya | 5:4199c0dfed33 | 88 | } |
sakthipriya | 5:4199c0dfed33 | 89 | |
sakthipriya | 5:4199c0dfed33 | 90 | float *EXECUTE_PNI() |
sakthipriya | 5:4199c0dfed33 | 91 | { |
sakthipriya | 5:4199c0dfed33 | 92 | //printf("\n\r mnm func \n"); |
sakthipriya | 5:4199c0dfed33 | 93 | toFlag=1; //toFlag is set to 1 so that it enters while loop |
sakthipriya | 5:4199c0dfed33 | 94 | to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated |
sakthipriya | 5:4199c0dfed33 | 95 | while(toFlag) { |
sakthipriya | 5:4199c0dfed33 | 96 | cmd[0]=EVT_STATUS; |
sakthipriya | 5:4199c0dfed33 | 97 | i2c.write(SLAVE_ADDR,cmd,1); |
sakthipriya | 5:4199c0dfed33 | 98 | i2c.read(SLAVE_ADDR_READ,&status,1); |
sakthipriya | 5:4199c0dfed33 | 99 | wait_ms(100); |
sakthipriya | 5:4199c0dfed33 | 100 | //mnm.printf("\nEvent Status is %x\n",(int)status); |
sakthipriya | 5:4199c0dfed33 | 101 | //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take |
sakthipriya | 5:4199c0dfed33 | 102 | if(((int)status&40)==40) { |
sakthipriya | 5:4199c0dfed33 | 103 | printf("\nin if of mnm\n"); |
sakthipriya | 5:4199c0dfed33 | 104 | cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x |
sakthipriya | 5:4199c0dfed33 | 105 | i2c.write(SLAVE_ADDR,cmd,1); |
sakthipriya | 5:4199c0dfed33 | 106 | i2c.read(SLAVE_ADDR_READ,raw_gyro,6); |
sakthipriya | 5:4199c0dfed33 | 107 | cmd[0]=MAG_XOUT_H; //LSB of x |
sakthipriya | 5:4199c0dfed33 | 108 | i2c.write(SLAVE_ADDR,cmd,1); |
sakthipriya | 5:4199c0dfed33 | 109 | i2c.read(SLAVE_ADDR_READ,raw_mag,6); |
sakthipriya | 5:4199c0dfed33 | 110 | //mnm.printf("\nGyro Values:\n"); |
sakthipriya | 5:4199c0dfed33 | 111 | for(int i=0; i<3; i++) { |
sakthipriya | 5:4199c0dfed33 | 112 | //concatenating gyro LSB and MSB to get 16 bit signed data values |
sakthipriya | 5:4199c0dfed33 | 113 | bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; |
sakthipriya | 5:4199c0dfed33 | 114 | gyro_data[i]=(float)bit_data; |
sakthipriya | 5:4199c0dfed33 | 115 | gyro_data[i]=gyro_data[i]/senstivity_gyro; |
sakthipriya | 5:4199c0dfed33 | 116 | gyro_data[i]+=gyro_error[i]; |
sakthipriya | 5:4199c0dfed33 | 117 | //mnm.printf("%f\t",gyro_data[i]); |
sakthipriya | 5:4199c0dfed33 | 118 | } |
sakthipriya | 5:4199c0dfed33 | 119 | //mnm.printf("\nMag Values:\n"); |
sakthipriya | 5:4199c0dfed33 | 120 | for(int i=0; i<3; i++) { |
sakthipriya | 5:4199c0dfed33 | 121 | //concatenating mag LSB and MSB to get 16 bit signed data values |
sakthipriya | 5:4199c0dfed33 | 122 | bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i]; |
sakthipriya | 5:4199c0dfed33 | 123 | mag_data[i]=(float)bit_data; |
sakthipriya | 5:4199c0dfed33 | 124 | mag_data[i]=mag_data[i]/senstivity_mag; |
sakthipriya | 5:4199c0dfed33 | 125 | mag_data[i]+=mag_error[i]; |
sakthipriya | 5:4199c0dfed33 | 126 | //mnm.printf("%f\t",mag_data[i]); |
sakthipriya | 5:4199c0dfed33 | 127 | } |
sakthipriya | 5:4199c0dfed33 | 128 | for(int i=0; i<3; i++) { |
sakthipriya | 5:4199c0dfed33 | 129 | combined_values[i]=gyro_data[i]; |
sakthipriya | 5:4199c0dfed33 | 130 | combined_values[i+3]=mag_data[i]; |
sakthipriya | 5:4199c0dfed33 | 131 | } |
sakthipriya | 5:4199c0dfed33 | 132 | return(combined_values); //returning poiter combined values |
sakthipriya | 5:4199c0dfed33 | 133 | } |
sakthipriya | 5:4199c0dfed33 | 134 | //checking for the error |
sakthipriya | 5:4199c0dfed33 | 135 | |
sakthipriya | 5:4199c0dfed33 | 136 | else if (((int)status&2)==2) { |
sakthipriya | 5:4199c0dfed33 | 137 | INIT_PNI(); //when there is any error then Again inilization is done to remove error |
sakthipriya | 5:4199c0dfed33 | 138 | } |
sakthipriya | 5:4199c0dfed33 | 139 | |
sakthipriya | 5:4199c0dfed33 | 140 | |
sakthipriya | 5:4199c0dfed33 | 141 | } |
sakthipriya | 5:4199c0dfed33 | 142 | } |