Abe Takumi / IMU_I2C

IMU

Dependents:   EM_Logger

IMU_I2C.cpp@0:7b3acf8e2a6f, 2013-07-05 (annotated)

Committer:
YSB
Date:
Fri Jul 05 04:16:49 2013 +0000
Revision:
0:7b3acf8e2a6f
ADXL345/ITG3200

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YSB 0:7b3acf8e2a6f 1 #include "IMU_I2C.h"
YSB 0:7b3acf8e2a6f 2 #include <math.h>
YSB 0:7b3acf8e2a6f 3 //#include "mbed.h"
YSB 0:7b3acf8e2a6f 4
YSB 0:7b3acf8e2a6f 5 #define GYR_ADDRESS 0xD2
YSB 0:7b3acf8e2a6f 6
YSB 0:7b3acf8e2a6f 7 IMU_I2C::IMU_I2C(PinName sda, PinName scl) : i2c_(sda, scl) {
YSB 0:7b3acf8e2a6f 8
YSB 0:7b3acf8e2a6f 9 ///////////////////ADXL345////////////////////////////////////////////////////
YSB 0:7b3acf8e2a6f 10 //400kHz, allowing us to use the fastest data rates.
YSB 0:7b3acf8e2a6f 11 i2c_.frequency(400000);
YSB 0:7b3acf8e2a6f 12 // initialize the BW data rate
YSB 0:7b3acf8e2a6f 13 char tx[2];
YSB 0:7b3acf8e2a6f 14 tx[0] = ADXL345_BW_RATE_REG;
YSB 0:7b3acf8e2a6f 15 tx[1] = ADXL345_1600HZ; //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register
YSB 0:7b3acf8e2a6f 16 i2c_.write( ADXL345_I2C_WRITE , tx, 2);
YSB 0:7b3acf8e2a6f 17
YSB 0:7b3acf8e2a6f 18 //Data format (for +-16g) - This is done by setting Bit D3 of the DATA_FORMAT register (Address 0x31) and writing a value of 0x03 to the range bits (Bit D1 and Bit D0) of the DATA_FORMAT register (Address 0x31).
YSB 0:7b3acf8e2a6f 19
YSB 0:7b3acf8e2a6f 20 char rx[2];
YSB 0:7b3acf8e2a6f 21 rx[0] = ADXL345_DATA_FORMAT_REG;
YSB 0:7b3acf8e2a6f 22 rx[1] = 0x0B;
YSB 0:7b3acf8e2a6f 23 // full res and +_16g
YSB 0:7b3acf8e2a6f 24 i2c_.write( ADXL345_I2C_WRITE , rx, 2);
YSB 0:7b3acf8e2a6f 25
YSB 0:7b3acf8e2a6f 26 // Set Offset - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
YSB 0:7b3acf8e2a6f 27 char x[2];
YSB 0:7b3acf8e2a6f 28 x[0] = ADXL345_OFSX_REG ;
YSB 0:7b3acf8e2a6f 29 x[1] = 0xFD;
YSB 0:7b3acf8e2a6f 30 i2c_.write( ADXL345_I2C_WRITE , x, 2);
YSB 0:7b3acf8e2a6f 31 char y[2];
YSB 0:7b3acf8e2a6f 32 y[0] = ADXL345_OFSY_REG ;
YSB 0:7b3acf8e2a6f 33 y[1] = 0x03;
YSB 0:7b3acf8e2a6f 34 i2c_.write( ADXL345_I2C_WRITE , y, 2);
YSB 0:7b3acf8e2a6f 35 char z[2];
YSB 0:7b3acf8e2a6f 36 z[0] = ADXL345_OFSZ_REG ;
YSB 0:7b3acf8e2a6f 37 z[1] = 0xFE;
YSB 0:7b3acf8e2a6f 38 i2c_.write( ADXL345_I2C_WRITE , z, 2);
YSB 0:7b3acf8e2a6f 39
YSB 0:7b3acf8e2a6f 40 //////////////////L3G4200D//////////////////////////////////////////////
YSB 0:7b3acf8e2a6f 41 // Turns on the L3G4200D's gyro and places it in normal mode.
YSB 0:7b3acf8e2a6f 42 // 0x0F = 0b00001111
YSB 0:7b3acf8e2a6f 43 // Normal power mode, all axes enabled
YSB 0:7b3acf8e2a6f 44 writeReg(L3G4200D_CTRL_REG1, 0x0F);
YSB 0:7b3acf8e2a6f 45 writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale_device.frequency(400000);
YSB 0:7b3acf8e2a6f 46 // Turns on the L3G4200D's gyro and places it in normal mode.
YSB 0:7b3acf8e2a6f 47 // 0x0F = 0b00001111
YSB 0:7b3acf8e2a6f 48 // Normal power mode, all axes enabled
YSB 0:7b3acf8e2a6f 49 writeReg(L3G4200D_CTRL_REG1, 0x0F);
YSB 0:7b3acf8e2a6f 50 writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale
YSB 0:7b3acf8e2a6f 51
YSB 0:7b3acf8e2a6f 52 /////////////////ITG3200//////////////////////////////////////////////////
YSB 0:7b3acf8e2a6f 53 //Set FS_SEL to 0x03 for proper operation.
YSB 0:7b3acf8e2a6f 54 //See datasheet for details.
YSB 0:7b3acf8e2a6f 55 char gtx[2];
YSB 0:7b3acf8e2a6f 56 gtx[0] = DLPF_FS_REG;
YSB 0:7b3acf8e2a6f 57 //FS_SEL bits sit in bits 4 and 3 of DLPF_FS register.
YSB 0:7b3acf8e2a6f 58 gtx[1] = 0x03 << 3;
YSB 0:7b3acf8e2a6f 59 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, gtx, 2);
YSB 0:7b3acf8e2a6f 60
YSB 0:7b3acf8e2a6f 61 }
YSB 0:7b3acf8e2a6f 62
YSB 0:7b3acf8e2a6f 63
YSB 0:7b3acf8e2a6f 64 char IMU_I2C::SingleByteRead(char address){
YSB 0:7b3acf8e2a6f 65 char tx = address;
YSB 0:7b3acf8e2a6f 66 char output;
YSB 0:7b3acf8e2a6f 67 i2c_.write( ADXL345_I2C_WRITE , &tx, 1); //tell it what you want to read
YSB 0:7b3acf8e2a6f 68 i2c_.read( ADXL345_I2C_READ , &output, 1); //tell it where to store the data
YSB 0:7b3acf8e2a6f 69 return output;
YSB 0:7b3acf8e2a6f 70
YSB 0:7b3acf8e2a6f 71 }
YSB 0:7b3acf8e2a6f 72
YSB 0:7b3acf8e2a6f 73 int IMU_I2C::SingleByteWrite(char address, char data){
YSB 0:7b3acf8e2a6f 74 int ack = 0;
YSB 0:7b3acf8e2a6f 75 char tx[2];
YSB 0:7b3acf8e2a6f 76 tx[0] = address;
YSB 0:7b3acf8e2a6f 77 tx[1] = data;
YSB 0:7b3acf8e2a6f 78 return ack | i2c_.write( ADXL345_I2C_WRITE , tx, 2);
YSB 0:7b3acf8e2a6f 79 }
YSB 0:7b3acf8e2a6f 80
YSB 0:7b3acf8e2a6f 81 void IMU_I2C::multiByteRead(char address, char* output, int size) {
YSB 0:7b3acf8e2a6f 82 i2c_.write( ADXL345_I2C_WRITE, &address, 1); //tell it where to read from
YSB 0:7b3acf8e2a6f 83 i2c_.read( ADXL345_I2C_READ , output, size); //tell it where to store the data read
YSB 0:7b3acf8e2a6f 84 }
YSB 0:7b3acf8e2a6f 85
YSB 0:7b3acf8e2a6f 86 int IMU_I2C::multiByteWrite(char address, char* ptr_data, int size) {
YSB 0:7b3acf8e2a6f 87 int ack;
YSB 0:7b3acf8e2a6f 88
YSB 0:7b3acf8e2a6f 89 ack = i2c_.write( ADXL345_I2C_WRITE, &address, 1); //tell it where to write to
YSB 0:7b3acf8e2a6f 90 return ack | i2c_.write( ADXL345_I2C_READ, ptr_data, size); //tell it what data to write
YSB 0:7b3acf8e2a6f 91
YSB 0:7b3acf8e2a6f 92 }
YSB 0:7b3acf8e2a6f 93
YSB 0:7b3acf8e2a6f 94 void IMU_I2C::getOutput(int* readings){
YSB 0:7b3acf8e2a6f 95 char buffer[6];
YSB 0:7b3acf8e2a6f 96 multiByteRead(ADXL345_DATAX0_REG, buffer, 6);
YSB 0:7b3acf8e2a6f 97
YSB 0:7b3acf8e2a6f 98 readings[0] = (int)buffer[1] << 8 | (int)buffer[0];
YSB 0:7b3acf8e2a6f 99 readings[1] = (int)buffer[3] << 8 | (int)buffer[2];
YSB 0:7b3acf8e2a6f 100 readings[2] = (int)buffer[5] << 8 | (int)buffer[4];
YSB 0:7b3acf8e2a6f 101
YSB 0:7b3acf8e2a6f 102 }
YSB 0:7b3acf8e2a6f 103
YSB 0:7b3acf8e2a6f 104 char IMU_I2C::getDeviceID() {
YSB 0:7b3acf8e2a6f 105 return SingleByteRead(ADXL345_DEVID_REG);
YSB 0:7b3acf8e2a6f 106 }
YSB 0:7b3acf8e2a6f 107
YSB 0:7b3acf8e2a6f 108 int IMU_I2C::setPowerMode(char mode) {
YSB 0:7b3acf8e2a6f 109
YSB 0:7b3acf8e2a6f 110 //Get the current register contents, so we don't clobber the rate value.
YSB 0:7b3acf8e2a6f 111 char registerContents = (mode << 4) | SingleByteRead(ADXL345_BW_RATE_REG);
YSB 0:7b3acf8e2a6f 112
YSB 0:7b3acf8e2a6f 113 return SingleByteWrite(ADXL345_BW_RATE_REG, registerContents);
YSB 0:7b3acf8e2a6f 114
YSB 0:7b3acf8e2a6f 115 }
YSB 0:7b3acf8e2a6f 116
YSB 0:7b3acf8e2a6f 117 char IMU_I2C::getPowerControl() {
YSB 0:7b3acf8e2a6f 118 return SingleByteRead(ADXL345_POWER_CTL_REG);
YSB 0:7b3acf8e2a6f 119 }
YSB 0:7b3acf8e2a6f 120
YSB 0:7b3acf8e2a6f 121 int IMU_I2C::setPowerControl(char settings) {
YSB 0:7b3acf8e2a6f 122 return SingleByteWrite(ADXL345_POWER_CTL_REG, settings);
YSB 0:7b3acf8e2a6f 123
YSB 0:7b3acf8e2a6f 124 }
YSB 0:7b3acf8e2a6f 125
YSB 0:7b3acf8e2a6f 126 char IMU_I2C::getDataFormatControl(void){
YSB 0:7b3acf8e2a6f 127
YSB 0:7b3acf8e2a6f 128 return SingleByteRead(ADXL345_DATA_FORMAT_REG);
YSB 0:7b3acf8e2a6f 129 }
YSB 0:7b3acf8e2a6f 130
YSB 0:7b3acf8e2a6f 131 int IMU_I2C::setDataFormatControl(char settings){
YSB 0:7b3acf8e2a6f 132
YSB 0:7b3acf8e2a6f 133 return SingleByteWrite(ADXL345_DATA_FORMAT_REG, settings);
YSB 0:7b3acf8e2a6f 134
YSB 0:7b3acf8e2a6f 135 }
YSB 0:7b3acf8e2a6f 136
YSB 0:7b3acf8e2a6f 137 int IMU_I2C::setDataRate(char rate) {
YSB 0:7b3acf8e2a6f 138
YSB 0:7b3acf8e2a6f 139 //Get the current register contents, so we don't clobber the power bit.
YSB 0:7b3acf8e2a6f 140 char registerContents = SingleByteRead(ADXL345_BW_RATE_REG);
YSB 0:7b3acf8e2a6f 141
YSB 0:7b3acf8e2a6f 142 registerContents &= 0x10;
YSB 0:7b3acf8e2a6f 143 registerContents |= rate;
YSB 0:7b3acf8e2a6f 144
YSB 0:7b3acf8e2a6f 145 return SingleByteWrite(ADXL345_BW_RATE_REG, registerContents);
YSB 0:7b3acf8e2a6f 146 }
YSB 0:7b3acf8e2a6f 147
YSB 0:7b3acf8e2a6f 148 char IMU_I2C::getOffset(char axis) {
YSB 0:7b3acf8e2a6f 149
YSB 0:7b3acf8e2a6f 150 char address = 0;
YSB 0:7b3acf8e2a6f 151
YSB 0:7b3acf8e2a6f 152 if (axis == ADXL345_X) {
YSB 0:7b3acf8e2a6f 153 address = ADXL345_OFSX_REG;
YSB 0:7b3acf8e2a6f 154 } else if (axis == ADXL345_Y) {
YSB 0:7b3acf8e2a6f 155 address = ADXL345_OFSY_REG;
YSB 0:7b3acf8e2a6f 156 } else if (axis == ADXL345_Z) {
YSB 0:7b3acf8e2a6f 157 address = ADXL345_OFSZ_REG;
YSB 0:7b3acf8e2a6f 158 }
YSB 0:7b3acf8e2a6f 159
YSB 0:7b3acf8e2a6f 160 return SingleByteRead(address);
YSB 0:7b3acf8e2a6f 161 }
YSB 0:7b3acf8e2a6f 162
YSB 0:7b3acf8e2a6f 163 int IMU_I2C::setOffset(char axis, char offset) {
YSB 0:7b3acf8e2a6f 164
YSB 0:7b3acf8e2a6f 165 char address = 0;
YSB 0:7b3acf8e2a6f 166
YSB 0:7b3acf8e2a6f 167 if (axis == ADXL345_X) {
YSB 0:7b3acf8e2a6f 168 address = ADXL345_OFSX_REG;
YSB 0:7b3acf8e2a6f 169 } else if (axis == ADXL345_Y) {
YSB 0:7b3acf8e2a6f 170 address = ADXL345_OFSY_REG;
YSB 0:7b3acf8e2a6f 171 } else if (axis == ADXL345_Z) {
YSB 0:7b3acf8e2a6f 172 address = ADXL345_OFSZ_REG;
YSB 0:7b3acf8e2a6f 173 }
YSB 0:7b3acf8e2a6f 174 return SingleByteWrite(address, offset);
YSB 0:7b3acf8e2a6f 175 }
YSB 0:7b3acf8e2a6f 176
YSB 0:7b3acf8e2a6f 177 char IMU_I2C::getFifoControl(void){
YSB 0:7b3acf8e2a6f 178
YSB 0:7b3acf8e2a6f 179 return SingleByteRead(ADXL345_FIFO_CTL);
YSB 0:7b3acf8e2a6f 180 }
YSB 0:7b3acf8e2a6f 181
YSB 0:7b3acf8e2a6f 182 int IMU_I2C::setFifoControl(char settings){
YSB 0:7b3acf8e2a6f 183 return SingleByteWrite(ADXL345_FIFO_STATUS, settings);
YSB 0:7b3acf8e2a6f 184
YSB 0:7b3acf8e2a6f 185 }
YSB 0:7b3acf8e2a6f 186
YSB 0:7b3acf8e2a6f 187 char IMU_I2C::getFifoStatus(void){
YSB 0:7b3acf8e2a6f 188 return SingleByteRead(ADXL345_FIFO_STATUS);
YSB 0:7b3acf8e2a6f 189 }
YSB 0:7b3acf8e2a6f 190
YSB 0:7b3acf8e2a6f 191 char IMU_I2C::getTapThreshold(void) {
YSB 0:7b3acf8e2a6f 192
YSB 0:7b3acf8e2a6f 193 return SingleByteRead(ADXL345_THRESH_TAP_REG);
YSB 0:7b3acf8e2a6f 194 }
YSB 0:7b3acf8e2a6f 195
YSB 0:7b3acf8e2a6f 196 int IMU_I2C::setTapThreshold(char threshold) {
YSB 0:7b3acf8e2a6f 197
YSB 0:7b3acf8e2a6f 198 return SingleByteWrite(ADXL345_THRESH_TAP_REG, threshold);
YSB 0:7b3acf8e2a6f 199 }
YSB 0:7b3acf8e2a6f 200
YSB 0:7b3acf8e2a6f 201 float IMU_I2C::getTapDuration(void) {
YSB 0:7b3acf8e2a6f 202
YSB 0:7b3acf8e2a6f 203 return (float)SingleByteRead(ADXL345_DUR_REG)*625;
YSB 0:7b3acf8e2a6f 204 }
YSB 0:7b3acf8e2a6f 205
YSB 0:7b3acf8e2a6f 206 int IMU_I2C::setTapDuration(short int duration_us) {
YSB 0:7b3acf8e2a6f 207
YSB 0:7b3acf8e2a6f 208 short int tapDuration = duration_us / 625;
YSB 0:7b3acf8e2a6f 209 char tapChar[2];
YSB 0:7b3acf8e2a6f 210 tapChar[0] = (tapDuration & 0x00FF);
YSB 0:7b3acf8e2a6f 211 tapChar[1] = (tapDuration >> 8) & 0x00FF;
YSB 0:7b3acf8e2a6f 212 return multiByteWrite(ADXL345_DUR_REG, tapChar, 2);
YSB 0:7b3acf8e2a6f 213 }
YSB 0:7b3acf8e2a6f 214
YSB 0:7b3acf8e2a6f 215 float IMU_I2C::getTapLatency(void) {
YSB 0:7b3acf8e2a6f 216
YSB 0:7b3acf8e2a6f 217 return (float)SingleByteRead(ADXL345_LATENT_REG)*1.25;
YSB 0:7b3acf8e2a6f 218 }
YSB 0:7b3acf8e2a6f 219
YSB 0:7b3acf8e2a6f 220 int IMU_I2C::setTapLatency(short int latency_ms) {
YSB 0:7b3acf8e2a6f 221
YSB 0:7b3acf8e2a6f 222 latency_ms = latency_ms / 1.25;
YSB 0:7b3acf8e2a6f 223 char latChar[2];
YSB 0:7b3acf8e2a6f 224 latChar[0] = (latency_ms & 0x00FF);
YSB 0:7b3acf8e2a6f 225 latChar[1] = (latency_ms << 8) & 0xFF00;
YSB 0:7b3acf8e2a6f 226 return multiByteWrite(ADXL345_LATENT_REG, latChar, 2);
YSB 0:7b3acf8e2a6f 227 }
YSB 0:7b3acf8e2a6f 228
YSB 0:7b3acf8e2a6f 229 float IMU_I2C::getWindowTime(void) {
YSB 0:7b3acf8e2a6f 230
YSB 0:7b3acf8e2a6f 231 return (float)SingleByteRead(ADXL345_WINDOW_REG)*1.25;
YSB 0:7b3acf8e2a6f 232 }
YSB 0:7b3acf8e2a6f 233
YSB 0:7b3acf8e2a6f 234 int IMU_I2C::setWindowTime(short int window_ms) {
YSB 0:7b3acf8e2a6f 235
YSB 0:7b3acf8e2a6f 236 window_ms = window_ms / 1.25;
YSB 0:7b3acf8e2a6f 237 char windowChar[2];
YSB 0:7b3acf8e2a6f 238 windowChar[0] = (window_ms & 0x00FF);
YSB 0:7b3acf8e2a6f 239 windowChar[1] = ((window_ms << 8) & 0xFF00);
YSB 0:7b3acf8e2a6f 240 return multiByteWrite(ADXL345_WINDOW_REG, windowChar, 2);
YSB 0:7b3acf8e2a6f 241 }
YSB 0:7b3acf8e2a6f 242
YSB 0:7b3acf8e2a6f 243 char IMU_I2C::getActivityThreshold(void) {
YSB 0:7b3acf8e2a6f 244
YSB 0:7b3acf8e2a6f 245 return SingleByteRead(ADXL345_THRESH_ACT_REG);
YSB 0:7b3acf8e2a6f 246 }
YSB 0:7b3acf8e2a6f 247
YSB 0:7b3acf8e2a6f 248 int IMU_I2C::setActivityThreshold(char threshold) {
YSB 0:7b3acf8e2a6f 249 return SingleByteWrite(ADXL345_THRESH_ACT_REG, threshold);
YSB 0:7b3acf8e2a6f 250 }
YSB 0:7b3acf8e2a6f 251
YSB 0:7b3acf8e2a6f 252 char IMU_I2C::getInactivityThreshold(void) {
YSB 0:7b3acf8e2a6f 253 return SingleByteRead(ADXL345_THRESH_INACT_REG);
YSB 0:7b3acf8e2a6f 254 }
YSB 0:7b3acf8e2a6f 255
YSB 0:7b3acf8e2a6f 256 //int FUNCTION(short int * ptr_Output)
YSB 0:7b3acf8e2a6f 257 //short int FUNCTION ()
YSB 0:7b3acf8e2a6f 258
YSB 0:7b3acf8e2a6f 259 int IMU_I2C::setInactivityThreshold(char threshold) {
YSB 0:7b3acf8e2a6f 260 return SingleByteWrite(ADXL345_THRESH_INACT_REG, threshold);
YSB 0:7b3acf8e2a6f 261 }
YSB 0:7b3acf8e2a6f 262
YSB 0:7b3acf8e2a6f 263 char IMU_I2C::getTimeInactivity(void) {
YSB 0:7b3acf8e2a6f 264
YSB 0:7b3acf8e2a6f 265 return SingleByteRead(ADXL345_TIME_INACT_REG);
YSB 0:7b3acf8e2a6f 266 }
YSB 0:7b3acf8e2a6f 267
YSB 0:7b3acf8e2a6f 268 int IMU_I2C::setTimeInactivity(char timeInactivity) {
YSB 0:7b3acf8e2a6f 269 return SingleByteWrite(ADXL345_TIME_INACT_REG, timeInactivity);
YSB 0:7b3acf8e2a6f 270 }
YSB 0:7b3acf8e2a6f 271
YSB 0:7b3acf8e2a6f 272 char IMU_I2C::getActivityInactivityControl(void) {
YSB 0:7b3acf8e2a6f 273
YSB 0:7b3acf8e2a6f 274 return SingleByteRead(ADXL345_ACT_INACT_CTL_REG);
YSB 0:7b3acf8e2a6f 275 }
YSB 0:7b3acf8e2a6f 276
YSB 0:7b3acf8e2a6f 277 int IMU_I2C::setActivityInactivityControl(char settings) {
YSB 0:7b3acf8e2a6f 278 return SingleByteWrite(ADXL345_ACT_INACT_CTL_REG, settings);
YSB 0:7b3acf8e2a6f 279 }
YSB 0:7b3acf8e2a6f 280
YSB 0:7b3acf8e2a6f 281 char IMU_I2C::getFreefallThreshold(void) {
YSB 0:7b3acf8e2a6f 282 return SingleByteRead(ADXL345_THRESH_FF_REG);
YSB 0:7b3acf8e2a6f 283 }
YSB 0:7b3acf8e2a6f 284
YSB 0:7b3acf8e2a6f 285 int IMU_I2C::setFreefallThreshold(char threshold) {
YSB 0:7b3acf8e2a6f 286 return SingleByteWrite(ADXL345_THRESH_FF_REG, threshold);
YSB 0:7b3acf8e2a6f 287 }
YSB 0:7b3acf8e2a6f 288
YSB 0:7b3acf8e2a6f 289 char IMU_I2C::getFreefallTime(void) {
YSB 0:7b3acf8e2a6f 290 return SingleByteRead(ADXL345_TIME_FF_REG)*5;
YSB 0:7b3acf8e2a6f 291 }
YSB 0:7b3acf8e2a6f 292
YSB 0:7b3acf8e2a6f 293 int IMU_I2C::setFreefallTime(short int freefallTime_ms) {
YSB 0:7b3acf8e2a6f 294 freefallTime_ms = freefallTime_ms / 5;
YSB 0:7b3acf8e2a6f 295 char fallChar[2];
YSB 0:7b3acf8e2a6f 296 fallChar[0] = (freefallTime_ms & 0x00FF);
YSB 0:7b3acf8e2a6f 297 fallChar[1] = (freefallTime_ms << 8) & 0xFF00;
YSB 0:7b3acf8e2a6f 298
YSB 0:7b3acf8e2a6f 299 return multiByteWrite(ADXL345_TIME_FF_REG, fallChar, 2);
YSB 0:7b3acf8e2a6f 300 }
YSB 0:7b3acf8e2a6f 301
YSB 0:7b3acf8e2a6f 302 char IMU_I2C::getTapAxisControl(void) {
YSB 0:7b3acf8e2a6f 303 return SingleByteRead(ADXL345_TAP_AXES_REG);
YSB 0:7b3acf8e2a6f 304 }
YSB 0:7b3acf8e2a6f 305
YSB 0:7b3acf8e2a6f 306 int IMU_I2C::setTapAxisControl(char settings) {
YSB 0:7b3acf8e2a6f 307 return SingleByteWrite(ADXL345_TAP_AXES_REG, settings);
YSB 0:7b3acf8e2a6f 308 }
YSB 0:7b3acf8e2a6f 309
YSB 0:7b3acf8e2a6f 310 char IMU_I2C::getTapSource(void) {
YSB 0:7b3acf8e2a6f 311 return SingleByteRead(ADXL345_ACT_TAP_STATUS_REG);
YSB 0:7b3acf8e2a6f 312 }
YSB 0:7b3acf8e2a6f 313
YSB 0:7b3acf8e2a6f 314 char IMU_I2C::getInterruptEnableControl(void) {
YSB 0:7b3acf8e2a6f 315 return SingleByteRead(ADXL345_INT_ENABLE_REG);
YSB 0:7b3acf8e2a6f 316 }
YSB 0:7b3acf8e2a6f 317
YSB 0:7b3acf8e2a6f 318 int IMU_I2C::setInterruptEnableControl(char settings) {
YSB 0:7b3acf8e2a6f 319 return SingleByteWrite(ADXL345_INT_ENABLE_REG, settings);
YSB 0:7b3acf8e2a6f 320 }
YSB 0:7b3acf8e2a6f 321
YSB 0:7b3acf8e2a6f 322 char IMU_I2C::getInterruptMappingControl(void) {
YSB 0:7b3acf8e2a6f 323 return SingleByteRead(ADXL345_INT_MAP_REG);
YSB 0:7b3acf8e2a6f 324 }
YSB 0:7b3acf8e2a6f 325
YSB 0:7b3acf8e2a6f 326 int IMU_I2C::setInterruptMappingControl(char settings) {
YSB 0:7b3acf8e2a6f 327 return SingleByteWrite(ADXL345_INT_MAP_REG, settings);
YSB 0:7b3acf8e2a6f 328 }
YSB 0:7b3acf8e2a6f 329
YSB 0:7b3acf8e2a6f 330 char IMU_I2C::getInterruptSource(void){
YSB 0:7b3acf8e2a6f 331 return SingleByteRead(ADXL345_INT_SOURCE_REG);
YSB 0:7b3acf8e2a6f 332 }
YSB 0:7b3acf8e2a6f 333
YSB 0:7b3acf8e2a6f 334 //////////////L3G4200D//////////////////////////////////////////////////////////
YSB 0:7b3acf8e2a6f 335
YSB 0:7b3acf8e2a6f 336 // Writes a gyro register
YSB 0:7b3acf8e2a6f 337 void IMU_I2C::writeReg(byte reg, byte value)
YSB 0:7b3acf8e2a6f 338 {
YSB 0:7b3acf8e2a6f 339 data[0] = reg;
YSB 0:7b3acf8e2a6f 340 data[1] = value;
YSB 0:7b3acf8e2a6f 341
YSB 0:7b3acf8e2a6f 342 i2c_.write(GYR_ADDRESS, data, 2);
YSB 0:7b3acf8e2a6f 343 }// Writes a gyro register
YSB 0:7b3acf8e2a6f 344
YSB 0:7b3acf8e2a6f 345 // Reads a gyro register
YSB 0:7b3acf8e2a6f 346 byte IMU_I2C::readReg(byte reg)
YSB 0:7b3acf8e2a6f 347 {
YSB 0:7b3acf8e2a6f 348 byte value = 0;
YSB 0:7b3acf8e2a6f 349
YSB 0:7b3acf8e2a6f 350 i2c_.write(GYR_ADDRESS, &reg, 1);
YSB 0:7b3acf8e2a6f 351 i2c_.read(GYR_ADDRESS, &value, 1);
YSB 0:7b3acf8e2a6f 352
YSB 0:7b3acf8e2a6f 353 return value;
YSB 0:7b3acf8e2a6f 354 }// Reads a gyro register
YSB 0:7b3acf8e2a6f 355
YSB 0:7b3acf8e2a6f 356 // Reads the 3 gyro channels and stores them in vector g
YSB 0:7b3acf8e2a6f 357 void IMU_I2C::read(int g[3])
YSB 0:7b3acf8e2a6f 358 {
YSB 0:7b3acf8e2a6f 359 // assert the MSB of the address to get the gyro
YSB 0:7b3acf8e2a6f 360 // to do slave-transmit subaddress updating.
YSB 0:7b3acf8e2a6f 361 data[0] = L3G4200D_OUT_X_L | (1 << 7);
YSB 0:7b3acf8e2a6f 362 i2c_.write(GYR_ADDRESS, data, 1);
YSB 0:7b3acf8e2a6f 363
YSB 0:7b3acf8e2a6f 364 // Wire.requestFrom(GYR_ADDRESS, 6);
YSB 0:7b3acf8e2a6f 365 // while (Wire.available() < 6);
YSB 0:7b3acf8e2a6f 366
YSB 0:7b3acf8e2a6f 367 i2c_.read(GYR_ADDRESS, data, 6);
YSB 0:7b3acf8e2a6f 368
YSB 0:7b3acf8e2a6f 369 uint8_t xla = data[0];
YSB 0:7b3acf8e2a6f 370 uint8_t xha = data[1];
YSB 0:7b3acf8e2a6f 371 uint8_t yla = data[2];
YSB 0:7b3acf8e2a6f 372 uint8_t yha = data[3];
YSB 0:7b3acf8e2a6f 373 uint8_t zla = data[4];
YSB 0:7b3acf8e2a6f 374 uint8_t zha = data[5];
YSB 0:7b3acf8e2a6f 375
YSB 0:7b3acf8e2a6f 376 g[0] = (short) (xha << 8 | xla);
YSB 0:7b3acf8e2a6f 377 g[1] = (short) (yha << 8 | yla);
YSB 0:7b3acf8e2a6f 378 g[2] = (short) (zha << 8 | zla);
YSB 0:7b3acf8e2a6f 379 }
YSB 0:7b3acf8e2a6f 380
YSB 0:7b3acf8e2a6f 381 char IMU_I2C::getWhoAmI(void){
YSB 0:7b3acf8e2a6f 382
YSB 0:7b3acf8e2a6f 383 //WhoAmI Register address.
YSB 0:7b3acf8e2a6f 384 char tx = WHO_AM_I_REG;
YSB 0:7b3acf8e2a6f 385 char rx;
YSB 0:7b3acf8e2a6f 386
YSB 0:7b3acf8e2a6f 387 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 388
YSB 0:7b3acf8e2a6f 389 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
YSB 0:7b3acf8e2a6f 390
YSB 0:7b3acf8e2a6f 391 return rx;
YSB 0:7b3acf8e2a6f 392
YSB 0:7b3acf8e2a6f 393 }
YSB 0:7b3acf8e2a6f 394
YSB 0:7b3acf8e2a6f 395 void IMU_I2C::setWhoAmI(char address){
YSB 0:7b3acf8e2a6f 396
YSB 0:7b3acf8e2a6f 397 char tx[2];
YSB 0:7b3acf8e2a6f 398 tx[0] = WHO_AM_I_REG;
YSB 0:7b3acf8e2a6f 399 tx[1] = address;
YSB 0:7b3acf8e2a6f 400
YSB 0:7b3acf8e2a6f 401 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
YSB 0:7b3acf8e2a6f 402
YSB 0:7b3acf8e2a6f 403 }
YSB 0:7b3acf8e2a6f 404
YSB 0:7b3acf8e2a6f 405 char IMU_I2C::getSampleRateDivider(void){
YSB 0:7b3acf8e2a6f 406
YSB 0:7b3acf8e2a6f 407 char tx = SMPLRT_DIV_REG;
YSB 0:7b3acf8e2a6f 408 char rx;
YSB 0:7b3acf8e2a6f 409
YSB 0:7b3acf8e2a6f 410 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 411
YSB 0:7b3acf8e2a6f 412 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
YSB 0:7b3acf8e2a6f 413
YSB 0:7b3acf8e2a6f 414 return rx;
YSB 0:7b3acf8e2a6f 415
YSB 0:7b3acf8e2a6f 416 }
YSB 0:7b3acf8e2a6f 417
YSB 0:7b3acf8e2a6f 418 void IMU_I2C::setSampleRateDivider(char divider){
YSB 0:7b3acf8e2a6f 419
YSB 0:7b3acf8e2a6f 420 char tx[2];
YSB 0:7b3acf8e2a6f 421 tx[0] = SMPLRT_DIV_REG;
YSB 0:7b3acf8e2a6f 422 tx[1] = divider;
YSB 0:7b3acf8e2a6f 423
YSB 0:7b3acf8e2a6f 424 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
YSB 0:7b3acf8e2a6f 425
YSB 0:7b3acf8e2a6f 426 }
YSB 0:7b3acf8e2a6f 427
YSB 0:7b3acf8e2a6f 428 int IMU_I2C::getInternalSampleRate(void){
YSB 0:7b3acf8e2a6f 429
YSB 0:7b3acf8e2a6f 430 char tx = DLPF_FS_REG;
YSB 0:7b3acf8e2a6f 431 char rx;
YSB 0:7b3acf8e2a6f 432
YSB 0:7b3acf8e2a6f 433 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 434
YSB 0:7b3acf8e2a6f 435 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
YSB 0:7b3acf8e2a6f 436
YSB 0:7b3acf8e2a6f 437 //DLPF_CFG == 0 -> sample rate = 8kHz.
YSB 0:7b3acf8e2a6f 438 if(rx == 0){
YSB 0:7b3acf8e2a6f 439 return 8;
YSB 0:7b3acf8e2a6f 440 }
YSB 0:7b3acf8e2a6f 441 //DLPF_CFG = 1..7 -> sample rate = 1kHz.
YSB 0:7b3acf8e2a6f 442 else if(rx >= 1 && rx <= 7){
YSB 0:7b3acf8e2a6f 443 return 1;
YSB 0:7b3acf8e2a6f 444 }
YSB 0:7b3acf8e2a6f 445 //DLPF_CFG = anything else -> something's wrong!
YSB 0:7b3acf8e2a6f 446 else{
YSB 0:7b3acf8e2a6f 447 return -1;
YSB 0:7b3acf8e2a6f 448 }
YSB 0:7b3acf8e2a6f 449
YSB 0:7b3acf8e2a6f 450 }
YSB 0:7b3acf8e2a6f 451
YSB 0:7b3acf8e2a6f 452 void IMU_I2C::setLpBandwidth(char bandwidth){
YSB 0:7b3acf8e2a6f 453
YSB 0:7b3acf8e2a6f 454 char tx[2];
YSB 0:7b3acf8e2a6f 455 tx[0] = DLPF_FS_REG;
YSB 0:7b3acf8e2a6f 456 //Bits 4,3 are required to be 0x03 for proper operation.
YSB 0:7b3acf8e2a6f 457 tx[1] = bandwidth | (0x03 << 3);
YSB 0:7b3acf8e2a6f 458
YSB 0:7b3acf8e2a6f 459 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
YSB 0:7b3acf8e2a6f 460
YSB 0:7b3acf8e2a6f 461 }
YSB 0:7b3acf8e2a6f 462
YSB 0:7b3acf8e2a6f 463 char IMU_I2C::getInterruptConfiguration(void){
YSB 0:7b3acf8e2a6f 464
YSB 0:7b3acf8e2a6f 465 char tx = INT_CFG_REG;
YSB 0:7b3acf8e2a6f 466 char rx;
YSB 0:7b3acf8e2a6f 467
YSB 0:7b3acf8e2a6f 468 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 469
YSB 0:7b3acf8e2a6f 470 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
YSB 0:7b3acf8e2a6f 471
YSB 0:7b3acf8e2a6f 472 return rx;
YSB 0:7b3acf8e2a6f 473
YSB 0:7b3acf8e2a6f 474 }
YSB 0:7b3acf8e2a6f 475
YSB 0:7b3acf8e2a6f 476 void IMU_I2C::setInterruptConfiguration(char config){
YSB 0:7b3acf8e2a6f 477
YSB 0:7b3acf8e2a6f 478 char tx[2];
YSB 0:7b3acf8e2a6f 479 tx[0] = INT_CFG_REG;
YSB 0:7b3acf8e2a6f 480 tx[1] = config;
YSB 0:7b3acf8e2a6f 481
YSB 0:7b3acf8e2a6f 482 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
YSB 0:7b3acf8e2a6f 483
YSB 0:7b3acf8e2a6f 484 }
YSB 0:7b3acf8e2a6f 485
YSB 0:7b3acf8e2a6f 486 bool IMU_I2C::isPllReady(void){
YSB 0:7b3acf8e2a6f 487
YSB 0:7b3acf8e2a6f 488 char tx = INT_STATUS;
YSB 0:7b3acf8e2a6f 489 char rx;
YSB 0:7b3acf8e2a6f 490
YSB 0:7b3acf8e2a6f 491 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 492
YSB 0:7b3acf8e2a6f 493 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
YSB 0:7b3acf8e2a6f 494
YSB 0:7b3acf8e2a6f 495 //ITG_RDY bit is bit 4 of INT_STATUS register.
YSB 0:7b3acf8e2a6f 496 if(rx & 0x04){
YSB 0:7b3acf8e2a6f 497 return true;
YSB 0:7b3acf8e2a6f 498 }
YSB 0:7b3acf8e2a6f 499 else{
YSB 0:7b3acf8e2a6f 500 return false;
YSB 0:7b3acf8e2a6f 501 }
YSB 0:7b3acf8e2a6f 502
YSB 0:7b3acf8e2a6f 503 }
YSB 0:7b3acf8e2a6f 504
YSB 0:7b3acf8e2a6f 505 bool IMU_I2C::isRawDataReady(void){
YSB 0:7b3acf8e2a6f 506
YSB 0:7b3acf8e2a6f 507 char tx = INT_STATUS;
YSB 0:7b3acf8e2a6f 508 char rx;
YSB 0:7b3acf8e2a6f 509
YSB 0:7b3acf8e2a6f 510 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 511
YSB 0:7b3acf8e2a6f 512 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
YSB 0:7b3acf8e2a6f 513
YSB 0:7b3acf8e2a6f 514 //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
YSB 0:7b3acf8e2a6f 515 if(rx & 0x01){
YSB 0:7b3acf8e2a6f 516 return true;
YSB 0:7b3acf8e2a6f 517 }
YSB 0:7b3acf8e2a6f 518 else{
YSB 0:7b3acf8e2a6f 519 return false;
YSB 0:7b3acf8e2a6f 520 }
YSB 0:7b3acf8e2a6f 521
YSB 0:7b3acf8e2a6f 522 }
YSB 0:7b3acf8e2a6f 523
YSB 0:7b3acf8e2a6f 524 float IMU_I2C::getTemperature(void){
YSB 0:7b3acf8e2a6f 525
YSB 0:7b3acf8e2a6f 526 char tx = TEMP_OUT_H_REG;
YSB 0:7b3acf8e2a6f 527 char rx[2];
YSB 0:7b3acf8e2a6f 528
YSB 0:7b3acf8e2a6f 529 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 530
YSB 0:7b3acf8e2a6f 531 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
YSB 0:7b3acf8e2a6f 532
YSB 0:7b3acf8e2a6f 533 int16_t temperature = ((int) rx[0] << 8) | ((int) rx[1]);
YSB 0:7b3acf8e2a6f 534 //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
YSB 0:7b3acf8e2a6f 535 return 35.0 + ((temperature + 13200)/280.0);
YSB 0:7b3acf8e2a6f 536
YSB 0:7b3acf8e2a6f 537 }
YSB 0:7b3acf8e2a6f 538
YSB 0:7b3acf8e2a6f 539 int IMU_I2C::getGyroX(void){
YSB 0:7b3acf8e2a6f 540
YSB 0:7b3acf8e2a6f 541 char tx = GYRO_XOUT_H_REG;
YSB 0:7b3acf8e2a6f 542 char rx[2];
YSB 0:7b3acf8e2a6f 543
YSB 0:7b3acf8e2a6f 544 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 545
YSB 0:7b3acf8e2a6f 546 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
YSB 0:7b3acf8e2a6f 547
YSB 0:7b3acf8e2a6f 548 int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
YSB 0:7b3acf8e2a6f 549
YSB 0:7b3acf8e2a6f 550 return output;
YSB 0:7b3acf8e2a6f 551
YSB 0:7b3acf8e2a6f 552 }
YSB 0:7b3acf8e2a6f 553
YSB 0:7b3acf8e2a6f 554 int IMU_I2C::getGyroY(void){
YSB 0:7b3acf8e2a6f 555
YSB 0:7b3acf8e2a6f 556 char tx = GYRO_YOUT_H_REG;
YSB 0:7b3acf8e2a6f 557 char rx[2];
YSB 0:7b3acf8e2a6f 558
YSB 0:7b3acf8e2a6f 559 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 560
YSB 0:7b3acf8e2a6f 561 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
YSB 0:7b3acf8e2a6f 562
YSB 0:7b3acf8e2a6f 563 int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
YSB 0:7b3acf8e2a6f 564
YSB 0:7b3acf8e2a6f 565 return output;
YSB 0:7b3acf8e2a6f 566
YSB 0:7b3acf8e2a6f 567 }
YSB 0:7b3acf8e2a6f 568
YSB 0:7b3acf8e2a6f 569 int IMU_I2C::getGyroZ(void){
YSB 0:7b3acf8e2a6f 570
YSB 0:7b3acf8e2a6f 571 char tx = GYRO_ZOUT_H_REG;
YSB 0:7b3acf8e2a6f 572 char rx[2];
YSB 0:7b3acf8e2a6f 573
YSB 0:7b3acf8e2a6f 574 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 575
YSB 0:7b3acf8e2a6f 576 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
YSB 0:7b3acf8e2a6f 577
YSB 0:7b3acf8e2a6f 578 int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
YSB 0:7b3acf8e2a6f 579
YSB 0:7b3acf8e2a6f 580 return output;
YSB 0:7b3acf8e2a6f 581
YSB 0:7b3acf8e2a6f 582 }
YSB 0:7b3acf8e2a6f 583
YSB 0:7b3acf8e2a6f 584 char IMU_I2C::getPowerManagement(void){
YSB 0:7b3acf8e2a6f 585
YSB 0:7b3acf8e2a6f 586 char tx = PWR_MGM_REG;
YSB 0:7b3acf8e2a6f 587 char rx;
YSB 0:7b3acf8e2a6f 588
YSB 0:7b3acf8e2a6f 589 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
YSB 0:7b3acf8e2a6f 590
YSB 0:7b3acf8e2a6f 591 i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
YSB 0:7b3acf8e2a6f 592
YSB 0:7b3acf8e2a6f 593 return rx;
YSB 0:7b3acf8e2a6f 594
YSB 0:7b3acf8e2a6f 595 }
YSB 0:7b3acf8e2a6f 596
YSB 0:7b3acf8e2a6f 597 void IMU_I2C::setPowerManagement(char config){
YSB 0:7b3acf8e2a6f 598
YSB 0:7b3acf8e2a6f 599 char tx[2];
YSB 0:7b3acf8e2a6f 600 tx[0] = PWR_MGM_REG;
YSB 0:7b3acf8e2a6f 601 tx[1] = config;
YSB 0:7b3acf8e2a6f 602
YSB 0:7b3acf8e2a6f 603 i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
YSB 0:7b3acf8e2a6f 604
YSB 0:7b3acf8e2a6f 605 }
YSB 0:7b3acf8e2a6f 606
YSB 0:7b3acf8e2a6f 607