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:
- 21:c2a2e7cbabdd
- Parent:
- 20:e116e596e540
- Child:
- 33:fd98776b6cc7
diff -r e116e596e540 -r c2a2e7cbabdd Sensors/Gyro/L3G4200D.cpp --- a/Sensors/Gyro/L3G4200D.cpp Mon Nov 05 09:19:01 2012 +0000 +++ b/Sensors/Gyro/L3G4200D.cpp Sat Nov 17 11:49:21 2012 +0000 @@ -27,22 +27,7 @@ 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 var 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 - readraw(); - for (int j = 0; j < 3; j++) - Gyro_calib[j] += raw[j]; - wait(0.001); // TODO: maybe less or no wait !! - } - - for (int j = 0; j < 3; j++) - offset[j] = Gyro_calib[j]/count; // take the average of the calibration measurements + calibrate(); } void L3G4200D::read() @@ -67,4 +52,21 @@ 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() +{ + // 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 + + const int count = 50; + for (int i = 0; i < count; i++) { // read 50 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 !! + } + + for (int i = 0; i < 3; i++) + offset[i] = Gyro_calib[i]/count; // take the average of the calibration measurements } \ No newline at end of file