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