grove_gyroscope.cpp

00001 
00002 
00003 #include "suli2.h"
00004 #include "grove_gyroscope.h"
00005 
00006 
00007 
00008 //local functions
00009 static char _read_char(I2C_T *i2c, unsigned char addr);
00010 static double grove_gyro_gettemperature(I2C_T *i2c);
00011 static void grove_gyro_getxyz(I2C_T *i2c, int16_t *x,int16_t *y,int16_t *z);
00012 
00013 //local variables
00014 static unsigned char cmdbuf[2];
00015 static unsigned char databuf[2];
00016 static int16_t x_offset;
00017 static int16_t y_offset;
00018 static int16_t z_offset;
00019 
00020 // Read 1 byte from I2C
00021 static char _read_char(I2C_T *i2c, unsigned char addr)
00022 {
00023     suli_i2c_write(i2c, GYRO_ADDRESS, &addr, 1);
00024     suli_i2c_read(i2c, GYRO_ADDRESS, databuf, 1);
00025     return databuf[0];
00026 }
00027 
00028 void grove_gyroscope_init(I2C_T *i2c, int pinsda, int pinscl)
00029 {
00030     suli_i2c_init(i2c, pinsda, pinscl);
00031 }
00032 
00033 bool grove_gyro_write_setup(I2C_T *i2c)
00034 {
00035     cmdbuf[0] = ITG3200_PWR_M;
00036     cmdbuf[1] = 0x80;
00037     suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//send a reset to the device
00038     cmdbuf[0] = ITG3200_SMPL;
00039     cmdbuf[1] = 0x00;
00040     suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//sample rate divider
00041     cmdbuf[0] = ITG3200_DLPF;
00042     cmdbuf[1] = 0x18;
00043     suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//+/-2000 degrees/s (default value)
00044     
00045     return true;
00046 }
00047 
00048 /*Function: Get the temperature from ITG3200 that with a on-chip*/
00049 /*           temperature sensor.                                */
00050 static double grove_gyro_gettemperature(I2C_T *i2c)
00051 {
00052     int temp;
00053     double temperature;
00054     //temp = read(ITG3200_TMP_H, ITG3200_TMP_L);
00055     temp = (_read_char(i2c, ITG3200_TMP_H) << 8) + _read_char(i2c, ITG3200_TMP_L);
00056     temperature = 35+ ((double) (temp + 13200)) / 280;
00057     return(temperature);
00058 }
00059 
00060 /*Function: Get the contents of the registers in the ITG3200*/
00061 /*          so as to calculate the angular velocity.        */
00062 static void grove_gyro_getxyz(I2C_T *i2c, int16_t *x,int16_t *y,int16_t *z)
00063 {
00064     *x  = (_read_char(i2c, ITG3200_GX_H) << 8) + _read_char(i2c, ITG3200_GX_L) + x_offset;
00065     *y = (_read_char(i2c, ITG3200_GY_H) << 8) + _read_char(i2c, ITG3200_GY_L) + y_offset;
00066     *z = (_read_char(i2c, ITG3200_GZ_H) << 8) + _read_char(i2c, ITG3200_GZ_L) + z_offset;
00067 }
00068 
00069 /*Function: Get the angular velocity and its unit is degree per second.*/
00070 bool grove_gyro_getangularvelocity(I2C_T *i2c, float *ax,float *ay,float *az)
00071 {
00072     int16_t x,y,z;
00073     grove_gyro_getxyz(i2c, &x,&y,&z);
00074     *ax = x/14.375;
00075     *ay = y/14.375;
00076     *az = z/14.375;
00077     
00078     return true;
00079 }
00080 
00081 bool grove_gyro_zerocalibrate(I2C_T *i2c) 
00082 {
00083   int16_t x_offset_temp;
00084   int16_t y_offset_temp;
00085   int16_t z_offset_temp;
00086   int16_t x,y,z;
00087   x_offset = 0;
00088   y_offset = 0;
00089   z_offset = 0;
00090   
00091   for (int i = 0;i < 200;i++){
00092     suli_delay_ms(10);
00093     grove_gyro_getxyz(i2c, &x,&y,&z);
00094     x_offset_temp += x;
00095     y_offset_temp += y;
00096     z_offset_temp += z;
00097   }
00098 
00099   x_offset = abs(x_offset_temp)/200;
00100   y_offset = abs(y_offset_temp)/200;
00101   z_offset = abs(z_offset_temp)/200;
00102   if(x_offset_temp > 0)x_offset = -x_offset;
00103   if(y_offset_temp > 0)y_offset = -y_offset;
00104   if(z_offset_temp > 0)z_offset = -z_offset;
00105   
00106   return true;
00107 }