L3G4200D.cpp

00001 #include "L3G4200D.h"
00002 
00003 L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS) {
00004     // Turns on the L3G4200D's gyro and places it in normal mode.
00005     // Normal power mode, all axes enabled (for detailed info see datasheet)
00006     
00007     //writeRegister(L3G4200D_CTRL_REG2, 0x05);               // control filter
00008     writeRegister(L3G4200D_CTRL_REG2, 0x00);            // highpass filter disabled
00009     writeRegister(L3G4200D_CTRL_REG3, 0x00);
00010     writeRegister(L3G4200D_CTRL_REG4, 0xA0);            // sets acuracy to 2000 dps (degree per second)
00011     
00012     writeRegister(L3G4200D_REFERENCE, 0x00);
00013     //writeRegister(L3G4200D_STATUS_REG, 0x0F);
00014     
00015     writeRegister(L3G4200D_INT1_THS_XH, 0x2C); // TODO: WTF??
00016     writeRegister(L3G4200D_INT1_THS_XL, 0xA4);
00017     writeRegister(L3G4200D_INT1_THS_YH, 0x2C);
00018     writeRegister(L3G4200D_INT1_THS_YL, 0xA4);
00019     writeRegister(L3G4200D_INT1_THS_ZH, 0x2C);
00020     writeRegister(L3G4200D_INT1_THS_ZL, 0xA4);
00021     //writeRegister(L3G4200D_INT1_DURATION, 0x00);
00022     
00023     writeRegister(L3G4200D_CTRL_REG5, 0x00);            // deactivates the filters (only use one of these options)
00024     //writeRegister(L3G4200D_CTRL_REG5, 0x02);            // activates low pass filter
00025     //writeRegister(L3G4200D_CTRL_REG5, 0x12);          // activates both high and low pass filters
00026     //writeRegister(L3G4200D_CTRL_REG5, 0x01);          // activates high pass filter
00027     
00028     writeRegister(L3G4200D_CTRL_REG1, 0xBF);            // starts Gyro measurement with 400Hz ODR (don't use 800Hz ODR it has peaks in the ADC measurements!!)
00029     
00030     calibrate(50, 0.02);
00031 }
00032 
00033 void L3G4200D::read() {
00034     if (!(readRegister(L3G4200D_STATUS_REG) & 8))
00035         return;
00036         
00037     readraw();                                          // read raw measurement data
00038     
00039     for (int i = 0; i < 3; i++)
00040             data[i] = (raw[i] - offset[i])*0.07;               // subtract offset from calibration and multiply unit factor (datasheet s.10)
00041 }
00042 
00043 int L3G4200D::readTemp() {
00044     return (char) readRegister(L3G4200D_OUT_TEMP);     // read the sensors register for the temperature
00045 }
00046 
00047 void L3G4200D::readraw() {
00048     char buffer[6];                                     // 8-Bit pieces of axis data
00049     
00050     readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C   // TODO: why?!   | (1 << 7)
00051     
00052     raw[0] = (short) (buffer[1] << 8 | buffer[0]);     // join 8-Bit pieces to 16-bit short integers
00053     raw[1] = (short) (buffer[3] << 8 | buffer[2]);
00054     raw[2] = (short) (buffer[5] << 8 | buffer[4]);
00055 }
00056 
00057 void L3G4200D::calibrate(int times, float separation_time) {
00058     // calibrate sensor with an average of count samples (result of calibration stored in offset[])
00059     float calib[3] = {0,0,0};                           // temporary array for the sum of calibration measurement
00060     
00061     for (int i = 0; i < times; i++) {                   // read 'times' times the data in a very short time
00062         readraw();
00063         for (int j = 0; j < 3; j++)
00064             calib[j] += raw[j];
00065         wait(separation_time);
00066     }
00067     
00068     for (int i = 0; i < 3; i++)
00069         offset[i] = calib[i]/times;                     // take the average of the calibration measurements
00070 }