grove_gyroscope
grove_gyroscope.cpp
- Committer:
- JackyZhangFromSeeed
- Date:
- 2015-06-09
- Revision:
- 0:d4c2464a3868
File content as of revision 0:d4c2464a3868:
#include "suli2.h" #include "grove_gyroscope.h" //local functions static char _read_char(I2C_T *i2c, unsigned char addr); static double grove_gyro_gettemperature(I2C_T *i2c); static void grove_gyro_getxyz(I2C_T *i2c, int16_t *x,int16_t *y,int16_t *z); //local variables static unsigned char cmdbuf[2]; static unsigned char databuf[2]; static int16_t x_offset; static int16_t y_offset; static int16_t z_offset; // Read 1 byte from I2C static char _read_char(I2C_T *i2c, unsigned char addr) { suli_i2c_write(i2c, GYRO_ADDRESS, &addr, 1); suli_i2c_read(i2c, GYRO_ADDRESS, databuf, 1); return databuf[0]; } void grove_gyroscope_init(I2C_T *i2c, int pinsda, int pinscl) { suli_i2c_init(i2c, pinsda, pinscl); } bool grove_gyro_write_setup(I2C_T *i2c) { cmdbuf[0] = ITG3200_PWR_M; cmdbuf[1] = 0x80; suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//send a reset to the device cmdbuf[0] = ITG3200_SMPL; cmdbuf[1] = 0x00; suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//sample rate divider cmdbuf[0] = ITG3200_DLPF; cmdbuf[1] = 0x18; suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//+/-2000 degrees/s (default value) return true; } /*Function: Get the temperature from ITG3200 that with a on-chip*/ /* temperature sensor. */ static double grove_gyro_gettemperature(I2C_T *i2c) { int temp; double temperature; //temp = read(ITG3200_TMP_H, ITG3200_TMP_L); temp = (_read_char(i2c, ITG3200_TMP_H) << 8) + _read_char(i2c, ITG3200_TMP_L); temperature = 35+ ((double) (temp + 13200)) / 280; return(temperature); } /*Function: Get the contents of the registers in the ITG3200*/ /* so as to calculate the angular velocity. */ static void grove_gyro_getxyz(I2C_T *i2c, int16_t *x,int16_t *y,int16_t *z) { *x = (_read_char(i2c, ITG3200_GX_H) << 8) + _read_char(i2c, ITG3200_GX_L) + x_offset; *y = (_read_char(i2c, ITG3200_GY_H) << 8) + _read_char(i2c, ITG3200_GY_L) + y_offset; *z = (_read_char(i2c, ITG3200_GZ_H) << 8) + _read_char(i2c, ITG3200_GZ_L) + z_offset; } /*Function: Get the angular velocity and its unit is degree per second.*/ bool grove_gyro_getangularvelocity(I2C_T *i2c, float *ax,float *ay,float *az) { int16_t x,y,z; grove_gyro_getxyz(i2c, &x,&y,&z); *ax = x/14.375; *ay = y/14.375; *az = z/14.375; return true; } bool grove_gyro_zerocalibrate(I2C_T *i2c) { int16_t x_offset_temp; int16_t y_offset_temp; int16_t z_offset_temp; int16_t x,y,z; x_offset = 0; y_offset = 0; z_offset = 0; for (int i = 0;i < 200;i++){ suli_delay_ms(10); grove_gyro_getxyz(i2c, &x,&y,&z); x_offset_temp += x; y_offset_temp += y; z_offset_temp += z; } x_offset = abs(x_offset_temp)/200; y_offset = abs(y_offset_temp)/200; z_offset = abs(z_offset_temp)/200; if(x_offset_temp > 0)x_offset = -x_offset; if(y_offset_temp > 0)y_offset = -y_offset; if(z_offset_temp > 0)z_offset = -z_offset; return true; }