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:
- 16:74a6531350b5
- Parent:
- 15:753c5d6a63b3
- Child:
- 17:e096e85f6c36
diff -r 753c5d6a63b3 -r 74a6531350b5 Sensors/Gyro/L3G4200D.cpp --- a/Sensors/Gyro/L3G4200D.cpp Mon Oct 29 16:43:10 2012 +0000 +++ b/Sensors/Gyro/L3G4200D.cpp Wed Oct 31 14:44:07 2012 +0000 @@ -2,96 +2,69 @@ #include "L3G4200D.h" #include <math.h> -#define L3G4200D_I2C_ADDRESS 0xD0 - +#define L3G4200D_I2C_ADDRESS 0xD0 // TODO: Adressen??? -L3G4200D::L3G4200D(PinName sda, PinName scl) : i2c(sda, scl) +L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS) { - i2c.frequency(400000); // Turns on the L3G4200D's gyro and places it in normal mode. - // Normal power mode, all axes enabled + // Normal power mode, all axes enabled (for detailed info see datasheet) - //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); // acuracy 2000 dps + //writeRegister(L3G4200D_CTRL_REG2, 0x05); // control filter + writeRegister(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled + writeRegister(L3G4200D_CTRL_REG3, 0x00); + writeRegister(L3G4200D_CTRL_REG4, 0x20); // sets acuracy to 2000 dps (degree per second) + + writeRegister(L3G4200D_REFERENCE, 0x00); + //writeRegister(L3G4200D_STATUS_REG, 0x0F); - writeReg(L3G4200D_REFERENCE, 0x00); - //writeReg(L3G4200D_STATUS_REG, 0x0F); - writeReg(L3G4200D_INT1_THS_XH, 0x2C); - writeReg(L3G4200D_INT1_THS_XL, 0xA4); - writeReg(L3G4200D_INT1_THS_YH, 0x2C); - writeReg(L3G4200D_INT1_THS_YL, 0xA4); - 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, 0x01); // hochpass Filter einschalten - writeReg(L3G4200D_CTRL_REG5, 0x00); // Filter ausschalten + writeRegister(L3G4200D_INT1_THS_XH, 0x2C); // TODO: WTF?? + writeRegister(L3G4200D_INT1_THS_XL, 0xA4); + writeRegister(L3G4200D_INT1_THS_YH, 0x2C); + writeRegister(L3G4200D_INT1_THS_YL, 0xA4); + writeRegister(L3G4200D_INT1_THS_ZH, 0x2C); + writeRegister(L3G4200D_INT1_THS_ZL, 0xA4); + //writeRegister(L3G4200D_INT1_DURATION, 0x00); - writeReg(L3G4200D_CTRL_REG1, 0x0F); // Gogo + writeRegister(L3G4200D_CTRL_REG5, 0x00); // deactivates the filters (only use one of these options) + //writeRegister(L3G4200D_CTRL_REG5, 0x12); // activates both high and low pass filters + //writeRegister(L3G4200D_CTRL_REG5, 0x01); // activates high pass filter - // calibrate gyro with an average of count samples (result to offset) - #define count 50 + writeRegister(L3G4200D_CTRL_REG1, 0x0F); // starts Gyro measurement + + // calibrate gyro with an average of count samples (result of calibration stored in offset[]) for (int j = 0; j < 3; j++) offset[j] = 0; - float Gyro_calib[3] = {0,0,0}; // temporary to sum up + float Gyro_calib[3] = {0,0,0}; // temporary var for the sum of calibration measurement - for (int i = 0; i < count; i++) { + for (int i = 0; i < 50; i++) { // read 50 times the data in a very short time read(); for (int j = 0; j < 3; j++) Gyro_calib[j] += data[j]; - wait(0.001); // maybe less or no wait !! + wait(0.001); // TODO: 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) -{ - byte buffer[2]; - buffer[0] = reg; - buffer[1] = value; - - i2c.write(L3G4200D_I2C_ADDRESS, buffer, 2); + offset[j] = Gyro_calib[j]/count; // take the average of the calibration measurements } -// Reads a gyro register -byte L3G4200D::readReg(byte reg) -{ - byte value = 0; - - i2c.write(L3G4200D_I2C_ADDRESS, ®, 1); - i2c.read(L3G4200D_I2C_ADDRESS, &value, 1); - - return value; -} - -// Reads the 3 gyro channels and stores them in vector g void L3G4200D::read() { - 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. + char buffer[6]; // 8-Bit pieces of axis data + buffer[0] = L3G4200D_OUT_X_L | (1 << 7); - i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1, true); - i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6, true); + readMultiRegister(L3G4200D_OUT_X_L, buffer, 6); // parameter: - data[0] = (short) (buffer[1] << 8 | buffer[0]); + data[0] = (short) (buffer[1] << 8 | buffer[0]); // compose 8-Bit pieces to 16-bit short integers data[1] = (short) (buffer[3] << 8 | buffer[2]); data[2] = (short) (buffer[5] << 8 | buffer[4]); - //with offset of calibration for (int j = 0; j < 3; j++) - data[j] -= offset[j]; + data[j] -= offset[j]; // add offset from calibration } -// Reads the gyros Temperature int L3G4200D::readTemp() { - return (short) readReg(L3G4200D_OUT_TEMP); + return (short) readRegister(L3G4200D_OUT_TEMP); // read the sensors register for the temperature } \ No newline at end of file