Seeed / grove_gyroscope

grove_gyroscope

grove_gyroscope.cpp@0:d4c2464a3868, 2015-06-09 (annotated)

Committer:
JackyZhangFromSeeed
Date:
Tue Jun 09 10:18:45 2015 +0000
Revision:
0:d4c2464a3868
grove_gyroscope

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JackyZhangFromSeeed 0:d4c2464a3868 1
JackyZhangFromSeeed 0:d4c2464a3868 2
JackyZhangFromSeeed 0:d4c2464a3868 3 #include "suli2.h"
JackyZhangFromSeeed 0:d4c2464a3868 4 #include "grove_gyroscope.h"
JackyZhangFromSeeed 0:d4c2464a3868 5
JackyZhangFromSeeed 0:d4c2464a3868 6
JackyZhangFromSeeed 0:d4c2464a3868 7
JackyZhangFromSeeed 0:d4c2464a3868 8 //local functions
JackyZhangFromSeeed 0:d4c2464a3868 9 static char _read_char(I2C_T *i2c, unsigned char addr);
JackyZhangFromSeeed 0:d4c2464a3868 10 static double grove_gyro_gettemperature(I2C_T *i2c);
JackyZhangFromSeeed 0:d4c2464a3868 11 static void grove_gyro_getxyz(I2C_T *i2c, int16_t *x,int16_t *y,int16_t *z);
JackyZhangFromSeeed 0:d4c2464a3868 12
JackyZhangFromSeeed 0:d4c2464a3868 13 //local variables
JackyZhangFromSeeed 0:d4c2464a3868 14 static unsigned char cmdbuf[2];
JackyZhangFromSeeed 0:d4c2464a3868 15 static unsigned char databuf[2];
JackyZhangFromSeeed 0:d4c2464a3868 16 static int16_t x_offset;
JackyZhangFromSeeed 0:d4c2464a3868 17 static int16_t y_offset;
JackyZhangFromSeeed 0:d4c2464a3868 18 static int16_t z_offset;
JackyZhangFromSeeed 0:d4c2464a3868 19
JackyZhangFromSeeed 0:d4c2464a3868 20 // Read 1 byte from I2C
JackyZhangFromSeeed 0:d4c2464a3868 21 static char _read_char(I2C_T *i2c, unsigned char addr)
JackyZhangFromSeeed 0:d4c2464a3868 22 {
JackyZhangFromSeeed 0:d4c2464a3868 23 suli_i2c_write(i2c, GYRO_ADDRESS, &addr, 1);
JackyZhangFromSeeed 0:d4c2464a3868 24 suli_i2c_read(i2c, GYRO_ADDRESS, databuf, 1);
JackyZhangFromSeeed 0:d4c2464a3868 25 return databuf[0];
JackyZhangFromSeeed 0:d4c2464a3868 26 }
JackyZhangFromSeeed 0:d4c2464a3868 27
JackyZhangFromSeeed 0:d4c2464a3868 28 void grove_gyroscope_init(I2C_T *i2c, int pinsda, int pinscl)
JackyZhangFromSeeed 0:d4c2464a3868 29 {
JackyZhangFromSeeed 0:d4c2464a3868 30 suli_i2c_init(i2c, pinsda, pinscl);
JackyZhangFromSeeed 0:d4c2464a3868 31 }
JackyZhangFromSeeed 0:d4c2464a3868 32
JackyZhangFromSeeed 0:d4c2464a3868 33 bool grove_gyro_write_setup(I2C_T *i2c)
JackyZhangFromSeeed 0:d4c2464a3868 34 {
JackyZhangFromSeeed 0:d4c2464a3868 35 cmdbuf[0] = ITG3200_PWR_M;
JackyZhangFromSeeed 0:d4c2464a3868 36 cmdbuf[1] = 0x80;
JackyZhangFromSeeed 0:d4c2464a3868 37 suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//send a reset to the device
JackyZhangFromSeeed 0:d4c2464a3868 38 cmdbuf[0] = ITG3200_SMPL;
JackyZhangFromSeeed 0:d4c2464a3868 39 cmdbuf[1] = 0x00;
JackyZhangFromSeeed 0:d4c2464a3868 40 suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//sample rate divider
JackyZhangFromSeeed 0:d4c2464a3868 41 cmdbuf[0] = ITG3200_DLPF;
JackyZhangFromSeeed 0:d4c2464a3868 42 cmdbuf[1] = 0x18;
JackyZhangFromSeeed 0:d4c2464a3868 43 suli_i2c_write(i2c, GYRO_ADDRESS, cmdbuf, 2);//+/-2000 degrees/s (default value)
JackyZhangFromSeeed 0:d4c2464a3868 44
JackyZhangFromSeeed 0:d4c2464a3868 45 return true;
JackyZhangFromSeeed 0:d4c2464a3868 46 }
JackyZhangFromSeeed 0:d4c2464a3868 47
JackyZhangFromSeeed 0:d4c2464a3868 48 /*Function: Get the temperature from ITG3200 that with a on-chip*/
JackyZhangFromSeeed 0:d4c2464a3868 49 /* temperature sensor. */
JackyZhangFromSeeed 0:d4c2464a3868 50 static double grove_gyro_gettemperature(I2C_T *i2c)
JackyZhangFromSeeed 0:d4c2464a3868 51 {
JackyZhangFromSeeed 0:d4c2464a3868 52 int temp;
JackyZhangFromSeeed 0:d4c2464a3868 53 double temperature;
JackyZhangFromSeeed 0:d4c2464a3868 54 //temp = read(ITG3200_TMP_H, ITG3200_TMP_L);
JackyZhangFromSeeed 0:d4c2464a3868 55 temp = (_read_char(i2c, ITG3200_TMP_H) << 8) + _read_char(i2c, ITG3200_TMP_L);
JackyZhangFromSeeed 0:d4c2464a3868 56 temperature = 35+ ((double) (temp + 13200)) / 280;
JackyZhangFromSeeed 0:d4c2464a3868 57 return(temperature);
JackyZhangFromSeeed 0:d4c2464a3868 58 }
JackyZhangFromSeeed 0:d4c2464a3868 59
JackyZhangFromSeeed 0:d4c2464a3868 60 /*Function: Get the contents of the registers in the ITG3200*/
JackyZhangFromSeeed 0:d4c2464a3868 61 /* so as to calculate the angular velocity. */
JackyZhangFromSeeed 0:d4c2464a3868 62 static void grove_gyro_getxyz(I2C_T *i2c, int16_t *x,int16_t *y,int16_t *z)
JackyZhangFromSeeed 0:d4c2464a3868 63 {
JackyZhangFromSeeed 0:d4c2464a3868 64 *x = (_read_char(i2c, ITG3200_GX_H) << 8) + _read_char(i2c, ITG3200_GX_L) + x_offset;
JackyZhangFromSeeed 0:d4c2464a3868 65 *y = (_read_char(i2c, ITG3200_GY_H) << 8) + _read_char(i2c, ITG3200_GY_L) + y_offset;
JackyZhangFromSeeed 0:d4c2464a3868 66 *z = (_read_char(i2c, ITG3200_GZ_H) << 8) + _read_char(i2c, ITG3200_GZ_L) + z_offset;
JackyZhangFromSeeed 0:d4c2464a3868 67 }
JackyZhangFromSeeed 0:d4c2464a3868 68
JackyZhangFromSeeed 0:d4c2464a3868 69 /*Function: Get the angular velocity and its unit is degree per second.*/
JackyZhangFromSeeed 0:d4c2464a3868 70 bool grove_gyro_getangularvelocity(I2C_T *i2c, float *ax,float *ay,float *az)
JackyZhangFromSeeed 0:d4c2464a3868 71 {
JackyZhangFromSeeed 0:d4c2464a3868 72 int16_t x,y,z;
JackyZhangFromSeeed 0:d4c2464a3868 73 grove_gyro_getxyz(i2c, &x,&y,&z);
JackyZhangFromSeeed 0:d4c2464a3868 74 *ax = x/14.375;
JackyZhangFromSeeed 0:d4c2464a3868 75 *ay = y/14.375;
JackyZhangFromSeeed 0:d4c2464a3868 76 *az = z/14.375;
JackyZhangFromSeeed 0:d4c2464a3868 77
JackyZhangFromSeeed 0:d4c2464a3868 78 return true;
JackyZhangFromSeeed 0:d4c2464a3868 79 }
JackyZhangFromSeeed 0:d4c2464a3868 80
JackyZhangFromSeeed 0:d4c2464a3868 81 bool grove_gyro_zerocalibrate(I2C_T *i2c)
JackyZhangFromSeeed 0:d4c2464a3868 82 {
JackyZhangFromSeeed 0:d4c2464a3868 83 int16_t x_offset_temp;
JackyZhangFromSeeed 0:d4c2464a3868 84 int16_t y_offset_temp;
JackyZhangFromSeeed 0:d4c2464a3868 85 int16_t z_offset_temp;
JackyZhangFromSeeed 0:d4c2464a3868 86 int16_t x,y,z;
JackyZhangFromSeeed 0:d4c2464a3868 87 x_offset = 0;
JackyZhangFromSeeed 0:d4c2464a3868 88 y_offset = 0;
JackyZhangFromSeeed 0:d4c2464a3868 89 z_offset = 0;
JackyZhangFromSeeed 0:d4c2464a3868 90
JackyZhangFromSeeed 0:d4c2464a3868 91 for (int i = 0;i < 200;i++){
JackyZhangFromSeeed 0:d4c2464a3868 92 suli_delay_ms(10);
JackyZhangFromSeeed 0:d4c2464a3868 93 grove_gyro_getxyz(i2c, &x,&y,&z);
JackyZhangFromSeeed 0:d4c2464a3868 94 x_offset_temp += x;
JackyZhangFromSeeed 0:d4c2464a3868 95 y_offset_temp += y;
JackyZhangFromSeeed 0:d4c2464a3868 96 z_offset_temp += z;
JackyZhangFromSeeed 0:d4c2464a3868 97 }
JackyZhangFromSeeed 0:d4c2464a3868 98
JackyZhangFromSeeed 0:d4c2464a3868 99 x_offset = abs(x_offset_temp)/200;
JackyZhangFromSeeed 0:d4c2464a3868 100 y_offset = abs(y_offset_temp)/200;
JackyZhangFromSeeed 0:d4c2464a3868 101 z_offset = abs(z_offset_temp)/200;
JackyZhangFromSeeed 0:d4c2464a3868 102 if(x_offset_temp > 0)x_offset = -x_offset;
JackyZhangFromSeeed 0:d4c2464a3868 103 if(y_offset_temp > 0)y_offset = -y_offset;
JackyZhangFromSeeed 0:d4c2464a3868 104 if(z_offset_temp > 0)z_offset = -z_offset;
JackyZhangFromSeeed 0:d4c2464a3868 105
JackyZhangFromSeeed 0:d4c2464a3868 106 return true;
JackyZhangFromSeeed 0:d4c2464a3868 107 }