Larry Littlefield / ITG3200

Ported to have same calls as L3G4200D lib so it can be substituted in HK10dof library ( which is being modified to use this and renamed IMU10DOF) platform NUCLEO-F401

WARNING: This project is not complete, but this library seems ok so far.

I have the module DFRobotics.com 10DOF MEMS IMU. I wanted a consise module for resolving direction and movement.

I found HK10DOF library (http://developer.mbed.org/users/pommzorz/code/HK10DOF/) with quaternions. But it used a different gyro. So I modified that code to use the same higher level calls but use the ITG3200 low level calls.

ITG3200.cpp@0:13648b72bdf8, 2014-11-17 (annotated)

Committer:
svkatielee
Date:
Mon Nov 17 11:05:55 2014 +0000
Revision:
0:13648b72bdf8
Modified the L3G4200D gyro lib to be ITG3200 with the same calls so it can be substituted in HK10DOF MEMS library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
svkatielee 0:13648b72bdf8 1 /**
svkatielee 0:13648b72bdf8 2 * Copyright (c) 2011 Pololu Corporation. For more information, see
svkatielee 0:13648b72bdf8 3 *
svkatielee 0:13648b72bdf8 4 * http://www.pololu.com/
svkatielee 0:13648b72bdf8 5 * http://forum.pololu.com/
svkatielee 0:13648b72bdf8 6 *
svkatielee 0:13648b72bdf8 7 * Permission is hereby granted, free of charge, to any person
svkatielee 0:13648b72bdf8 8 * obtaining a copy of this software and associated documentation
svkatielee 0:13648b72bdf8 9 * files (the "Software"), to deal in the Software without
svkatielee 0:13648b72bdf8 10 * restriction, including without limitation the rights to use,
svkatielee 0:13648b72bdf8 11 * copy, modify, merge, publish, distribute, sublicense, and/or sell
svkatielee 0:13648b72bdf8 12 * copies of the Software, and to permit persons to whom the
svkatielee 0:13648b72bdf8 13 * Software is furnished to do so, subject to the following
svkatielee 0:13648b72bdf8 14 * conditions:
svkatielee 0:13648b72bdf8 15 *
svkatielee 0:13648b72bdf8 16 * The above copyright notice and this permission notice shall be
svkatielee 0:13648b72bdf8 17 * included in all copies or substantial portions of the Software.
svkatielee 0:13648b72bdf8 18 *
svkatielee 0:13648b72bdf8 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
svkatielee 0:13648b72bdf8 20 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
svkatielee 0:13648b72bdf8 21 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
svkatielee 0:13648b72bdf8 22 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
svkatielee 0:13648b72bdf8 23 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
svkatielee 0:13648b72bdf8 24 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
svkatielee 0:13648b72bdf8 25 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
svkatielee 0:13648b72bdf8 26 * OTHER DEALINGS IN THE SOFTWARE.
svkatielee 0:13648b72bdf8 27 */
svkatielee 0:13648b72bdf8 28
svkatielee 0:13648b72bdf8 29 #include "mbed.h"
svkatielee 0:13648b72bdf8 30 #include "ITG3200.h"
svkatielee 0:13648b72bdf8 31 #include <math.h>
svkatielee 0:13648b72bdf8 32
svkatielee 0:13648b72bdf8 33 // Defines ////////////////////////////////////////////////////////////////
svkatielee 0:13648b72bdf8 34
svkatielee 0:13648b72bdf8 35 // The Arduino two-wire interface uses a 7-bit number for the address,
svkatielee 0:13648b72bdf8 36 // and sets the last bit correctly based on reads and writes
svkatielee 0:13648b72bdf8 37 // mbed I2C libraries take the 7-bit address shifted left 1 bit
svkatielee 0:13648b72bdf8 38 // #define GYR_ADDRESS (0xD2 >> 1)
svkatielee 0:13648b72bdf8 39 #define GYR_ADDRESS (0x68 << 1) //ITG3200
svkatielee 0:13648b72bdf8 40
svkatielee 0:13648b72bdf8 41 // Public Methods //////////////////////////////////////////////////////////////
svkatielee 0:13648b72bdf8 42
svkatielee 0:13648b72bdf8 43 // Constructor
svkatielee 0:13648b72bdf8 44 ITG3200::ITG3200(PinName sda, PinName scl):
svkatielee 0:13648b72bdf8 45 _device(sda, scl)
svkatielee 0:13648b72bdf8 46 {
svkatielee 0:13648b72bdf8 47 _device.frequency(50000);
svkatielee 0:13648b72bdf8 48
svkatielee 0:13648b72bdf8 49 //Set FS_SEL to 0x03 for proper operation.
svkatielee 0:13648b72bdf8 50 writeReg(SMPLRT_DIV_REG, 0x07); // samplr rte 1000hz 7sample low pass filter
svkatielee 0:13648b72bdf8 51 writeReg(DLPF_FS_REG, 0x03 << 3);
svkatielee 0:13648b72bdf8 52 // writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale default for ITG3200
svkatielee 0:13648b72bdf8 53 writeReg(PWR_MGM_REG, 0x01); // CLK_SEL to x gyro
svkatielee 0:13648b72bdf8 54
svkatielee 0:13648b72bdf8 55 setGains(2.0,2.0,2.0);
svkatielee 0:13648b72bdf8 56 setOffsets(0.0,0.0,0.0);
svkatielee 0:13648b72bdf8 57
svkatielee 0:13648b72bdf8 58 }
svkatielee 0:13648b72bdf8 59
svkatielee 0:13648b72bdf8 60 // Initialize gyro again
svkatielee 0:13648b72bdf8 61 void ITG3200::init(void)
svkatielee 0:13648b72bdf8 62 {
svkatielee 0:13648b72bdf8 63 //Set FS_SEL to 0x03 for proper operation.
svkatielee 0:13648b72bdf8 64 writeReg(SMPLRT_DIV_REG, 0x07); // samplr rte 1000hz 7sample low pass filter
svkatielee 0:13648b72bdf8 65 writeReg(DLPF_FS_REG, 0x03 << 3);
svkatielee 0:13648b72bdf8 66 // writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale default for ITG3200
svkatielee 0:13648b72bdf8 67 writeReg(PWR_MGM_REG, 0x01); // CLK_SEL to x gyro
svkatielee 0:13648b72bdf8 68
svkatielee 0:13648b72bdf8 69 // initialize varaibles
svkatielee 0:13648b72bdf8 70 setRevPolarity(false, false, false);
svkatielee 0:13648b72bdf8 71 setGains(1.0,1.0,1.0);
svkatielee 0:13648b72bdf8 72 setOffsets(0.0,0.0,0.0);
svkatielee 0:13648b72bdf8 73 }
svkatielee 0:13648b72bdf8 74
svkatielee 0:13648b72bdf8 75 // read status registers of ITG3200
svkatielee 0:13648b72bdf8 76 void ITG3200::status(byte *s)
svkatielee 0:13648b72bdf8 77 {
svkatielee 0:13648b72bdf8 78 s[0]=readReg(WHO_AM_I_REG);
svkatielee 0:13648b72bdf8 79 s[1]=readReg(SMPLRT_DIV_REG);
svkatielee 0:13648b72bdf8 80 s[2]=readReg(DLPF_FS_REG);
svkatielee 0:13648b72bdf8 81 s[3]=readReg(PWR_MGM_REG);
svkatielee 0:13648b72bdf8 82 }
svkatielee 0:13648b72bdf8 83
svkatielee 0:13648b72bdf8 84 // Writes a gyro register
svkatielee 0:13648b72bdf8 85 void ITG3200::writeReg(byte reg, byte value)
svkatielee 0:13648b72bdf8 86 {
svkatielee 0:13648b72bdf8 87 data[0] = reg;
svkatielee 0:13648b72bdf8 88 data[1] = value;
svkatielee 0:13648b72bdf8 89
svkatielee 0:13648b72bdf8 90 _device.write(GYR_ADDRESS, data, 2);
svkatielee 0:13648b72bdf8 91 }
svkatielee 0:13648b72bdf8 92
svkatielee 0:13648b72bdf8 93 // Reads a gyro register
svkatielee 0:13648b72bdf8 94 byte ITG3200::readReg(byte reg)
svkatielee 0:13648b72bdf8 95 {
svkatielee 0:13648b72bdf8 96 byte value = 0;
svkatielee 0:13648b72bdf8 97
svkatielee 0:13648b72bdf8 98 _device.write(GYR_ADDRESS, &reg, 1);
svkatielee 0:13648b72bdf8 99 _device.read(GYR_ADDRESS, &value, 1);
svkatielee 0:13648b72bdf8 100
svkatielee 0:13648b72bdf8 101 return value;
svkatielee 0:13648b72bdf8 102 }
svkatielee 0:13648b72bdf8 103
svkatielee 0:13648b72bdf8 104 void ITG3200::setGains(float _Xgain, float _Ygain, float _Zgain) {
svkatielee 0:13648b72bdf8 105 gains[0] = _Xgain;
svkatielee 0:13648b72bdf8 106 gains[1] = _Ygain;
svkatielee 0:13648b72bdf8 107 gains[2] = _Zgain;
svkatielee 0:13648b72bdf8 108 }
svkatielee 0:13648b72bdf8 109
svkatielee 0:13648b72bdf8 110 void ITG3200::setOffsets(int _Xoffset, int _Yoffset, int _Zoffset) {
svkatielee 0:13648b72bdf8 111 offsets[0] = _Xoffset;
svkatielee 0:13648b72bdf8 112 offsets[1] = _Yoffset;
svkatielee 0:13648b72bdf8 113 offsets[2] = _Zoffset;
svkatielee 0:13648b72bdf8 114 }
svkatielee 0:13648b72bdf8 115
svkatielee 0:13648b72bdf8 116 void ITG3200::setRevPolarity(bool _Xpol, bool _Ypol, bool _Zpol) {
svkatielee 0:13648b72bdf8 117 polarities[0] = _Xpol ? -1 : 1;
svkatielee 0:13648b72bdf8 118 polarities[1] = _Ypol ? -1 : 1;
svkatielee 0:13648b72bdf8 119 polarities[2] = _Zpol ? -1 : 1;
svkatielee 0:13648b72bdf8 120 }
svkatielee 0:13648b72bdf8 121
svkatielee 0:13648b72bdf8 122
svkatielee 0:13648b72bdf8 123 void ITG3200::zeroCalibrate(unsigned int totSamples, unsigned int sampleDelayMS) {
svkatielee 0:13648b72bdf8 124 int xyz[3];
svkatielee 0:13648b72bdf8 125 float tmpOffsets[] = {0,0,0};
svkatielee 0:13648b72bdf8 126
svkatielee 0:13648b72bdf8 127 for (unsigned int i = 0;i < totSamples;i++){
svkatielee 0:13648b72bdf8 128 wait_ms(sampleDelayMS);
svkatielee 0:13648b72bdf8 129 read(xyz);
svkatielee 0:13648b72bdf8 130 tmpOffsets[0] += xyz[0];
svkatielee 0:13648b72bdf8 131 tmpOffsets[1] += xyz[1];
svkatielee 0:13648b72bdf8 132 tmpOffsets[2] += xyz[2];
svkatielee 0:13648b72bdf8 133 }
svkatielee 0:13648b72bdf8 134 setOffsets(-tmpOffsets[0] / totSamples, -tmpOffsets[1] / totSamples, -tmpOffsets[2] / totSamples);
svkatielee 0:13648b72bdf8 135 }
svkatielee 0:13648b72bdf8 136
svkatielee 0:13648b72bdf8 137
svkatielee 0:13648b72bdf8 138 // Reads the 3 gyro channels and stores them in vector g
svkatielee 0:13648b72bdf8 139 void ITG3200::read(int *g)
svkatielee 0:13648b72bdf8 140 {
svkatielee 0:13648b72bdf8 141 // assert the MSB of the address to get the gyro
svkatielee 0:13648b72bdf8 142 // to do slave-transmit subaddress updating. (ITG default)
svkatielee 0:13648b72bdf8 143 data[0] = GYRO_XOUT_H_REG;
svkatielee 0:13648b72bdf8 144 _device.write(GYR_ADDRESS, data, 1);
svkatielee 0:13648b72bdf8 145
svkatielee 0:13648b72bdf8 146 // Wire.requestFrom(GYR_ADDRESS, 6);
svkatielee 0:13648b72bdf8 147 // while (Wire.available() < 6);
svkatielee 0:13648b72bdf8 148
svkatielee 0:13648b72bdf8 149 _device.read(GYR_ADDRESS, data, 6);
svkatielee 0:13648b72bdf8 150
svkatielee 0:13648b72bdf8 151 uint8_t xha = data[0];
svkatielee 0:13648b72bdf8 152 uint8_t xla = data[1];
svkatielee 0:13648b72bdf8 153 uint8_t yha = data[2];
svkatielee 0:13648b72bdf8 154 uint8_t yla = data[3];
svkatielee 0:13648b72bdf8 155 uint8_t zha = data[4];
svkatielee 0:13648b72bdf8 156 uint8_t zla = data[5];
svkatielee 0:13648b72bdf8 157
svkatielee 0:13648b72bdf8 158 g[0] = (short) (yha << 8 | yla);
svkatielee 0:13648b72bdf8 159 g[1] = (short) (xha << 8 | xla);
svkatielee 0:13648b72bdf8 160 g[2] = (short) (zha << 8 | zla);
svkatielee 0:13648b72bdf8 161 }
svkatielee 0:13648b72bdf8 162
svkatielee 0:13648b72bdf8 163 void ITG3200::readRawCal(int *_GyroXYZ) {
svkatielee 0:13648b72bdf8 164 read(_GyroXYZ);
svkatielee 0:13648b72bdf8 165 _GyroXYZ[0] += offsets[0];
svkatielee 0:13648b72bdf8 166 _GyroXYZ[1] += offsets[1];
svkatielee 0:13648b72bdf8 167 _GyroXYZ[2] += offsets[2];
svkatielee 0:13648b72bdf8 168 }
svkatielee 0:13648b72bdf8 169
svkatielee 0:13648b72bdf8 170
svkatielee 0:13648b72bdf8 171
svkatielee 0:13648b72bdf8 172 void ITG3200::read3(int x, int y, int z) {
svkatielee 0:13648b72bdf8 173 int* r2;
svkatielee 0:13648b72bdf8 174 //readings[0]=0;
svkatielee 0:13648b72bdf8 175 //readings[1]=0;
svkatielee 0:13648b72bdf8 176 //readings[2]=0;
svkatielee 0:13648b72bdf8 177 read(r2);
svkatielee 0:13648b72bdf8 178
svkatielee 0:13648b72bdf8 179 x = r2[0];
svkatielee 0:13648b72bdf8 180 y = r2[1];
svkatielee 0:13648b72bdf8 181 z = r2[2];
svkatielee 0:13648b72bdf8 182 }
svkatielee 0:13648b72bdf8 183
svkatielee 0:13648b72bdf8 184 void ITG3200::readFin(float *_GyroXYZ){
svkatielee 0:13648b72bdf8 185 int xyz[3];
svkatielee 0:13648b72bdf8 186
svkatielee 0:13648b72bdf8 187 readRawCal(xyz); // x,y,z will contain calibrated integer values from the sensor
svkatielee 0:13648b72bdf8 188 _GyroXYZ[0] = (float)(xyz[0]) / (14.375 * polarities[0] * gains[0]);
svkatielee 0:13648b72bdf8 189 _GyroXYZ[1] = (float)(xyz[1]) / (14.375 * polarities[1] * gains[1]);
svkatielee 0:13648b72bdf8 190 _GyroXYZ[2] = (float)(xyz[2]) / (14.375 * polarities[2] * gains[2]);
svkatielee 0:13648b72bdf8 191 }