grove_gyroscope
Diff: grove_gyroscope.cpp
- Revision:
- 0:d4c2464a3868
diff -r 000000000000 -r d4c2464a3868 grove_gyroscope.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/grove_gyroscope.cpp Tue Jun 09 10:18:45 2015 +0000 @@ -0,0 +1,107 @@ + + +#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; +}