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@10:953afcbcebfc, 2012-10-17 (annotated)
- Committer:
- maetugr
- Date:
- Wed Oct 17 08:37:08 2012 +0000
- Revision:
- 10:953afcbcebfc
- Parent:
- 2:93f703d2c4d7
- Child:
- 11:9bf69bc6df45
neue Acc_Winkelberechnung, vor Kompassklassenrevision
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 | 2:93f703d2c4d7 | 8 | L3G4200D::L3G4200D(PinName sda, PinName scl) : i2c(sda, scl) |
maetugr | 0:0c4fafa398b4 | 9 | { |
maetugr | 0:0c4fafa398b4 | 10 | i2c.frequency(400000); |
maetugr | 0:0c4fafa398b4 | 11 | // Turns on the L3G4200D's gyro and places it in normal mode. |
maetugr | 0:0c4fafa398b4 | 12 | // 0x0F = 0b00001111 |
maetugr | 0:0c4fafa398b4 | 13 | // Normal power mode, all axes enabled |
maetugr | 0:0c4fafa398b4 | 14 | |
maetugr | 2:93f703d2c4d7 | 15 | //writeReg(L3G4200D_CTRL_REG2, 0x05); // control filter |
maetugr | 2:93f703d2c4d7 | 16 | writeReg(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled |
maetugr | 0:0c4fafa398b4 | 17 | writeReg(L3G4200D_CTRL_REG3, 0x00); |
maetugr | 2:93f703d2c4d7 | 18 | writeReg(L3G4200D_CTRL_REG4, 0x20); // acuracy 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 | 2:93f703d2c4d7 | 29 | //writeReg(L3G4200D_CTRL_REG5, 0x12); // Filter einschalten |
maetugr | 2:93f703d2c4d7 | 30 | //writeReg(L3G4200D_CTRL_REG5, 0x01); // hochpass Filter einschalten |
maetugr | 2:93f703d2c4d7 | 31 | writeReg(L3G4200D_CTRL_REG5, 0x00); // Filter ausschalten |
maetugr | 0:0c4fafa398b4 | 32 | |
maetugr | 0:0c4fafa398b4 | 33 | writeReg(L3G4200D_CTRL_REG1, 0x0F); // Gogo |
maetugr | 2:93f703d2c4d7 | 34 | |
maetugr | 2:93f703d2c4d7 | 35 | // calibrate gyro with an average of count samples (result to offset) |
maetugr | 2:93f703d2c4d7 | 36 | #define count 50 |
maetugr | 2:93f703d2c4d7 | 37 | for (int j = 0; j < 3; j++) |
maetugr | 2:93f703d2c4d7 | 38 | offset[j] = 0; |
maetugr | 2:93f703d2c4d7 | 39 | |
maetugr | 2:93f703d2c4d7 | 40 | float Gyro_calib[3] = {0,0,0}; // temporary to sum up |
maetugr | 2:93f703d2c4d7 | 41 | |
maetugr | 2:93f703d2c4d7 | 42 | for (int i = 0; i < count; i++) { |
maetugr | 10:953afcbcebfc | 43 | read(); |
maetugr | 2:93f703d2c4d7 | 44 | for (int j = 0; j < 3; j++) |
maetugr | 10:953afcbcebfc | 45 | Gyro_calib[j] += data[j]; |
maetugr | 2:93f703d2c4d7 | 46 | wait(0.001); // maybe less or no wait !! |
maetugr | 2:93f703d2c4d7 | 47 | } |
maetugr | 2:93f703d2c4d7 | 48 | |
maetugr | 2:93f703d2c4d7 | 49 | for (int j = 0; j < 3; j++) |
maetugr | 2:93f703d2c4d7 | 50 | offset[j] = Gyro_calib[j]/count; |
maetugr | 0:0c4fafa398b4 | 51 | } |
maetugr | 0:0c4fafa398b4 | 52 | |
maetugr | 0:0c4fafa398b4 | 53 | // Writes a gyro register |
maetugr | 1:5a64632b1eb9 | 54 | void L3G4200D::writeReg(byte reg, byte value) |
maetugr | 0:0c4fafa398b4 | 55 | { |
maetugr | 2:93f703d2c4d7 | 56 | byte buffer[2]; |
maetugr | 2:93f703d2c4d7 | 57 | buffer[0] = reg; |
maetugr | 2:93f703d2c4d7 | 58 | buffer[1] = value; |
maetugr | 0:0c4fafa398b4 | 59 | |
maetugr | 2:93f703d2c4d7 | 60 | i2c.write(L3G4200D_I2C_ADDRESS, buffer, 2); |
maetugr | 0:0c4fafa398b4 | 61 | } |
maetugr | 0:0c4fafa398b4 | 62 | |
maetugr | 0:0c4fafa398b4 | 63 | // Reads a gyro register |
maetugr | 1:5a64632b1eb9 | 64 | byte L3G4200D::readReg(byte reg) |
maetugr | 0:0c4fafa398b4 | 65 | { |
maetugr | 0:0c4fafa398b4 | 66 | byte value = 0; |
maetugr | 0:0c4fafa398b4 | 67 | |
maetugr | 0:0c4fafa398b4 | 68 | i2c.write(L3G4200D_I2C_ADDRESS, ®, 1); |
maetugr | 0:0c4fafa398b4 | 69 | i2c.read(L3G4200D_I2C_ADDRESS, &value, 1); |
maetugr | 0:0c4fafa398b4 | 70 | |
maetugr | 0:0c4fafa398b4 | 71 | return value; |
maetugr | 0:0c4fafa398b4 | 72 | } |
maetugr | 0:0c4fafa398b4 | 73 | |
maetugr | 0:0c4fafa398b4 | 74 | // Reads the 3 gyro channels and stores them in vector g |
maetugr | 10:953afcbcebfc | 75 | void L3G4200D::read() |
maetugr | 0:0c4fafa398b4 | 76 | { |
maetugr | 2:93f703d2c4d7 | 77 | byte buffer[6]; // 8-Bit pieces of axis data |
maetugr | 0:0c4fafa398b4 | 78 | // assert the MSB of the address to get the gyro |
maetugr | 0:0c4fafa398b4 | 79 | // to do slave-transmit subaddress updating. |
maetugr | 2:93f703d2c4d7 | 80 | buffer[0] = L3G4200D_OUT_X_L | (1 << 7); |
maetugr | 2:93f703d2c4d7 | 81 | i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1); |
maetugr | 0:0c4fafa398b4 | 82 | |
maetugr | 0:0c4fafa398b4 | 83 | // Wire.requestFrom(GYR_ADDRESS, 6); |
maetugr | 0:0c4fafa398b4 | 84 | // while (Wire.available() < 6); |
maetugr | 0:0c4fafa398b4 | 85 | |
maetugr | 2:93f703d2c4d7 | 86 | i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6); |
maetugr | 0:0c4fafa398b4 | 87 | |
maetugr | 2:93f703d2c4d7 | 88 | uint8_t xla = buffer[0]; |
maetugr | 2:93f703d2c4d7 | 89 | uint8_t xha = buffer[1]; |
maetugr | 2:93f703d2c4d7 | 90 | uint8_t yla = buffer[2]; |
maetugr | 2:93f703d2c4d7 | 91 | uint8_t yha = buffer[3]; |
maetugr | 2:93f703d2c4d7 | 92 | uint8_t zla = buffer[4]; |
maetugr | 2:93f703d2c4d7 | 93 | uint8_t zha = buffer[5]; |
maetugr | 0:0c4fafa398b4 | 94 | |
maetugr | 10:953afcbcebfc | 95 | data[0] = (short) (xha << 8 | xla); |
maetugr | 10:953afcbcebfc | 96 | data[1] = (short) (yha << 8 | yla); |
maetugr | 10:953afcbcebfc | 97 | data[2] = (short) (zha << 8 | zla); |
maetugr | 2:93f703d2c4d7 | 98 | |
maetugr | 2:93f703d2c4d7 | 99 | //with offset of calibration |
maetugr | 2:93f703d2c4d7 | 100 | for (int j = 0; j < 3; j++) |
maetugr | 10:953afcbcebfc | 101 | data[j] -= offset[j]; |
maetugr | 0:0c4fafa398b4 | 102 | } |
maetugr | 0:0c4fafa398b4 | 103 | |
maetugr | 0:0c4fafa398b4 | 104 | // Reads the gyros Temperature |
maetugr | 1:5a64632b1eb9 | 105 | int L3G4200D::readTemp() |
maetugr | 0:0c4fafa398b4 | 106 | { |
maetugr | 0:0c4fafa398b4 | 107 | return (short) readReg(L3G4200D_OUT_TEMP); |
maetugr | 0:0c4fafa398b4 | 108 | } |