Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

Sensors/Gyro/L3G4200D.cpp@1:5a64632b1eb9, 2012-09-28 (annotated)

Committer:
maetugr
Date:
Fri Sep 28 13:24:03 2012 +0000
Revision:
1:5a64632b1eb9
Parent:
0:0c4fafa398b4
Child:
2:93f703d2c4d7
bevor anpassung/vorschl?ge f?r ADXL345 library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:0c4fafa398b4 1 #include "mbed.h"
maetugr 0:0c4fafa398b4 2 #include "L3G4200D.h"
maetugr 0:0c4fafa398b4 3 #include <math.h>
maetugr 0:0c4fafa398b4 4
maetugr 0:0c4fafa398b4 5 #define L3G4200D_I2C_ADDRESS 0xD0
maetugr 0:0c4fafa398b4 6
maetugr 0:0c4fafa398b4 7
maetugr 1:5a64632b1eb9 8 L3G4200D::L3G4200D(PinName sda, PinName scl):
maetugr 0:0c4fafa398b4 9 i2c(sda, scl)
maetugr 0:0c4fafa398b4 10 {
maetugr 0:0c4fafa398b4 11 i2c.frequency(400000);
maetugr 0:0c4fafa398b4 12 // Turns on the L3G4200D's gyro and places it in normal mode.
maetugr 0:0c4fafa398b4 13 // 0x0F = 0b00001111
maetugr 0:0c4fafa398b4 14 // Normal power mode, all axes enabled
maetugr 0:0c4fafa398b4 15
maetugr 0:0c4fafa398b4 16 writeReg(L3G4200D_CTRL_REG2, 0x05); // Filter steuern
maetugr 0:0c4fafa398b4 17 writeReg(L3G4200D_CTRL_REG3, 0x00);
maetugr 0:0c4fafa398b4 18 writeReg(L3G4200D_CTRL_REG4, 0x20); // Genauigkeit 2000 dps
maetugr 0:0c4fafa398b4 19
maetugr 0:0c4fafa398b4 20 writeReg(L3G4200D_REFERENCE, 0x00);
maetugr 0:0c4fafa398b4 21 //writeReg(L3G4200D_STATUS_REG, 0x0F);
maetugr 0:0c4fafa398b4 22 writeReg(L3G4200D_INT1_THS_XH, 0x2C);
maetugr 0:0c4fafa398b4 23 writeReg(L3G4200D_INT1_THS_XL, 0xA4);
maetugr 0:0c4fafa398b4 24 writeReg(L3G4200D_INT1_THS_YH, 0x2C);
maetugr 0:0c4fafa398b4 25 writeReg(L3G4200D_INT1_THS_YL, 0xA4);
maetugr 0:0c4fafa398b4 26 writeReg(L3G4200D_INT1_THS_ZH, 0x2C);
maetugr 0:0c4fafa398b4 27 writeReg(L3G4200D_INT1_THS_ZL, 0xA4);
maetugr 0:0c4fafa398b4 28 //writeReg(L3G4200D_INT1_DURATION, 0x00);
maetugr 0:0c4fafa398b4 29 writeReg(L3G4200D_CTRL_REG5, 0x12); // Filter einschalten
maetugr 0:0c4fafa398b4 30
maetugr 0:0c4fafa398b4 31 writeReg(L3G4200D_CTRL_REG1, 0x0F); // Gogo
maetugr 0:0c4fafa398b4 32 }
maetugr 0:0c4fafa398b4 33
maetugr 0:0c4fafa398b4 34 // Writes a gyro register
maetugr 1:5a64632b1eb9 35 void L3G4200D::writeReg(byte reg, byte value)
maetugr 0:0c4fafa398b4 36 {
maetugr 0:0c4fafa398b4 37 data[0] = reg;
maetugr 0:0c4fafa398b4 38 data[1] = value;
maetugr 0:0c4fafa398b4 39
maetugr 0:0c4fafa398b4 40 i2c.write(L3G4200D_I2C_ADDRESS, data, 2);
maetugr 0:0c4fafa398b4 41 }
maetugr 0:0c4fafa398b4 42
maetugr 0:0c4fafa398b4 43 // Reads a gyro register
maetugr 1:5a64632b1eb9 44 byte L3G4200D::readReg(byte reg)
maetugr 0:0c4fafa398b4 45 {
maetugr 0:0c4fafa398b4 46 byte value = 0;
maetugr 0:0c4fafa398b4 47
maetugr 0:0c4fafa398b4 48 i2c.write(L3G4200D_I2C_ADDRESS, &reg, 1);
maetugr 0:0c4fafa398b4 49 i2c.read(L3G4200D_I2C_ADDRESS, &value, 1);
maetugr 0:0c4fafa398b4 50
maetugr 0:0c4fafa398b4 51 return value;
maetugr 0:0c4fafa398b4 52 }
maetugr 0:0c4fafa398b4 53
maetugr 0:0c4fafa398b4 54 // Reads the 3 gyro channels and stores them in vector g
maetugr 1:5a64632b1eb9 55 void L3G4200D::read(int g[3])
maetugr 0:0c4fafa398b4 56 {
maetugr 0:0c4fafa398b4 57 // assert the MSB of the address to get the gyro
maetugr 0:0c4fafa398b4 58 // to do slave-transmit subaddress updating.
maetugr 0:0c4fafa398b4 59 data[0] = L3G4200D_OUT_X_L | (1 << 7);
maetugr 0:0c4fafa398b4 60 i2c.write(L3G4200D_I2C_ADDRESS, data, 1);
maetugr 0:0c4fafa398b4 61
maetugr 0:0c4fafa398b4 62 // Wire.requestFrom(GYR_ADDRESS, 6);
maetugr 0:0c4fafa398b4 63 // while (Wire.available() < 6);
maetugr 0:0c4fafa398b4 64
maetugr 0:0c4fafa398b4 65 i2c.read(L3G4200D_I2C_ADDRESS, data, 6);
maetugr 0:0c4fafa398b4 66
maetugr 0:0c4fafa398b4 67 uint8_t xla = data[0];
maetugr 0:0c4fafa398b4 68 uint8_t xha = data[1];
maetugr 0:0c4fafa398b4 69 uint8_t yla = data[2];
maetugr 0:0c4fafa398b4 70 uint8_t yha = data[3];
maetugr 0:0c4fafa398b4 71 uint8_t zla = data[4];
maetugr 0:0c4fafa398b4 72 uint8_t zha = data[5];
maetugr 0:0c4fafa398b4 73
maetugr 0:0c4fafa398b4 74 g[0] = (short) (xha << 8 | xla);
maetugr 0:0c4fafa398b4 75 g[1] = (short) (yha << 8 | yla);
maetugr 0:0c4fafa398b4 76 g[2] = (short) (zha << 8 | zla);
maetugr 0:0c4fafa398b4 77 }
maetugr 0:0c4fafa398b4 78
maetugr 0:0c4fafa398b4 79 // Reads the gyros Temperature
maetugr 1:5a64632b1eb9 80 int L3G4200D::readTemp()
maetugr 0:0c4fafa398b4 81 {
maetugr 0:0c4fafa398b4 82 return (short) readReg(L3G4200D_OUT_TEMP);
maetugr 0:0c4fafa398b4 83 }