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@33:fd98776b6cc7, 2013-04-04 (annotated)

Committer:
maetugr
Date:
Thu Apr 04 14:25:21 2013 +0000
Revision:
33:fd98776b6cc7
Parent:
21:c2a2e7cbabdd 
New version developed in eastern holidays, ported Madgwick Filter, added support for chaning PID values while flying over bluetooth, still not flying stable or even controllable

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 33:fd98776b6cc7 30 //calibrate(50, 0.01);
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 33:fd98776b6cc7 38 data[i] = (raw[i] - offset[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10)
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 33:fd98776b6cc7 50 readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (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 33:fd98776b6cc7 57 void L3G4200D::calibrate(int times, float separation_time)
maetugr 21:c2a2e7cbabdd 58 {
maetugr 33:fd98776b6cc7 59 // calibrate sensor with an average of count samples (result of calibration stored in offset[])
maetugr 33:fd98776b6cc7 60 float calib[3] = {0,0,0}; // temporary array for the sum of calibration measurement
maetugr 21:c2a2e7cbabdd 61
maetugr 33:fd98776b6cc7 62 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
maetugr 21:c2a2e7cbabdd 63 readraw();
maetugr 21:c2a2e7cbabdd 64 for (int j = 0; j < 3; j++)
maetugr 33:fd98776b6cc7 65 calib[j] += raw[j];
maetugr 33:fd98776b6cc7 66 wait(separation_time);
maetugr 21:c2a2e7cbabdd 67 }
maetugr 21:c2a2e7cbabdd 68
maetugr 21:c2a2e7cbabdd 69 for (int i = 0; i < 3; i++)
maetugr 33:fd98776b6cc7 70 offset[i] = calib[i]/times; // take the average of the calibration measurements
maetugr 0:0c4fafa398b4 71 }