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.
Sensors/Gyro/L3G4200D.cpp@0:0c4fafa398b4, 2012-09-26 (annotated)
- Committer:
- maetugr
- Date:
- Wed Sep 26 12:15:00 2012 +0000
- Revision:
- 0:0c4fafa398b4
- Child:
- 1:5a64632b1eb9
Sensor Test; Acc, Gyro working, result on LCD (servo lib included)
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:0c4fafa398b4 | 8 | Gyro::Gyro(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 | 0:0c4fafa398b4 | 35 | void Gyro::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 | 0:0c4fafa398b4 | 44 | byte Gyro::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, ®, 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 | 0:0c4fafa398b4 | 55 | void Gyro::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 | 0:0c4fafa398b4 | 80 | int Gyro::readTemp() |
maetugr | 0:0c4fafa398b4 | 81 | { |
maetugr | 0:0c4fafa398b4 | 82 | return (short) readReg(L3G4200D_OUT_TEMP); |
maetugr | 0:0c4fafa398b4 | 83 | } |