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.
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?
User | Revision | Line number | New 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 | } |