IMU.cpp

00001 /**
00002  * @author Eric Van den Bulck
00003  *
00004  * @section LICENSE
00005  *
00006  * Copyright (c) 2010 ARM Limited
00007  *
00008  * @section DESCRIPTION
00009  *
00010  * Pololu MinIMU-9 v2 sensor:
00011  *   L3GD20 three-axis digital output gyroscope.
00012  *   LSM303 three-axis digital accelerometer and magnetometer 
00013  *
00014  * Information to build this library:
00015  * 1. The Arduino libraries for this sensor from the Pololu and Adafruit websites, available at gitbub.
00016  *       https://github.com/adafruit/Adafruit_L3GD20
00017  *       https://github.com/pololu/L3G/tree/master/L3G
00018  * 2. The Rasberry Pi code at https://github.com/idavidstory/goPiCopter/tree/master/io/sensors
00019  *       https://github.com/idavidstory/goPiCopter/tree/master/io/sensors
00020  * 3. Information on how to write libraries:  http://mbed.org/cookbook/Writing-a-Library
00021  * 4. Information on I2C control: http://mbed.org/users/mbed_official/code/mbed/
00022  * 5. The Youtube videos from Brian Douglas (3 x 15') at http://www.youtube.com/playlist?list=PLUMWjy5jgHK30fkGrufluENJqZmLZkmqI
00023  * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
00024  * Reading an IMU Without Kalman: The Complementary Filter:  http://www.pieter-jan.com/node/11
00025  * setup info on the minIMU-9 v2 on http://forum.pololu.com/viewtopic.php?f=3&t=4801&start=30
00026  */
00027 
00028 #include "mbed.h"
00029 #include "IMU.h"
00030 
00031 IMU::IMU(PinName sda, PinName scl) : _i2c(sda, scl) {
00032     _i2c.frequency(400000);  /* 400kHz, fast mode. */
00033 }
00034 
00035 char IMU::init(void)  /* returns error upon a non-zero return */
00036 {
00037    char ack, rx, tx[2];
00038    double pi, a, A;
00039 
00040 // 1. Initialize selected registers: 2c.read and i2c.write return 0 on success (ack)
00041 // --------------------------------
00042 //
00043 // 1.a Enable L3DG20 gyrosensor and set operational mode:
00044 // CTRL_REG1: set to 0x1F = 0001-1111 --> enable sensor, DR= 95Hz, LPF-Cut-off-freq=25Hz.
00045 // CTRL_REG1: set to 0x5F = 0101-1111 --> enable sensor, DR=190Hz, LPF-Cut-off-freq=25Hz.
00046 // CTRL_REG4: left at default = 0x00 --> Full Scale = 250 degrees/second --> Sensitivity = 0.00875 dps/digit.
00047     address = L3GD20_ADDR;
00048     tx[0] = L3GD20_CTRL_REG1; // address contrl_register 1
00049     tx[1] = 0x1F; // 00-01-1-111 enable sensor and set operational mode. 
00050     ack = _i2c.write(address, tx, 2);
00051     ack |= _i2c.write(address, tx, 1);
00052     ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x1F) ack |= 1;
00053 //
00054 // 1.b Enable LSM303 accelerometer and set operational mode:
00055 // CTRL_REG1: set to 0x37 = 0011 1111 --> DR =  25Hz & enable sensor in low power mode (normal mode zmienic ostatnie 4 na 0111)
00056 // CTRL_REG1: set to 0x47 = 0100 1111 --> DR =  50Hz & enable sensor -//
00057 // CTRL_REG1: set to 0x57 = 0101 1111 --> DR = 100Hz & enable sensor
00058 // CTRL_REG1: set to 0x67 = 0110 1111 --> DR = 200Hz & enable sensor 
00059 // CTRL_REG1: set to 0x77 = 0111 0111 --> DR = 400Hz & enable sensor (0x7F low power mode - b duze oscylacje)
00060 // CTRL_REG4: set to 0x08 = 0000 1000 --> Full Scale = +/- 2G & high resolution --> Sensitivity = 0.001G/digit.
00061     address = LSM303_A_ADDR; 
00062     tx[0] = LSM303_A_CTRL_REG1;
00063     tx[1] = 0x77; //                    --> 400 Hz Data rate speed - p.24/42 of datasheet
00064     ack |= _i2c.write(address, tx, 2);
00065     ack |= _i2c.write(address, tx, 1);
00066     ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x77) ack |= 1;    
00067     tx[0] = LSM303_A_CTRL_REG4;
00068     //tx[1] = 0x08; // 0000 1000 enable high resolution mode + selects default 2G scale. p.26/42
00069     tx[1] = 0x18; // 0001 1000 enable high resolution mode + selects 4G scale.
00070     ack |= _i2c.write(address, tx ,2);
00071     ack |= _i2c.write(address, tx, 1);
00072     ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x18) ack |= 1;    
00073 //
00074 // 1.c enable LSM303 magnetometer and set operational mode:
00075 // CRA_REG is reset from 0x10 to 0x14 = 00010100 -->  30 Hz data output rate.
00076 // CRA_REG is reset from 0x10 to 0x18 = 00011000 -->  75 Hz data output rate.
00077 // CRA_REG is reset from 0x10 to 0x1C = 00011100 --> 220 Hz data output rate.
00078 // CRB_REG is kept at default = 00100000 = 0x20 --> range +/- 1.3 Gauss, Gain = 1100/980(Z) LSB/Gauss.
00079 // MR_REG is reset from 0x03 to 0x00 -> continuos conversion mode in stead of sleep mode.
00080     address = LSM303_M_ADDR; 
00081     tx[0] = LSM303_M_CRA_REG;
00082     tx[1] = 0x18;                       //  -->  75 Hz minimum output rate - p.36/42 of datasheet
00083     ack |= _i2c.write(address, tx, 2);
00084     ack |= _i2c.write(address, tx, 1);
00085     ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x18) ack |= 1;    
00086     tx[0] = LSM303_M_MR_REG;
00087     tx[1] = 0x00; // 0000 0000 --> continuous-conversion mode 25 Hz Data rate speed - p.24/42 of datasheet
00088     ack |= _i2c.write(address, tx, 2);
00089     ack |= _i2c.write(address, tx, 1);
00090     ack |= _i2c.read(address+1, &rx, 1); if (rx != 0x00) ack |= 1;    
00091  
00092 // 2. Initialize calibration constants with predetermined values.
00093 // acceleration:
00094 // My calibration values, vs. the website http://rwsarduino.blogspot.be/2013/01/inertial-orientation-sensing.html
00095 
00096 /* my predetermined static bias counts */
00097     L3GD20_biasX = (int16_t)  97; // BTLO 90 /* digit counts */
00098     L3GD20_biasY = (int16_t)  5; //BYLO 180
00099     L3GD20_biasZ = (int16_t) 140; //BYLO -10
00100 
00101 /* reference gravity acceleration */
00102     //g_0 = 9.815; //przy ustawieniu zakresu na +/- 2g
00103     g_0 = 19.63; //przy ustawienu zakresu na +/- 4g  // CTR_REG 4 = 0001 1000 0x18
00104 
00105 /* filter parameters: assume 400 Hz sampling rare and 2nd orcer Butterworth filter with fc = 5Hz. */
00106     pi = 3.1415926536;
00107     A = tan(pi*5/400); a = 1 + sqrt(2.0)*A + A*A;
00108     FF[1] = 2*(A*A-1)/a;
00109     FF[2] = (1-sqrt(2.0)*A+A*A)/a;
00110     FF[0] = (1+FF[1]+FF[2])/4;
00111 
00112     return ack;
00113 }
00114 
00115 char IMU::readData(float *d) 
00116 {
00117       char ack, reg, D[6];
00118       int16_t W[3];
00119 
00120 // report the data in rad/s
00121 // gyro data are 16 bit readings per axis, stored: X_l, X_h, Y_l, Y_h, Z_l, Z_h
00122 // #define L3GD20_SENSITIVITY_250DPS 0.00875  ---  #define L3GD20_DPS_TO_RADS  0.017453293
00123          address = L3GD20_ADDR;
00124          reg = L3GD20_OUT_X_L | 0x80; // set address auto-increment bit
00125          ack = _i2c.write(address,&reg,1);  ack |= _i2c.read(address+1,D,6); 
00126          W[0] = (int16_t) (D[1] << 8 | D[0]); 
00127          W[1] = (int16_t) (D[3] << 8 | D[2]); 
00128          W[2] = (int16_t) (D[5] << 8 | D[4]); 
00129          *(d+0) = (float) 0.971*(W[0]-L3GD20_biasX)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
00130          *(d+1) = (float) 0.998*(W[1]-L3GD20_biasY)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
00131          *(d+2) = (float) 1.002*(W[2]-L3GD20_biasZ)*L3GD20_SENSITIVITY_250DPS*L3GD20_DPS_TO_RADS;
00132          
00133 // Accelerometer data are stored as 12 bit readings, left justified per axis.
00134 // The data needs to be shifted 4 digits to the right! This is not general, only for the A measurement.
00135          address = LSM303_A_ADDR; 
00136          reg = LSM303_A_OUT_X_L | 0x80; // set address auto-increment bit
00137          ack |= _i2c.write(address,&reg,1);  ack |= _i2c.read(address+1,D,6); 
00138          W[0] = ((int16_t) (D[1] << 8 | D[0])) >> 4;
00139          W[1] = ((int16_t) (D[3] << 8 | D[2])) >> 4;
00140          W[2] = ((int16_t) (D[5] << 8 | D[4])) >> 4;
00141          *(d+3) = (float) g_0*0.986*(W[0]+18)/1000;  // kalibracja - zmiana z 0.991 na 0.986 i z +34 na +18
00142          *(d+4) = (float) g_0*0.99*(W[1]-4)/1000;   // kalibracja - zmiana z 0.970 na 0.99 i z +2 na -4
00143          *(d+5) = (float) g_0*0.983*(W[2]+9)/1000;  // kalibracja - zmiana +28 na +9
00144 // GN = 001  
00145 // Magnetometer; are stored as 12 bit readings, right justified per axis.
00146          address = LSM303_M_ADDR; 
00147          reg = LSM303_M_OUT_X_H | 0x80; // set address auto-increment bit
00148          ack |= _i2c.write(address,&reg,1);  ack |= _i2c.read(address+1,D,6); 
00149          W[0] = ((int16_t) (D[0] << 8 | D[1]));
00150          W[1] = ((int16_t) (D[4] << 8 | D[5]));
00151          W[2] = ((int16_t) (D[2] << 8 | D[3]));
00152          *(d+6) = (float) 2.813*(W[0]-220)/1100; //Z bylo 264
00153          *(d+7) = (float) 2.822*(W[1]+ 230)/1100; //X bylo 
00154          *(d+8) = (float) 2.880*(W[2]- 380)/980;  //Y
00155 
00156          return ack;
00157 }
00158 
00159 void IMU::filterData(float *d, double *D) 
00160 // 2nd order Butterworth filter. Filter coefficients FF computed in function init.
00161 {
00162     for (int i=0; i<9; ++i) {
00163 //        *(FD+9*i+2) = *(FD+9*i+1); *(FD+9*i+1) = *(FD+9*i); *(FD+9*i) = (double) d[i];
00164          FD[2][i] = FD[1][i]; FD[1][i] = FD[0][i]; FD[0][i] = (double) d[i];
00165          FD[5][i] = FD[4][i]; FD[4][i] = FD[3][i]; 
00166          FD[3][i] = FF[0]*(FD[0][i] + 2*FD[1][i] + FD[2][i]) - FF[1]*FD[4][i] - FF[2]*FD[5][i];
00167          D[i] = FD[3][i]; 
00168     }
00169 //         D[0] = FD[0][2]; D[1] = FD[1][2]; D[2] = FD[2][2];
00170 }