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
ITG3200.cpp
00001 /** 00002 * Copyright (c) 2011 Pololu Corporation. For more information, see 00003 * 00004 * http://www.pololu.com/ 00005 * http://forum.pololu.com/ 00006 * 00007 * Permission is hereby granted, free of charge, to any person 00008 * obtaining a copy of this software and associated documentation 00009 * files (the "Software"), to deal in the Software without 00010 * restriction, including without limitation the rights to use, 00011 * copy, modify, merge, publish, distribute, sublicense, and/or sell 00012 * copies of the Software, and to permit persons to whom the 00013 * Software is furnished to do so, subject to the following 00014 * conditions: 00015 * 00016 * The above copyright notice and this permission notice shall be 00017 * included in all copies or substantial portions of the Software. 00018 * 00019 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 00020 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 00021 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 00022 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 00023 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 00024 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 00025 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 00026 * OTHER DEALINGS IN THE SOFTWARE. 00027 */ 00028 00029 #include "mbed.h" 00030 #include "ITG3200.h" 00031 #include <math.h> 00032 00033 // Defines //////////////////////////////////////////////////////////////// 00034 00035 // The Arduino two-wire interface uses a 7-bit number for the address, 00036 // and sets the last bit correctly based on reads and writes 00037 // mbed I2C libraries take the 7-bit address shifted left 1 bit 00038 // #define GYR_ADDRESS (0xD2 >> 1) 00039 #define GYR_ADDRESS (0x68 << 1) //ITG3200 00040 00041 // Public Methods ////////////////////////////////////////////////////////////// 00042 00043 // Constructor 00044 ITG3200::ITG3200(PinName sda, PinName scl): 00045 _device(sda, scl) 00046 { 00047 _device.frequency(50000); 00048 00049 //Set FS_SEL to 0x03 for proper operation. 00050 writeReg(SMPLRT_DIV_REG, 0x07); // samplr rte 1000hz 7sample low pass filter 00051 writeReg(DLPF_FS_REG, 0x03 << 3); 00052 // writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale default for ITG3200 00053 writeReg(PWR_MGM_REG, 0x01); // CLK_SEL to x gyro 00054 00055 setGains(2.0,2.0,2.0); 00056 setOffsets(0.0,0.0,0.0); 00057 00058 } 00059 00060 // Initialize gyro again 00061 void ITG3200::init(void) 00062 { 00063 //Set FS_SEL to 0x03 for proper operation. 00064 writeReg(SMPLRT_DIV_REG, 0x07); // samplr rte 1000hz 7sample low pass filter 00065 writeReg(DLPF_FS_REG, 0x03 << 3); 00066 // writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale default for ITG3200 00067 writeReg(PWR_MGM_REG, 0x01); // CLK_SEL to x gyro 00068 00069 // initialize varaibles 00070 setRevPolarity(false, false, false); 00071 setGains(1.0,1.0,1.0); 00072 setOffsets(0.0,0.0,0.0); 00073 } 00074 00075 // read status registers of ITG3200 00076 void ITG3200::status(byte *s) 00077 { 00078 s[0]=readReg(WHO_AM_I_REG); 00079 s[1]=readReg(SMPLRT_DIV_REG); 00080 s[2]=readReg(DLPF_FS_REG); 00081 s[3]=readReg(PWR_MGM_REG); 00082 } 00083 00084 // Writes a gyro register 00085 void ITG3200::writeReg(byte reg, byte value) 00086 { 00087 data[0] = reg; 00088 data[1] = value; 00089 00090 _device.write(GYR_ADDRESS, data, 2); 00091 } 00092 00093 // Reads a gyro register 00094 byte ITG3200::readReg(byte reg) 00095 { 00096 byte value = 0; 00097 00098 _device.write(GYR_ADDRESS, ®, 1); 00099 _device.read(GYR_ADDRESS, &value, 1); 00100 00101 return value; 00102 } 00103 00104 void ITG3200::setGains(float _Xgain, float _Ygain, float _Zgain) { 00105 gains[0] = _Xgain; 00106 gains[1] = _Ygain; 00107 gains[2] = _Zgain; 00108 } 00109 00110 void ITG3200::setOffsets(int _Xoffset, int _Yoffset, int _Zoffset) { 00111 offsets[0] = _Xoffset; 00112 offsets[1] = _Yoffset; 00113 offsets[2] = _Zoffset; 00114 } 00115 00116 void ITG3200::setRevPolarity(bool _Xpol, bool _Ypol, bool _Zpol) { 00117 polarities[0] = _Xpol ? -1 : 1; 00118 polarities[1] = _Ypol ? -1 : 1; 00119 polarities[2] = _Zpol ? -1 : 1; 00120 } 00121 00122 00123 void ITG3200::zeroCalibrate(unsigned int totSamples, unsigned int sampleDelayMS) { 00124 int xyz[3]; 00125 float tmpOffsets[] = {0,0,0}; 00126 00127 for (unsigned int i = 0;i < totSamples;i++){ 00128 wait_ms(sampleDelayMS); 00129 read(xyz); 00130 tmpOffsets[0] += xyz[0]; 00131 tmpOffsets[1] += xyz[1]; 00132 tmpOffsets[2] += xyz[2]; 00133 } 00134 setOffsets(-tmpOffsets[0] / totSamples, -tmpOffsets[1] / totSamples, -tmpOffsets[2] / totSamples); 00135 } 00136 00137 00138 // Reads the 3 gyro channels and stores them in vector g 00139 void ITG3200::read(int *g) 00140 { 00141 // assert the MSB of the address to get the gyro 00142 // to do slave-transmit subaddress updating. (ITG default) 00143 data[0] = GYRO_XOUT_H_REG; 00144 _device.write(GYR_ADDRESS, data, 1); 00145 00146 // Wire.requestFrom(GYR_ADDRESS, 6); 00147 // while (Wire.available() < 6); 00148 00149 _device.read(GYR_ADDRESS, data, 6); 00150 00151 uint8_t xha = data[0]; 00152 uint8_t xla = data[1]; 00153 uint8_t yha = data[2]; 00154 uint8_t yla = data[3]; 00155 uint8_t zha = data[4]; 00156 uint8_t zla = data[5]; 00157 00158 g[0] = (short) (yha << 8 | yla); 00159 g[1] = (short) (xha << 8 | xla); 00160 g[2] = (short) (zha << 8 | zla); 00161 } 00162 00163 void ITG3200::readRawCal(int *_GyroXYZ) { 00164 read(_GyroXYZ); 00165 _GyroXYZ[0] += offsets[0]; 00166 _GyroXYZ[1] += offsets[1]; 00167 _GyroXYZ[2] += offsets[2]; 00168 } 00169 00170 00171 00172 void ITG3200::read3(int x, int y, int z) { 00173 int* r2; 00174 //readings[0]=0; 00175 //readings[1]=0; 00176 //readings[2]=0; 00177 read(r2); 00178 00179 x = r2[0]; 00180 y = r2[1]; 00181 z = r2[2]; 00182 } 00183 00184 void ITG3200::readFin(float *_GyroXYZ){ 00185 int xyz[3]; 00186 00187 readRawCal(xyz); // x,y,z will contain calibrated integer values from the sensor 00188 _GyroXYZ[0] = (float)(xyz[0]) / (14.375 * polarities[0] * gains[0]); 00189 _GyroXYZ[1] = (float)(xyz[1]) / (14.375 * polarities[1] * gains[1]); 00190 _GyroXYZ[2] = (float)(xyz[2]) / (14.375 * polarities[2] * gains[2]); 00191 }
Generated on Wed Jul 20 2022 14:59:46 by 1.7.2