An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085
IMU/Sensors/Gyro/L3G4200D.cpp@0:3e7450f1a938, 2013-08-27 (annotated)
- Committer:
- maetugr
- Date:
- Tue Aug 27 17:37:06 2013 +0000
- Revision:
- 0:3e7450f1a938
- Child:
- 4:f62337b907e5
before 10DOF class (only files already created)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:3e7450f1a938 | 1 | #include "L3G4200D.h" |
maetugr | 0:3e7450f1a938 | 2 | |
maetugr | 0:3e7450f1a938 | 3 | L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS) |
maetugr | 0:3e7450f1a938 | 4 | { |
maetugr | 0:3e7450f1a938 | 5 | // Turns on the L3G4200D's gyro and places it in normal mode. |
maetugr | 0:3e7450f1a938 | 6 | // Normal power mode, all axes enabled (for detailed info see datasheet) |
maetugr | 0:3e7450f1a938 | 7 | |
maetugr | 0:3e7450f1a938 | 8 | //writeRegister(L3G4200D_CTRL_REG2, 0x05); // control filter |
maetugr | 0:3e7450f1a938 | 9 | writeRegister(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled |
maetugr | 0:3e7450f1a938 | 10 | writeRegister(L3G4200D_CTRL_REG3, 0x00); |
maetugr | 0:3e7450f1a938 | 11 | writeRegister(L3G4200D_CTRL_REG4, 0x20); // sets acuracy to 2000 dps (degree per second) |
maetugr | 0:3e7450f1a938 | 12 | |
maetugr | 0:3e7450f1a938 | 13 | writeRegister(L3G4200D_REFERENCE, 0x00); |
maetugr | 0:3e7450f1a938 | 14 | //writeRegister(L3G4200D_STATUS_REG, 0x0F); |
maetugr | 0:3e7450f1a938 | 15 | |
maetugr | 0:3e7450f1a938 | 16 | writeRegister(L3G4200D_INT1_THS_XH, 0x2C); // TODO: WTF?? |
maetugr | 0:3e7450f1a938 | 17 | writeRegister(L3G4200D_INT1_THS_XL, 0xA4); |
maetugr | 0:3e7450f1a938 | 18 | writeRegister(L3G4200D_INT1_THS_YH, 0x2C); |
maetugr | 0:3e7450f1a938 | 19 | writeRegister(L3G4200D_INT1_THS_YL, 0xA4); |
maetugr | 0:3e7450f1a938 | 20 | writeRegister(L3G4200D_INT1_THS_ZH, 0x2C); |
maetugr | 0:3e7450f1a938 | 21 | writeRegister(L3G4200D_INT1_THS_ZL, 0xA4); |
maetugr | 0:3e7450f1a938 | 22 | //writeRegister(L3G4200D_INT1_DURATION, 0x00); |
maetugr | 0:3e7450f1a938 | 23 | |
maetugr | 0:3e7450f1a938 | 24 | writeRegister(L3G4200D_CTRL_REG5, 0x00); // deactivates the filters (only use one of these options) |
maetugr | 0:3e7450f1a938 | 25 | //writeRegister(L3G4200D_CTRL_REG5, 0x12); // activates both high and low pass filters |
maetugr | 0:3e7450f1a938 | 26 | //writeRegister(L3G4200D_CTRL_REG5, 0x01); // activates high pass filter |
maetugr | 0:3e7450f1a938 | 27 | |
maetugr | 0:3e7450f1a938 | 28 | writeRegister(L3G4200D_CTRL_REG1, 0x0F); // starts Gyro measurement |
maetugr | 0:3e7450f1a938 | 29 | |
maetugr | 0:3e7450f1a938 | 30 | //calibrate(50, 0.01); |
maetugr | 0:3e7450f1a938 | 31 | } |
maetugr | 0:3e7450f1a938 | 32 | |
maetugr | 0:3e7450f1a938 | 33 | void L3G4200D::read() |
maetugr | 0:3e7450f1a938 | 34 | { |
maetugr | 0:3e7450f1a938 | 35 | readraw(); // read raw measurement data |
maetugr | 0:3e7450f1a938 | 36 | |
maetugr | 0:3e7450f1a938 | 37 | for (int i = 0; i < 3; i++) |
maetugr | 0:3e7450f1a938 | 38 | data[i] = (raw[i] - offset[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10) |
maetugr | 0:3e7450f1a938 | 39 | } |
maetugr | 0:3e7450f1a938 | 40 | |
maetugr | 0:3e7450f1a938 | 41 | int L3G4200D::readTemp() |
maetugr | 0:3e7450f1a938 | 42 | { |
maetugr | 0:3e7450f1a938 | 43 | return (short) readRegister(L3G4200D_OUT_TEMP); // read the sensors register for the temperature |
maetugr | 0:3e7450f1a938 | 44 | } |
maetugr | 0:3e7450f1a938 | 45 | |
maetugr | 0:3e7450f1a938 | 46 | void L3G4200D::readraw() |
maetugr | 0:3e7450f1a938 | 47 | { |
maetugr | 0:3e7450f1a938 | 48 | char buffer[6]; // 8-Bit pieces of axis data |
maetugr | 0:3e7450f1a938 | 49 | |
maetugr | 0:3e7450f1a938 | 50 | readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7) |
maetugr | 0:3e7450f1a938 | 51 | |
maetugr | 0:3e7450f1a938 | 52 | raw[0] = (short) (buffer[1] << 8 | buffer[0]); // join 8-Bit pieces to 16-bit short integers |
maetugr | 0:3e7450f1a938 | 53 | raw[1] = (short) (buffer[3] << 8 | buffer[2]); |
maetugr | 0:3e7450f1a938 | 54 | raw[2] = (short) (buffer[5] << 8 | buffer[4]); |
maetugr | 0:3e7450f1a938 | 55 | } |
maetugr | 0:3e7450f1a938 | 56 | |
maetugr | 0:3e7450f1a938 | 57 | void L3G4200D::calibrate(int times, float separation_time) |
maetugr | 0:3e7450f1a938 | 58 | { |
maetugr | 0:3e7450f1a938 | 59 | // calibrate sensor with an average of count samples (result of calibration stored in offset[]) |
maetugr | 0:3e7450f1a938 | 60 | float calib[3] = {0,0,0}; // temporary array for the sum of calibration measurement |
maetugr | 0:3e7450f1a938 | 61 | |
maetugr | 0:3e7450f1a938 | 62 | for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time |
maetugr | 0:3e7450f1a938 | 63 | readraw(); |
maetugr | 0:3e7450f1a938 | 64 | for (int j = 0; j < 3; j++) |
maetugr | 0:3e7450f1a938 | 65 | calib[j] += raw[j]; |
maetugr | 0:3e7450f1a938 | 66 | wait(separation_time); |
maetugr | 0:3e7450f1a938 | 67 | } |
maetugr | 0:3e7450f1a938 | 68 | |
maetugr | 0:3e7450f1a938 | 69 | for (int i = 0; i < 3; i++) |
maetugr | 0:3e7450f1a938 | 70 | offset[i] = calib[i]/times; // take the average of the calibration measurements |
maetugr | 0:3e7450f1a938 | 71 | } |