Matthias Grob / Mbed 2 deprecated IMU_10DOF

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

Dependencies:   mbed

IMU/Sensors/Gyro/L3G4200D.cpp@4:f62337b907e5, 2013-08-29 (annotated)

Committer:
maetugr
Date:
Thu Aug 29 13:52:30 2013 +0000
Revision:
4:f62337b907e5
Parent:
0:3e7450f1a938 
The Altitude Sensor is now implemented, it's really 10DOF now ;); TODO: Autocalibration

Who changed what in which revision?

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