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.
Diff: Sensors/Gyro/L3G4200D.cpp
- Revision:
- 2:93f703d2c4d7
- Parent:
- 1:5a64632b1eb9
- Child:
- 10:953afcbcebfc
--- a/Sensors/Gyro/L3G4200D.cpp Fri Sep 28 13:24:03 2012 +0000 +++ b/Sensors/Gyro/L3G4200D.cpp Tue Oct 02 17:53:40 2012 +0000 @@ -5,17 +5,17 @@ #define L3G4200D_I2C_ADDRESS 0xD0 -L3G4200D::L3G4200D(PinName sda, PinName scl): - i2c(sda, scl) +L3G4200D::L3G4200D(PinName sda, PinName scl) : i2c(sda, scl) { i2c.frequency(400000); // Turns on the L3G4200D's gyro and places it in normal mode. // 0x0F = 0b00001111 // Normal power mode, all axes enabled - writeReg(L3G4200D_CTRL_REG2, 0x05); // Filter steuern + //writeReg(L3G4200D_CTRL_REG2, 0x05); // control filter + writeReg(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled writeReg(L3G4200D_CTRL_REG3, 0x00); - writeReg(L3G4200D_CTRL_REG4, 0x20); // Genauigkeit 2000 dps + writeReg(L3G4200D_CTRL_REG4, 0x20); // acuracy 2000 dps writeReg(L3G4200D_REFERENCE, 0x00); //writeReg(L3G4200D_STATUS_REG, 0x0F); @@ -26,18 +26,39 @@ writeReg(L3G4200D_INT1_THS_ZH, 0x2C); writeReg(L3G4200D_INT1_THS_ZL, 0xA4); //writeReg(L3G4200D_INT1_DURATION, 0x00); - writeReg(L3G4200D_CTRL_REG5, 0x12); // Filter einschalten + //writeReg(L3G4200D_CTRL_REG5, 0x12); // Filter einschalten + //writeReg(L3G4200D_CTRL_REG5, 0x01); // hochpass Filter einschalten + writeReg(L3G4200D_CTRL_REG5, 0x00); // Filter ausschalten writeReg(L3G4200D_CTRL_REG1, 0x0F); // Gogo + + // calibrate gyro with an average of count samples (result to offset) + #define count 50 + for (int j = 0; j < 3; j++) + offset[j] = 0; + + float Gyro_calib[3] = {0,0,0}; // temporary to sum up + float Gyro_data[3]; + + for (int i = 0; i < count; i++) { + read(Gyro_data); + for (int j = 0; j < 3; j++) + Gyro_calib[j] += Gyro_data[j]; + wait(0.001); // maybe less or no wait !! + } + + for (int j = 0; j < 3; j++) + offset[j] = Gyro_calib[j]/count; } // Writes a gyro register void L3G4200D::writeReg(byte reg, byte value) { - data[0] = reg; - data[1] = value; + byte buffer[2]; + buffer[0] = reg; + buffer[1] = value; - i2c.write(L3G4200D_I2C_ADDRESS, data, 2); + i2c.write(L3G4200D_I2C_ADDRESS, buffer, 2); } // Reads a gyro register @@ -52,28 +73,33 @@ } // Reads the 3 gyro channels and stores them in vector g -void L3G4200D::read(int g[3]) +void L3G4200D::read(float g[3]) { + byte buffer[6]; // 8-Bit pieces of axis data // assert the MSB of the address to get the gyro // to do slave-transmit subaddress updating. - data[0] = L3G4200D_OUT_X_L | (1 << 7); - i2c.write(L3G4200D_I2C_ADDRESS, data, 1); + buffer[0] = L3G4200D_OUT_X_L | (1 << 7); + i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1); // Wire.requestFrom(GYR_ADDRESS, 6); // while (Wire.available() < 6); - i2c.read(L3G4200D_I2C_ADDRESS, data, 6); + i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6); - uint8_t xla = data[0]; - uint8_t xha = data[1]; - uint8_t yla = data[2]; - uint8_t yha = data[3]; - uint8_t zla = data[4]; - uint8_t zha = data[5]; + uint8_t xla = buffer[0]; + uint8_t xha = buffer[1]; + uint8_t yla = buffer[2]; + uint8_t yha = buffer[3]; + uint8_t zla = buffer[4]; + uint8_t zha = buffer[5]; g[0] = (short) (xha << 8 | xla); g[1] = (short) (yha << 8 | yla); g[2] = (short) (zha << 8 | zla); + + //with offset of calibration + for (int j = 0; j < 3; j++) + g[j] -= offset[j]; } // Reads the gyros Temperature