MPU6000 library to get accelerometer, gyroscope and temperature readings via SPI.

Files at this revision

API Documentation at this revision

Comitter:
brunoalfano
Date:
Sat Mar 08 12:38:14 2014 +0000
Commit message:
First version of a simple MPU6000 SPI library to read sensors' data

Changed in this revision

MPU6000.cpp Show annotated file Show diff for this revision Revisions of this file
MPU6000.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 2edbd561b520 MPU6000.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6000.cpp	Sat Mar 08 12:38:14 2014 +0000
@@ -0,0 +1,348 @@
+/*CODED by Bruno Alfano on 07/03/2014
+www.xene.it
+*/
+
+#include <mbed.h>
+#include "MPU6000.h"
+
+mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
+
+/*-----------------------------------------------------------------------------------------------
+                                    INITIALIZATION
+usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
+low pass filter value; suitable values are:
+BITS_DLPF_CFG_256HZ_NOLPF2
+BITS_DLPF_CFG_188HZ
+BITS_DLPF_CFG_98HZ
+BITS_DLPF_CFG_42HZ
+BITS_DLPF_CFG_20HZ
+BITS_DLPF_CFG_10HZ 
+BITS_DLPF_CFG_5HZ 
+BITS_DLPF_CFG_2100HZ_NOLPF
+returns 1 if an error occurred
+-----------------------------------------------------------------------------------------------*/
+bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter){
+    unsigned int response;
+    spi.format(8,0);
+    spi.frequency(1000000);
+    //FIRST OF ALL DISABLE I2C
+    select();
+    response=spi.write(MPUREG_USER_CTRL);
+    response=spi.write(BIT_I2C_IF_DIS);
+    deselect();
+    //RESET CHIP
+    select();
+    response=spi.write(MPUREG_PWR_MGMT_1);
+    response=spi.write(BIT_H_RESET); 
+    deselect();
+    wait(0.15);
+    //WAKE UP AND SET GYROZ CLOCK
+    select();
+    response=spi.write(MPUREG_PWR_MGMT_1);
+    response=spi.write(MPU_CLK_SEL_PLLGYROZ); 
+    deselect();
+    //DISABLE I2C
+    select();
+    response=spi.write(MPUREG_USER_CTRL);
+    response=spi.write(BIT_I2C_IF_DIS);
+    deselect();
+    //WHO AM I?
+    select();
+    response=spi.write(MPUREG_WHOAMI|READ_FLAG);
+    response=spi.write(0x00);
+    deselect();
+    if(response<100){return 0;}//COULDN'T RECEIVE WHOAMI
+    //SET SAMPLE RATE
+    select();
+    response=spi.write(MPUREG_SMPLRT_DIV);
+    response=spi.write(sample_rate_div); 
+    deselect();
+    // FS & DLPF
+    select();
+    response=spi.write(MPUREG_CONFIG);
+    response=spi.write(low_pass_filter);
+    deselect();
+    //DISABLE INTERRUPTS
+    select();
+    response=spi.write(MPUREG_INT_ENABLE);
+    response=spi.write(0x00);
+    deselect();
+    return 0;
+}
+
+/*-----------------------------------------------------------------------------------------------
+                                ACCELEROMETER SCALE
+usage: call this function at startup, after initialization, to set the right range for the
+accelerometers. Suitable ranges are:
+BITS_FS_2G
+BITS_FS_4G
+BITS_FS_8G
+BITS_FS_16G
+returns the range set (2,4,8 or 16)
+-----------------------------------------------------------------------------------------------*/
+unsigned int mpu6000_spi::set_acc_scale(int scale){
+    unsigned int temp_scale;
+    select();
+    spi.write(MPUREG_ACCEL_CONFIG);
+    spi.write(scale);  
+    deselect();    
+    switch (scale){
+        case BITS_FS_2G:
+            acc_divider=16384;
+        break;
+        case BITS_FS_4G:
+            acc_divider=8192;
+        break;
+        case BITS_FS_8G:
+            acc_divider=4096;
+        break;
+        case BITS_FS_16G:
+            acc_divider=2048;
+        break;   
+    }
+    wait(0.01);
+    select();
+    temp_scale=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
+    temp_scale=spi.write(0x00);  
+    deselect();
+    switch (temp_scale){
+        case BITS_FS_2G:
+            temp_scale=2;
+        break;
+        case BITS_FS_4G:
+            temp_scale=4;
+        break;
+        case BITS_FS_8G:
+            temp_scale=8;
+        break;
+        case BITS_FS_16G:
+            temp_scale=16;
+        break;   
+    }
+    return temp_scale;
+}
+
+
+/*-----------------------------------------------------------------------------------------------
+                                GYROSCOPE SCALE
+usage: call this function at startup, after initialization, to set the right range for the
+gyroscopes. Suitable ranges are:
+BITS_FS_250DPS
+BITS_FS_500DPS
+BITS_FS_1000DPS
+BITS_FS_2000DPS
+returns the range set (250,500,1000 or 2000)
+-----------------------------------------------------------------------------------------------*/
+unsigned int mpu6000_spi::set_gyro_scale(int scale){
+    unsigned int temp_scale;
+    select();
+    spi.write(MPUREG_GYRO_CONFIG);
+    spi.write(scale);  
+    deselect();    
+    switch (scale){
+        case BITS_FS_250DPS:
+            gyro_divider=131;
+        break;
+        case BITS_FS_500DPS:
+            gyro_divider=65.5;
+        break;
+        case BITS_FS_1000DPS:
+            gyro_divider=32.8;
+        break;
+        case BITS_FS_2000DPS:
+            gyro_divider=16.4;
+        break;   
+    }
+    wait(0.01);
+    select();
+    temp_scale=spi.write(MPUREG_GYRO_CONFIG|READ_FLAG);
+    temp_scale=spi.write(0x00);  
+    deselect();
+    switch (temp_scale){
+        case BITS_FS_250DPS:
+            temp_scale=250;
+        break;
+        case BITS_FS_500DPS:
+            temp_scale=500;
+        break;
+        case BITS_FS_1000DPS:
+            temp_scale=1000;
+        break;
+        case BITS_FS_2000DPS:
+            temp_scale=2000;
+        break;   
+    }
+    return temp_scale;
+}
+
+
+/*-----------------------------------------------------------------------------------------------
+                                WHO AM I?
+usage: call this function to know if SPI is working correctly. It checks the I2C address of the
+mpu6000 which should be 104 when in SPI mode.
+returns the I2C address (104)
+-----------------------------------------------------------------------------------------------*/
+unsigned int mpu6000_spi::whoami(){
+    unsigned int response;
+    select();
+    response=spi.write(MPUREG_WHOAMI|READ_FLAG);
+    response=spi.write(0x00);
+    deselect();
+    return response;
+}
+
+
+/*-----------------------------------------------------------------------------------------------
+                                READ ACCELEROMETER
+usage: call this function to read accelerometer data. Axis represents selected axis:
+0 -> X axis
+1 -> Y axis
+2 -> Z axis
+returns the value in Gs
+-----------------------------------------------------------------------------------------------*/
+float mpu6000_spi::read_acc(int axis){
+    uint8_t responseH,responseL;
+    int16_t bit_data;
+    float data;
+    select();
+    switch (axis){
+        case 0:
+        responseH=spi.write(MPUREG_ACCEL_XOUT_H | READ_FLAG);
+        break;
+        case 1:
+        responseH=spi.write(MPUREG_ACCEL_YOUT_H | READ_FLAG);
+        break;
+        case 2:
+        responseH=spi.write(MPUREG_ACCEL_ZOUT_H | READ_FLAG);
+        break;
+    }
+    responseH=spi.write(0x00);
+    responseL=spi.write(0x00);
+    bit_data=((int16_t)responseH<<8)|responseL;
+    data=(float)bit_data;
+    data=data/acc_divider;
+    deselect();
+    return data;
+}
+
+/*-----------------------------------------------------------------------------------------------
+                                READ GYROSCOPE
+usage: call this function to read gyroscope data. Axis represents selected axis:
+0 -> X axis
+1 -> Y axis
+2 -> Z axis
+returns the value in Degrees per second
+-----------------------------------------------------------------------------------------------*/
+float mpu6000_spi::read_rot(int axis){
+    uint8_t responseH,responseL;
+    int16_t bit_data;
+    float data;
+    select();
+    switch (axis){
+        case 0:
+        responseH=spi.write(MPUREG_GYRO_XOUT_H | READ_FLAG);
+        break;
+        case 1:
+        responseH=spi.write(MPUREG_GYRO_YOUT_H | READ_FLAG);
+        break;
+        case 2:
+        responseH=spi.write(MPUREG_GYRO_ZOUT_H | READ_FLAG);
+        break;
+    }
+    responseH=spi.write(0x00);
+    responseL=spi.write(0x00);
+    bit_data=((int16_t)responseH<<8)|responseL;
+    data=(float)bit_data;
+    data=data/gyro_divider;
+    deselect();
+    return data;
+}
+
+/*-----------------------------------------------------------------------------------------------
+                                READ TEMPERATURE
+usage: call this function to read temperature data. 
+returns the value in °C
+-----------------------------------------------------------------------------------------------*/
+float mpu6000_spi::read_temp(){
+    uint8_t responseH,responseL;
+    int16_t bit_data;
+    float data;
+    select();
+    responseH=spi.write(MPUREG_TEMP_OUT_H | READ_FLAG);
+    responseH=spi.write(0x00);
+    responseL=spi.write(0x00);
+    bit_data=((int16_t)responseH<<8)|responseL;
+    data=(float)bit_data;
+    data=(data/340)+36.53;
+    deselect();
+    return data;
+}
+
+/*-----------------------------------------------------------------------------------------------
+                                READ ACCELEROMETER CALIBRATION
+usage: call this function to read accelerometer data. Axis represents selected axis:
+0 -> X axis
+1 -> Y axis
+2 -> Z axis
+returns Factory Trim value
+-----------------------------------------------------------------------------------------------*/
+int mpu6000_spi::calib_acc(int axis){
+    uint8_t responseH,responseL,calib_data;
+    int temp_scale;
+    //READ CURRENT ACC SCALE
+    select();
+    responseH=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
+    temp_scale=spi.write(0x00);  
+    deselect();
+    wait(0.01);
+    set_acc_scale(BITS_FS_8G);
+    wait(0.01);
+    //ENABLE SELF TEST
+    select();
+    responseH=spi.write(MPUREG_ACCEL_CONFIG);
+    temp_scale=spi.write(0x80>>axis);  
+    deselect();
+    wait(0.01);
+    select();
+    responseH=spi.write(MPUREG_SELF_TEST_X|READ_FLAG);
+    switch(axis){
+        case 0:
+            responseH=spi.write(0x00);
+            responseL=spi.write(0x00);
+            responseL=spi.write(0x00);
+            responseL=spi.write(0x00);
+            calib_data=((responseH&11100000)>>3)|((responseL&00110000)>>4);
+        break;
+        case 1:
+            responseH=spi.write(0x00);
+            responseH=spi.write(0x00);
+            responseL=spi.write(0x00);
+            responseL=spi.write(0x00);
+            calib_data=((responseH&11100000)>>3)|((responseL&00001100)>>2);
+        break;
+        case 2:
+            responseH=spi.write(0x00);
+            responseH=spi.write(0x00);
+            responseH=spi.write(0x00);
+            responseL=spi.write(0x00);
+            calib_data=((responseH&11100000)>>3)|((responseL&00000011));
+        break;
+    }
+    deselect();
+    wait(0.01);
+    set_acc_scale(temp_scale);
+    return calib_data;
+} 
+
+/*-----------------------------------------------------------------------------------------------
+                                SPI SELECT AND DESELECT
+usage: enable and disable mpu6000 communication bus
+-----------------------------------------------------------------------------------------------*/
+void mpu6000_spi::select() {
+    //Set CS low to start transmission (interrupts conversion)
+    cs = 0;
+}
+void mpu6000_spi::deselect() {
+    //Set CS high to stop transmission (restarts conversion)
+    cs = 1;
+}
\ No newline at end of file
diff -r 000000000000 -r 2edbd561b520 MPU6000.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6000.h	Sat Mar 08 12:38:14 2014 +0000
@@ -0,0 +1,162 @@
+/*CODED by Bruno Alfano on 07/03/2014
+www.xene.it
+
+USAGE (example program):
+#include "mbed.h"
+#include "MPU6000.h"        //Include library
+SPI spi(p11, p12, p13);     //define the SPI (mosi, miso, sclk)
+mpu6000_spi imu(spi,p22);   //define the mpu6000 object
+int main(){
+    if(imu.init(1,BITS_DLPF_CFG_5HZ)){  //INIT the mpu6000
+        printf("\nCouldn't initialize MPU6000 via SPI!");
+    }    
+    wait(0.1);    
+    printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
+    wait(0.1);    
+    printf("\nGyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
+    wait(1);  
+    printf("\nAcc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
+    wait(0.1);    
+    while(1) {
+        myled = 1;
+        wait(0.3);
+        myled = 0;
+        wait(0.3);
+        printf("\nT=%.3f",imu.read_temp());
+        printf(" X=%.3f",imu.read_acc(0));  
+        printf(" Y=%.3f",imu.read_acc(1)); 
+        printf(" Z=%.3f",imu.read_acc(2));   
+        printf(" rX=%.3f",imu.read_rot(0));  
+        printf(" rY=%.3f",imu.read_rot(1)); 
+        printf(" rZ=%.3f",imu.read_rot(2));
+    }
+}
+*/
+
+
+#ifndef MPU6000_h
+#define MPU6000_h
+#include "mbed.h"
+
+
+class mpu6000_spi
+{
+    SPI& spi;
+    DigitalOut cs;
+    
+  public:
+    mpu6000_spi(SPI& _spi, PinName _cs);
+    bool init(int sample_rate_div,int low_pass_filter);
+    float read_acc(int axis);
+    float read_rot(int axis);
+    unsigned int set_gyro_scale(int scale);
+    unsigned int set_acc_scale(int scale);
+    int calib_acc(int axis);
+    float read_temp();
+    void select();
+    void deselect();
+    unsigned int whoami();
+    
+    float acc_divider;
+    float gyro_divider;
+    
+  private:
+    PinName _CS_pin;
+    PinName _SO_pin;
+    PinName _SCK_pin;
+    float _error;
+};
+
+#endif
+
+
+
+// MPU6000 registers
+#define MPUREG_XG_OFFS_TC 0x00
+#define MPUREG_YG_OFFS_TC 0x01
+#define MPUREG_ZG_OFFS_TC 0x02
+#define MPUREG_X_FINE_GAIN 0x03
+#define MPUREG_Y_FINE_GAIN 0x04
+#define MPUREG_Z_FINE_GAIN 0x05
+#define MPUREG_XA_OFFS_H 0x06
+#define MPUREG_XA_OFFS_L 0x07
+#define MPUREG_YA_OFFS_H 0x08
+#define MPUREG_YA_OFFS_L 0x09
+#define MPUREG_ZA_OFFS_H 0x0A
+#define MPUREG_ZA_OFFS_L 0x0B
+#define MPUREG_PRODUCT_ID 0x0C
+#define MPUREG_SELF_TEST_X 0x0D
+#define MPUREG_SELF_TEST_Y 0x0E
+#define MPUREG_SELF_TEST_Z 0x0F
+#define MPUREG_SELF_TEST_A 0x10
+#define MPUREG_XG_OFFS_USRH 0x13
+#define MPUREG_XG_OFFS_USRL 0x14
+#define MPUREG_YG_OFFS_USRH 0x15
+#define MPUREG_YG_OFFS_USRL 0x16
+#define MPUREG_ZG_OFFS_USRH 0x17
+#define MPUREG_ZG_OFFS_USRL 0x18
+#define MPUREG_SMPLRT_DIV 0x19
+#define MPUREG_CONFIG 0x1A
+#define MPUREG_GYRO_CONFIG 0x1B
+#define MPUREG_ACCEL_CONFIG 0x1C
+#define MPUREG_INT_PIN_CFG 0x37
+#define MPUREG_INT_ENABLE 0x38
+#define MPUREG_ACCEL_XOUT_H 0x3B
+#define MPUREG_ACCEL_XOUT_L 0x3C
+#define MPUREG_ACCEL_YOUT_H 0x3D
+#define MPUREG_ACCEL_YOUT_L 0x3E
+#define MPUREG_ACCEL_ZOUT_H 0x3F
+#define MPUREG_ACCEL_ZOUT_L 0x40
+#define MPUREG_TEMP_OUT_H 0x41
+#define MPUREG_TEMP_OUT_L 0x42
+#define MPUREG_GYRO_XOUT_H 0x43
+#define MPUREG_GYRO_XOUT_L 0x44
+#define MPUREG_GYRO_YOUT_H 0x45
+#define MPUREG_GYRO_YOUT_L 0x46
+#define MPUREG_GYRO_ZOUT_H 0x47
+#define MPUREG_GYRO_ZOUT_L 0x48
+#define MPUREG_USER_CTRL 0x6A
+#define MPUREG_PWR_MGMT_1 0x6B
+#define MPUREG_PWR_MGMT_2 0x6C
+#define MPUREG_BANK_SEL 0x6D
+#define MPUREG_MEM_START_ADDR 0x6E
+#define MPUREG_MEM_R_W 0x6F
+#define MPUREG_DMP_CFG_1 0x70
+#define MPUREG_DMP_CFG_2 0x71
+#define MPUREG_FIFO_COUNTH 0x72
+#define MPUREG_FIFO_COUNTL 0x73
+#define MPUREG_FIFO_R_W 0x74
+#define MPUREG_WHOAMI 0x75
+
+// Configuration bits MPU6000
+#define BIT_SLEEP 0x40
+#define BIT_H_RESET 0x80
+#define BITS_CLKSEL 0x07
+#define MPU_CLK_SEL_PLLGYROX 0x01
+#define MPU_CLK_SEL_PLLGYROZ 0x03
+#define MPU_EXT_SYNC_GYROX 0x02
+#define BITS_FS_250DPS              0x00
+#define BITS_FS_500DPS              0x08
+#define BITS_FS_1000DPS             0x10
+#define BITS_FS_2000DPS             0x18
+#define BITS_FS_2G                  0x00
+#define BITS_FS_4G                  0x08
+#define BITS_FS_8G                  0x10
+#define BITS_FS_16G                 0x18
+#define BITS_FS_MASK                0x18
+#define BITS_DLPF_CFG_256HZ_NOLPF2  0x00
+#define BITS_DLPF_CFG_188HZ         0x01
+#define BITS_DLPF_CFG_98HZ          0x02
+#define BITS_DLPF_CFG_42HZ          0x03
+#define BITS_DLPF_CFG_20HZ          0x04
+#define BITS_DLPF_CFG_10HZ          0x05
+#define BITS_DLPF_CFG_5HZ           0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
+#define BITS_DLPF_CFG_MASK          0x07
+#define BIT_INT_ANYRD_2CLEAR        0x10
+#define BIT_RAW_RDY_EN              0x01
+#define BIT_I2C_IF_DIS              0x10
+
+#define READ_FLAG   0x80
+
+