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