Programmation / ITG3200

Dependents:   Lab6_Gyro

ITG3200.cpp@0:012badc736c0, 2019-12-17 (annotated)

Committer:
brunostgr
Date:
Tue Dec 17 17:49:17 2019 +0000
Revision:
0:012badc736c0
Asdf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brunostgr 0:012badc736c0 1
brunostgr 0:012badc736c0 2 /**
brunostgr 0:012badc736c0 3 * Includes
brunostgr 0:012badc736c0 4 */
brunostgr 0:012badc736c0 5 #include "ITG3200.h"
brunostgr 0:012badc736c0 6
brunostgr 0:012badc736c0 7 ITG3200::ITG3200(PinName sda, PinName scl, int i2cAddressW, int i2cAddressR) {
brunostgr 0:012badc736c0 8
brunostgr 0:012badc736c0 9 i2c = new I2C(sda, scl);
brunostgr 0:012badc736c0 10 // Gyro designed to work at 400KHz. See datasheet for details.
brunostgr 0:012badc736c0 11 i2c->frequency(100000);
brunostgr 0:012badc736c0 12 i2cAddW = i2cAddressW;
brunostgr 0:012badc736c0 13 i2cAddR = i2cAddressR;
brunostgr 0:012badc736c0 14
brunostgr 0:012badc736c0 15 }
brunostgr 0:012badc736c0 16
brunostgr 0:012badc736c0 17 void ITG3200::Init(void){
brunostgr 0:012badc736c0 18 wait_ms(50); // Temps necessaire pour le demarrage du gyro
brunostgr 0:012badc736c0 19 char registerNumber[2] = {0x16,0x18};
brunostgr 0:012badc736c0 20 char test = 0x3E;
brunostgr 0:012badc736c0 21 char registerContents[1] = {};
brunostgr 0:012badc736c0 22 i2c->write(i2cAddW, registerNumber, 2);
brunostgr 0:012badc736c0 23
brunostgr 0:012badc736c0 24 i2c->write(i2cAddW, &test, 1);
brunostgr 0:012badc736c0 25 i2c->read(i2cAddR, registerContents, 1);
brunostgr 0:012badc736c0 26 registerContents[0] = registerContents[0] | 0x02;
brunostgr 0:012badc736c0 27 i2c->write(i2cAddW, &registerContents[0], 1);
brunostgr 0:012badc736c0 28 }
brunostgr 0:012badc736c0 29
brunostgr 0:012badc736c0 30 int ITG3200::GetX(void){
brunostgr 0:012badc736c0 31
brunostgr 0:012badc736c0 32 char registerNumber = GYRO_XOUT_H;
brunostgr 0:012badc736c0 33 char registerContents[2] = {0x00, 0x00};
brunostgr 0:012badc736c0 34
brunostgr 0:012badc736c0 35 //First, send the number of register we wish to read,
brunostgr 0:012badc736c0 36 //in this case, register numbers 2, 3, which hold the
brunostgr 0:012badc736c0 37 //compass bearing as a 16-bit word.
brunostgr 0:012badc736c0 38 i2c->write(i2cAddW, &registerNumber, 1);
brunostgr 0:012badc736c0 39
brunostgr 0:012badc736c0 40 //Now read two bytes which will be the contents of
brunostgr 0:012badc736c0 41 //these registers.
brunostgr 0:012badc736c0 42 i2c->read(i2cAddR, registerContents, 2);
brunostgr 0:012badc736c0 43
brunostgr 0:012badc736c0 44 //Register 2 [read first], was the high byte, followed by
brunostgr 0:012badc736c0 45 //register 3 [read second], which was the low byte.
brunostgr 0:012badc736c0 46
brunostgr 0:012badc736c0 47 return (int16_t)((registerContents[0] << 8) | registerContents[1]);
brunostgr 0:012badc736c0 48
brunostgr 0:012badc736c0 49 }
brunostgr 0:012badc736c0 50
brunostgr 0:012badc736c0 51 int ITG3200::GetY(void){
brunostgr 0:012badc736c0 52
brunostgr 0:012badc736c0 53 char registerNumber = GYRO_YOUT_H;
brunostgr 0:012badc736c0 54 char registerContents[2] = {0x00, 0x00};
brunostgr 0:012badc736c0 55
brunostgr 0:012badc736c0 56 //First, send the number of register we wish to read,
brunostgr 0:012badc736c0 57 //in this case, register numbers 2, 3, which hold the
brunostgr 0:012badc736c0 58 //compass bearing as a 16-bit word.
brunostgr 0:012badc736c0 59 i2c->write(i2cAddW, &registerNumber, 1);
brunostgr 0:012badc736c0 60
brunostgr 0:012badc736c0 61 //Now read two bytes which will be the contents of
brunostgr 0:012badc736c0 62 //these registers.
brunostgr 0:012badc736c0 63 i2c->read(i2cAddR, registerContents, 2);
brunostgr 0:012badc736c0 64
brunostgr 0:012badc736c0 65 //Register 2 [read first], was the high byte, followed by
brunostgr 0:012badc736c0 66 //register 3 [read second], which was the low byte.
brunostgr 0:012badc736c0 67
brunostgr 0:012badc736c0 68 return (int16_t)((registerContents[0] << 8) | registerContents[1]);
brunostgr 0:012badc736c0 69
brunostgr 0:012badc736c0 70 }
brunostgr 0:012badc736c0 71
brunostgr 0:012badc736c0 72 int ITG3200::GetZ(void){
brunostgr 0:012badc736c0 73
brunostgr 0:012badc736c0 74 char registerNumber = GYRO_ZOUT_H;
brunostgr 0:012badc736c0 75 char registerContents[2] = {0x00, 0x00};
brunostgr 0:012badc736c0 76
brunostgr 0:012badc736c0 77 //First, send the number of register we wish to read,
brunostgr 0:012badc736c0 78 //in this case, register numbers 2, 3, which hold the
brunostgr 0:012badc736c0 79 //compass bearing as a 16-bit word.
brunostgr 0:012badc736c0 80 i2c->write(i2cAddW, &registerNumber, 1);
brunostgr 0:012badc736c0 81
brunostgr 0:012badc736c0 82 //Now read two bytes which will be the contents of
brunostgr 0:012badc736c0 83 //these registers.
brunostgr 0:012badc736c0 84 i2c->read(i2cAddR, registerContents, 2);
brunostgr 0:012badc736c0 85
brunostgr 0:012badc736c0 86 //Register 2 [read first], was the high byte, followed by
brunostgr 0:012badc736c0 87 //register 3 [read second], which was the low byte.
brunostgr 0:012badc736c0 88
brunostgr 0:012badc736c0 89 return (int16_t)((registerContents[0] << 8) | registerContents[1]);
brunostgr 0:012badc736c0 90
brunostgr 0:012badc736c0 91 }
brunostgr 0:012badc736c0 92
brunostgr 0:012badc736c0 93 int ITG3200::GetTemp(void){
brunostgr 0:012badc736c0 94
brunostgr 0:012badc736c0 95 char registerNumber = GYRO_TEMP_H;
brunostgr 0:012badc736c0 96 char registerContents[2] = {0x00, 0x00};
brunostgr 0:012badc736c0 97 uint16_t temp;
brunostgr 0:012badc736c0 98 //First, send the number of register we wish to read,
brunostgr 0:012badc736c0 99 //in this case, register numbers 2, 3, which hold the
brunostgr 0:012badc736c0 100 //compass bearing as a 16-bit word.
brunostgr 0:012badc736c0 101 i2c->write(i2cAddW, &registerNumber, 1);
brunostgr 0:012badc736c0 102
brunostgr 0:012badc736c0 103 //Now read two bytes which will be the contents of
brunostgr 0:012badc736c0 104 //these registers.
brunostgr 0:012badc736c0 105 i2c->read(i2cAddR, registerContents, 2);
brunostgr 0:012badc736c0 106
brunostgr 0:012badc736c0 107 //Register 2 [read first], was the high byte, followed by
brunostgr 0:012badc736c0 108 //register 3 [read second], which was the low byte.
brunostgr 0:012badc736c0 109
brunostgr 0:012badc736c0 110 temp = (int16_t)((registerContents[0] << 8) | registerContents[1]);
brunostgr 0:012badc736c0 111 temp = (temp / 280) - 150; // 280LSB / °C - offset du capteur
brunostgr 0:012badc736c0 112 return temp;
brunostgr 0:012badc736c0 113
brunostgr 0:012badc736c0 114 }