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:
- 33:fd98776b6cc7
- Parent:
- 21:c2a2e7cbabdd
diff -r e2e02338805e -r fd98776b6cc7 Sensors/Gyro/L3G4200D.cpp --- a/Sensors/Gyro/L3G4200D.cpp Tue Apr 02 16:57:56 2013 +0000 +++ b/Sensors/Gyro/L3G4200D.cpp Thu Apr 04 14:25:21 2013 +0000 @@ -27,7 +27,7 @@ writeRegister(L3G4200D_CTRL_REG1, 0x0F); // starts Gyro measurement - calibrate(); + //calibrate(50, 0.01); } void L3G4200D::read() @@ -35,7 +35,7 @@ readraw(); // read raw measurement data for (int i = 0; i < 3; i++) - data[i] = raw[i] - offset[i]; // subtract offset from calibration + data[i] = (raw[i] - offset[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10) } int L3G4200D::readTemp() @@ -47,26 +47,25 @@ { char buffer[6]; // 8-Bit pieces of axis data - readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: wiiiiiiso?! | (1 << 7) + readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7) raw[0] = (short) (buffer[1] << 8 | buffer[0]); // join 8-Bit pieces to 16-bit short integers raw[1] = (short) (buffer[3] << 8 | buffer[2]); raw[2] = (short) (buffer[5] << 8 | buffer[4]); } -void L3G4200D::calibrate() +void L3G4200D::calibrate(int times, float separation_time) { - // calibrate gyro with an average of count samples (result of calibration stored in offset[]) - float Gyro_calib[3] = {0,0,0}; // temporary var for the sum of calibration measurement + // calibrate sensor with an average of count samples (result of calibration stored in offset[]) + float calib[3] = {0,0,0}; // temporary array for the sum of calibration measurement - const int count = 50; - for (int i = 0; i < count; i++) { // read 50 times the data in a very short time + for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time readraw(); for (int j = 0; j < 3; j++) - Gyro_calib[j] += raw[j]; - wait(0.001); // TODO: maybe less or no wait !! + calib[j] += raw[j]; + wait(separation_time); } for (int i = 0; i < 3; i++) - offset[i] = Gyro_calib[i]/count; // take the average of the calibration measurements + offset[i] = calib[i]/times; // take the average of the calibration measurements } \ No newline at end of file