Matthias Grob / Mbed 2 deprecated FlyBed1

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.

Dependencies:   mbed MODI2C

Sensors/Gyro/L3G4200D.cpp@21:c2a2e7cbabdd, 2012-11-17 (annotated)

Committer:
maetugr
Date:
Sat Nov 17 11:49:21 2012 +0000
Revision:
21:c2a2e7cbabdd
Parent:
20:e116e596e540
Child:
33:fd98776b6cc7
Erster Flugtest, noch nicht stabil!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:0c4fafa398b4 1 #include "L3G4200D.h"
maetugr 0:0c4fafa398b4 2
maetugr 16:74a6531350b5 3 L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS)
maetugr 0:0c4fafa398b4 4 {
maetugr 0:0c4fafa398b4 5 // Turns on the L3G4200D's gyro and places it in normal mode.
maetugr 16:74a6531350b5 6 // Normal power mode, all axes enabled (for detailed info see datasheet)
maetugr 0:0c4fafa398b4 7
maetugr 16:74a6531350b5 8 //writeRegister(L3G4200D_CTRL_REG2, 0x05); // control filter
maetugr 16:74a6531350b5 9 writeRegister(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled
maetugr 16:74a6531350b5 10 writeRegister(L3G4200D_CTRL_REG3, 0x00);
maetugr 16:74a6531350b5 11 writeRegister(L3G4200D_CTRL_REG4, 0x20); // sets acuracy to 2000 dps (degree per second)
maetugr 16:74a6531350b5 12
maetugr 16:74a6531350b5 13 writeRegister(L3G4200D_REFERENCE, 0x00);
maetugr 16:74a6531350b5 14 //writeRegister(L3G4200D_STATUS_REG, 0x0F);
maetugr 0:0c4fafa398b4 15
maetugr 16:74a6531350b5 16 writeRegister(L3G4200D_INT1_THS_XH, 0x2C); // TODO: WTF??
maetugr 16:74a6531350b5 17 writeRegister(L3G4200D_INT1_THS_XL, 0xA4);
maetugr 16:74a6531350b5 18 writeRegister(L3G4200D_INT1_THS_YH, 0x2C);
maetugr 16:74a6531350b5 19 writeRegister(L3G4200D_INT1_THS_YL, 0xA4);
maetugr 16:74a6531350b5 20 writeRegister(L3G4200D_INT1_THS_ZH, 0x2C);
maetugr 16:74a6531350b5 21 writeRegister(L3G4200D_INT1_THS_ZL, 0xA4);
maetugr 16:74a6531350b5 22 //writeRegister(L3G4200D_INT1_DURATION, 0x00);
maetugr 0:0c4fafa398b4 23
maetugr 16:74a6531350b5 24 writeRegister(L3G4200D_CTRL_REG5, 0x00); // deactivates the filters (only use one of these options)
maetugr 16:74a6531350b5 25 //writeRegister(L3G4200D_CTRL_REG5, 0x12); // activates both high and low pass filters
maetugr 16:74a6531350b5 26 //writeRegister(L3G4200D_CTRL_REG5, 0x01); // activates high pass filter
maetugr 2:93f703d2c4d7 27
maetugr 16:74a6531350b5 28 writeRegister(L3G4200D_CTRL_REG1, 0x0F); // starts Gyro measurement
maetugr 16:74a6531350b5 29
maetugr 21:c2a2e7cbabdd 30 calibrate();
maetugr 0:0c4fafa398b4 31 }
maetugr 0:0c4fafa398b4 32
maetugr 10:953afcbcebfc 33 void L3G4200D::read()
maetugr 0:0c4fafa398b4 34 {
maetugr 20:e116e596e540 35 readraw(); // read raw measurement data
maetugr 0:0c4fafa398b4 36
maetugr 20:e116e596e540 37 for (int i = 0; i < 3; i++)
maetugr 20:e116e596e540 38 data[i] = raw[i] - offset[i]; // subtract offset from calibration
maetugr 0:0c4fafa398b4 39 }
maetugr 0:0c4fafa398b4 40
maetugr 1:5a64632b1eb9 41 int L3G4200D::readTemp()
maetugr 0:0c4fafa398b4 42 {
maetugr 16:74a6531350b5 43 return (short) readRegister(L3G4200D_OUT_TEMP); // read the sensors register for the temperature
maetugr 20:e116e596e540 44 }
maetugr 20:e116e596e540 45
maetugr 20:e116e596e540 46 void L3G4200D::readraw()
maetugr 20:e116e596e540 47 {
maetugr 20:e116e596e540 48 char buffer[6]; // 8-Bit pieces of axis data
maetugr 20:e116e596e540 49
maetugr 20:e116e596e540 50 readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: wiiiiiiso?! | (1 << 7)
maetugr 20:e116e596e540 51
maetugr 20:e116e596e540 52 raw[0] = (short) (buffer[1] << 8 | buffer[0]); // join 8-Bit pieces to 16-bit short integers
maetugr 20:e116e596e540 53 raw[1] = (short) (buffer[3] << 8 | buffer[2]);
maetugr 20:e116e596e540 54 raw[2] = (short) (buffer[5] << 8 | buffer[4]);
maetugr 21:c2a2e7cbabdd 55 }
maetugr 21:c2a2e7cbabdd 56
maetugr 21:c2a2e7cbabdd 57 void L3G4200D::calibrate()
maetugr 21:c2a2e7cbabdd 58 {
maetugr 21:c2a2e7cbabdd 59 // calibrate gyro with an average of count samples (result of calibration stored in offset[])
maetugr 21:c2a2e7cbabdd 60 float Gyro_calib[3] = {0,0,0}; // temporary var for the sum of calibration measurement
maetugr 21:c2a2e7cbabdd 61
maetugr 21:c2a2e7cbabdd 62 const int count = 50;
maetugr 21:c2a2e7cbabdd 63 for (int i = 0; i < count; i++) { // read 50 times the data in a very short time
maetugr 21:c2a2e7cbabdd 64 readraw();
maetugr 21:c2a2e7cbabdd 65 for (int j = 0; j < 3; j++)
maetugr 21:c2a2e7cbabdd 66 Gyro_calib[j] += raw[j];
maetugr 21:c2a2e7cbabdd 67 wait(0.001); // TODO: maybe less or no wait !!
maetugr 21:c2a2e7cbabdd 68 }
maetugr 21:c2a2e7cbabdd 69
maetugr 21:c2a2e7cbabdd 70 for (int i = 0; i < 3; i++)
maetugr 21:c2a2e7cbabdd 71 offset[i] = Gyro_calib[i]/count; // take the average of the calibration measurements
maetugr 0:0c4fafa398b4 72 }