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?
User | Revision | Line number | New 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 | } |