MPU6000.cpp

00001 /*CODED by Bruno Alfano on 07/03/2014
00002 www.xene.it
00003 */
00004 
00005 #include <mbed.h>
00006 #include "MPU6000.h"
00007 #include "float.h"
00008 
00009 mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs), accFilterCurrent(0), accFilterPre(0), gyroFilterCurrent(0), gyroFliterPre(0) {}
00010 
00011 /*-----------------------------------------------------------------------------------------------
00012                                     INITIALIZATION
00013 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
00014 low pass filter value; suitable values are:
00015 BITS_DLPF_CFG_256HZ_NOLPF2
00016 BITS_DLPF_CFG_188HZ
00017 BITS_DLPF_CFG_98HZ
00018 BITS_DLPF_CFG_42HZ
00019 BITS_DLPF_CFG_20HZ
00020 BITS_DLPF_CFG_10HZ
00021 BITS_DLPF_CFG_5HZ
00022 BITS_DLPF_CFG_2100HZ_NOLPF
00023 returns 1 if an error occurred
00024 -----------------------------------------------------------------------------------------------*/
00025 bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter)
00026 {
00027     unsigned int response;
00028     spi.format(8,0);
00029     spi.frequency(10000000);
00030     //FIRST OF ALL DISABLE I2C
00031     select();
00032     response=spi.write(MPUREG_USER_CTRL);
00033     response=spi.write(BIT_I2C_IF_DIS);
00034     deselect();
00035     //RESET CHIP
00036     select();
00037     response=spi.write(MPUREG_PWR_MGMT_1);
00038     response=spi.write(BIT_H_RESET);
00039     deselect();
00040     wait(0.15);
00041     //WAKE UP AND SET GYROZ CLOCK
00042     select();
00043     response=spi.write(MPUREG_PWR_MGMT_1);
00044     response=spi.write(MPU_CLK_SEL_PLLGYROZ);
00045     deselect();
00046     //DISABLE I2C
00047     select();
00048     response=spi.write(MPUREG_USER_CTRL);
00049     response=spi.write(BIT_I2C_IF_DIS);
00050     deselect();
00051 
00052     //WHO AM I?
00053     // Begin transmission
00054     select();
00055     // Bitwise OR of WHOAMI Register (default 0x68) and READ_FLAG
00056     response=spi.write(MPUREG_WHOAMI|READ_FLAG);
00057     response=spi.write(0x00);
00058     // end transmission
00059     deselect();
00060     if(response<100) {
00061         return 0;   //COULDN'T RECEIVE WHOAMI
00062     }
00063 
00064     //SET SAMPLE RATE
00065     select();
00066     response=spi.write(MPUREG_SMPLRT_DIV);
00067     response=spi.write(sample_rate_div);
00068     deselect();
00069     // FS & DLPF
00070     select();
00071     response=spi.write(MPUREG_CONFIG);
00072     response=spi.write(low_pass_filter);
00073     deselect();
00074     //DISABLE INTERRUPTS
00075     select();
00076     response=spi.write(MPUREG_INT_ENABLE);
00077     response=spi.write(0x00);
00078     deselect();
00079     return 0;
00080 }
00081 
00082 /*-----------------------------------------------------------------------------------------------
00083                                 ACCELEROMETER SCALE
00084 usage: call this function at startup, after initialization, to set the right range for the
00085 accelerometers. Suitable ranges are:
00086 BITS_FS_2G
00087 BITS_FS_4G
00088 BITS_FS_8G
00089 BITS_FS_16G
00090 returns the range set (2,4,8 or 16)
00091 -----------------------------------------------------------------------------------------------*/
00092 unsigned int mpu6000_spi::set_acc_scale(int scale)
00093 {
00094     unsigned int temp_scale;
00095     select();
00096     spi.write(MPUREG_ACCEL_CONFIG);
00097     spi.write(scale);
00098     deselect();
00099     switch (scale) {
00100         case BITS_FS_2G:
00101             acc_divider=16384;
00102             break;
00103         case BITS_FS_4G:
00104             acc_divider=8192;
00105             break;
00106         case BITS_FS_8G:
00107             acc_divider=4096;
00108             break;
00109         case BITS_FS_16G:
00110             acc_divider=2048;
00111             break;
00112     }
00113     wait(0.01);
00114     select();
00115     temp_scale=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
00116     temp_scale=spi.write(0x00);
00117     deselect();
00118     switch (temp_scale) {
00119         case BITS_FS_2G:
00120             temp_scale=2;
00121             break;
00122         case BITS_FS_4G:
00123             temp_scale=4;
00124             break;
00125         case BITS_FS_8G:
00126             temp_scale=8;
00127             break;
00128         case BITS_FS_16G:
00129             temp_scale=16;
00130             break;
00131     }
00132     return temp_scale;
00133 }
00134 
00135 
00136 /*-----------------------------------------------------------------------------------------------
00137                                 GYROSCOPE SCALE
00138 usage: call this function at startup, after initialization, to set the right range for the
00139 gyroscopes. Suitable ranges are:
00140 BITS_FS_250DPS
00141 BITS_FS_500DPS
00142 BITS_FS_1000DPS
00143 BITS_FS_2000DPS
00144 returns the range set (250,500,1000 or 2000)
00145 -----------------------------------------------------------------------------------------------*/
00146 unsigned int mpu6000_spi::set_gyro_scale(int scale)
00147 {
00148     unsigned int temp_scale;
00149     select();
00150     spi.write(MPUREG_GYRO_CONFIG);
00151     spi.write(scale);
00152     deselect();
00153     switch (scale) {
00154         case BITS_FS_250DPS:
00155             gyro_divider=131;
00156             break;
00157         case BITS_FS_500DPS:
00158             gyro_divider=65.5;
00159             break;
00160         case BITS_FS_1000DPS:
00161             gyro_divider=32.8;
00162             break;
00163         case BITS_FS_2000DPS:
00164             gyro_divider=16.4;
00165             break;
00166     }
00167     wait(0.01);
00168     select();
00169     temp_scale=spi.write(MPUREG_GYRO_CONFIG|READ_FLAG);
00170     temp_scale=spi.write(0x00);
00171     deselect();
00172     switch (temp_scale) {
00173         case BITS_FS_250DPS:
00174             temp_scale=250;
00175             break;
00176         case BITS_FS_500DPS:
00177             temp_scale=500;
00178             break;
00179         case BITS_FS_1000DPS:
00180             temp_scale=1000;
00181             break;
00182         case BITS_FS_2000DPS:
00183             temp_scale=2000;
00184             break;
00185     }
00186     return temp_scale;
00187 }
00188 
00189 
00190 /*-----------------------------------------------------------------------------------------------
00191                                 WHO AM I?
00192 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
00193 mpu6000 which should be 104 when in SPI mode.
00194 returns the I2C address (104)
00195 -----------------------------------------------------------------------------------------------*/
00196 unsigned int mpu6000_spi::whoami()
00197 {
00198     // create instance of response
00199     unsigned int response;
00200 
00201     // Begin transmission
00202     select();
00203 
00204     // Bitwise OR of WHOAMI Register (default 0x68) and READ_FLAG
00205     response=spi.write(MPUREG_WHOAMI|READ_FLAG);
00206     response=spi.write(0x00);
00207 
00208     // End transmission
00209     deselect();
00210 
00211     return response;
00212 }
00213 
00214 /** High = Safe. Low = Unsafe */
00215 int mpu6000_spi::whoamiCheck()
00216 {
00217     // mask whoami() against expected value
00218     return whoami()!=0x68;
00219 }
00220 
00221 
00222 // Get Tilt Angle from Accelerometer
00223 float mpu6000_spi::getAccTilt()
00224 {
00225     float Z,X;
00226     wait(0.0001);
00227     Z = read_acc(2);
00228     wait_us(10);
00229     X=read_acc(0);
00230     float temp=Z/X;
00231     if(temp !=temp) {
00232         if (X>=0 && Z>=0) {
00233             temp=FLT_MAX;
00234         } else {
00235             temp=FLT_MIN;
00236         }
00237     }
00238 
00239     return atan(temp)*180/pi;
00240 
00241 }
00242 
00243 
00244 /*-----------------------------------------------------------------------------------------------
00245                                 READ ACCELEROMETER
00246 usage: call this function to read accelerometer data. Axis represents selected axis:
00247 0 -> X axis
00248 1 -> Y axis
00249 2 -> Z axis
00250 returns the value in Gs
00251 -----------------------------------------------------------------------------------------------*/
00252 float mpu6000_spi::read_acc(int axis)
00253 {
00254     uint8_t responseH,responseL;
00255     int16_t bit_data;
00256     float data;
00257     select();
00258     switch (axis) {
00259         case 0:
00260             responseH=spi.write(MPUREG_ACCEL_XOUT_H | READ_FLAG);
00261             break;
00262         case 1:
00263             responseH=spi.write(MPUREG_ACCEL_YOUT_H | READ_FLAG);
00264             break;
00265         case 2:
00266             responseH=spi.write(MPUREG_ACCEL_ZOUT_H | READ_FLAG);
00267             break;
00268     }
00269 //    wait(0.0001);
00270     responseH=spi.write(0x00);
00271     responseL=spi.write(0x00);
00272     bit_data=((int16_t)responseH<<8)|responseL;
00273     data=(float)bit_data;
00274     data = data/acc_divider;
00275     deselect();
00276     //printf("data = %f\n\r",data);
00277     return data;
00278 }
00279 
00280 /*-----------------------------------------------------------------------------------------------
00281                                 READ GYROSCOPE
00282 usage: call this function to read gyroscope data. Axis represents selected axis:
00283 0 -> X axis
00284 1 -> Y axis
00285 2 -> Z axis
00286 returns the value in Degrees per second
00287 -----------------------------------------------------------------------------------------------*/
00288 float mpu6000_spi::read_rot(int axis)
00289 {
00290     uint8_t responseH,responseL;
00291     int16_t bit_data;
00292     float data;
00293     select();
00294     switch (axis) {
00295         case 0:
00296             responseH=spi.write(MPUREG_GYRO_XOUT_H | READ_FLAG);
00297             break;
00298         case 1:
00299             responseH=spi.write(MPUREG_GYRO_YOUT_H | READ_FLAG);
00300             break;
00301         case 2:
00302             responseH=spi.write(MPUREG_GYRO_ZOUT_H | READ_FLAG);
00303             break;
00304     }
00305     wait(0.0001);
00306     responseH=spi.write(0x00);
00307     responseL=spi.write(0x00);
00308     bit_data=((int16_t)responseH<<8)|responseL;
00309     data=(float)bit_data;
00310     data=data/gyro_divider;
00311     deselect();
00312     return data;
00313 }
00314 
00315 /*-----------------------------------------------------------------------------------------------
00316                                 READ TEMPERATURE
00317 usage: call this function to read temperature data.
00318 returns the value in °C
00319 -----------------------------------------------------------------------------------------------*/
00320 float mpu6000_spi::read_temp()
00321 {
00322     uint8_t responseH,responseL;
00323     int16_t bit_data;
00324     float data;
00325     select();
00326     responseH=spi.write(MPUREG_TEMP_OUT_H | READ_FLAG);
00327     responseH=spi.write(0x00);
00328     responseL=spi.write(0x00);
00329     bit_data=((int16_t)responseH<<8)|responseL;
00330     data=(float)bit_data;
00331     data=(data/340)+36.53;
00332     deselect();
00333     return data;
00334 }
00335 
00336 /*-----------------------------------------------------------------------------------------------
00337                                 READ ACCELEROMETER CALIBRATION
00338 usage: call this function to read accelerometer data. Axis represents selected axis:
00339 0 -> X axis
00340 1 -> Y axis
00341 2 -> Z axis
00342 returns Factory Trim value
00343 -----------------------------------------------------------------------------------------------*/
00344 int mpu6000_spi::calib_acc(int axis)
00345 {
00346     uint8_t responseH,responseL,calib_data;
00347     int temp_scale;
00348     //READ CURRENT ACC SCALE
00349     select();
00350     responseH=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
00351     temp_scale=spi.write(0x00);
00352     deselect();
00353     wait(0.01);
00354     set_acc_scale(BITS_FS_8G);
00355     wait(0.01);
00356     //ENABLE SELF TEST
00357     select();
00358     responseH=spi.write(MPUREG_ACCEL_CONFIG);
00359     temp_scale=spi.write(0x80>>axis);
00360     deselect();
00361     wait(0.01);
00362     select();
00363     responseH=spi.write(MPUREG_SELF_TEST_X|READ_FLAG);
00364     switch(axis) {
00365         case 0:
00366             responseH=spi.write(0x00);
00367             responseL=spi.write(0x00);
00368             responseL=spi.write(0x00);
00369             responseL=spi.write(0x00);
00370             calib_data=((responseH&11100000)>>3)|((responseL&00110000)>>4);
00371             break;
00372         case 1:
00373             responseH=spi.write(0x00);
00374             responseH=spi.write(0x00);
00375             responseL=spi.write(0x00);
00376             responseL=spi.write(0x00);
00377             calib_data=((responseH&11100000)>>3)|((responseL&00001100)>>2);
00378             break;
00379         case 2:
00380             responseH=spi.write(0x00);
00381             responseH=spi.write(0x00);
00382             responseH=spi.write(0x00);
00383             responseL=spi.write(0x00);
00384             calib_data=((responseH&11100000)>>3)|((responseL&00000011));
00385             break;
00386     }
00387     deselect();
00388     wait(0.01);
00389     set_acc_scale(temp_scale);
00390     return calib_data;
00391 }
00392 
00393 /*-----------------------------------------------------------------------------------------------
00394                                 SPI SELECT AND DESELECT
00395 usage: enable and disable mpu6000 communication bus
00396 -----------------------------------------------------------------------------------------------*/
00397 void mpu6000_spi::select()
00398 {
00399     //Set CS low to start transmission (interrupts conversion)
00400     cs = 0;
00401 }
00402 void mpu6000_spi::deselect()
00403 {
00404     //Set CS high to stop transmission (restarts conversion)
00405     cs = 1;
00406 }
00407 
00408 float mpu6000_spi::angle_y()
00409 {
00410     float gyro = read_rot(1);
00411     float acc = getAccTilt();
00412 
00413     // complementary filter
00414     accFilterCurrent = 0.8187*accFilterPre+0.1813*acc;
00415     gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro;
00416 
00417     //pc.printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
00418     //pc.printf("acc=%f\n\r",acc);
00419     //pc.printf("GYRO=%f\n\r",gyro);
00420     accFilterPre = accFilterCurrent;
00421     gyroFliterPre = gyroFilterCurrent;
00422     return accFilterCurrent + gyroFilterCurrent;
00423 }
00424 
00425 float mpu6000_spi::rereadAngle_y()
00426 {
00427     return accFilterCurrent + gyroFilterCurrent;
00428 }