mpu6000.cpp

00001 // MPU6000 MBED library developed for swimmers
00002 
00003 #include "mbed.h"
00004 #include "mpu6000.h"
00005 
00006 // Constructor
00007 MPU6000::MPU6000(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
00008 
00009 // Function that writes data to register
00010 void MPU6000::regWrite(uint8_t ADDR, uint8_t DATA)
00011 {
00012     // CS low -> start communicating
00013     cs = 0;
00014     // Send register address first
00015     spi.write(ADDR);
00016     // Send value to be written
00017     spi.write(DATA);
00018     // Wait a bit, since chip select is only software defined
00019     wait_us(1);
00020     // CS high -> stop communicating
00021     cs = 1;  
00022 }
00023 
00024 // Function that reads data from register
00025 uint8_t MPU6000::regRead(uint8_t ADDR)
00026 {
00027     // CS low -> start communicating
00028     cs = 0;
00029     // Write register address to read from ored with read flag
00030     spi.write((ADDR | READ));
00031     // Write dummy data to get respond clocked out
00032     int DATA = spi.write(0x00);
00033     // Wait a bit
00034     wait_us(1);
00035     // CS high -> stop communicating
00036     cs = 1;   
00037     // Return what has been read
00038     return DATA;
00039 }
00040 
00041 // Initializes MPU6000
00042 uint8_t MPU6000::init()
00043 {
00044     // Set chipselect high
00045     cs = 1;
00046     // Set spi bit length and format (use mode 3)
00047     spi.format(8, 3);
00048     // Set spi frquency to 1MHz (1MHz is max of MPU6000)
00049     spi.frequency(1000000);  
00050     // Disable I2C first
00051     regWrite(USER_CTRL, 0x10);
00052     // Reboot
00053     regWrite(PWR_MGMT_1, 0x80);
00054     // Wait a bit for MPU to reset
00055     wait_ms(150);
00056     // Read WHOAMI register
00057     uint8_t name = regRead(WHO_AM_I);
00058     // Select gyro pll for clk (manufacturers recommendation)
00059     regWrite(PWR_MGMT_1, 0x03);
00060     // Disable I2C again (after reset registers clear themselves)
00061     regWrite(USER_CTRL, 0x10);
00062     // Set gyro full scale range to 500 dps (degrees per second)
00063     regWrite(GYRO_CONFIG, 0x10); 
00064     // Set gyro full scale range to +- 4g
00065     regWrite(ACCEL_CONFIG, 0x08); 
00066     // Wait a bit
00067     wait_ms (150);
00068     // Return name
00069     return name;
00070 }
00071 
00072 // Reads current ACC values and stores them to provided buffer
00073 void MPU6000::valRead(bool device, short int &x, short int &y, short int &z)
00074 {
00075     uint8_t bufHigh, bufLow;
00076     // Switch based on selected device
00077     switch(device)
00078     {
00079         // Handle accelerometer selection
00080         case ACCEL:
00081             {
00082                 // Wait a bit
00083                 wait_us(5);
00084                 // CS low -> start communicating
00085                 cs = 0;
00086                 // Write ACC_X high byte address, rest of values will come until we remain clocking -> burst mode
00087                 spi.write(ACCEL_XOUT_H | READ);
00088                 // ACC_X high byte
00089                 bufHigh = spi.write(0x00);
00090                 // ACC_X low byte
00091                 bufLow = spi.write(0x00);
00092                 // Merge ACC_X and add it to buffer
00093                 x = (short int)((bufHigh << 8) | bufLow);
00094                 // ACC_Y high byte
00095                 bufHigh = spi.write(0x00);
00096                 // ACC_Y low byte
00097                 bufLow = spi.write(0x00);
00098                 // Merge ACC_Y and add it to buffer
00099                 y = (short int)((bufHigh << 8) | bufLow); 
00100                 // ACC_Z high byte
00101                 bufHigh = spi.write(0x00);
00102                 // ACC_Y low byte
00103                 bufLow = spi.write(0x00);
00104                 // Merge ACC_Z and add it to buffer
00105                 z = (short int)((bufHigh << 8) | bufLow);
00106                 // CS high -> stop communicating
00107                 cs = 1;
00108                 break;
00109             }
00110         // Handle gyro selection
00111         case GYRO:
00112             {
00113                 // Wait a bit
00114                 wait_us(5);
00115                 // CS low -> start communicating
00116                 cs = 0;
00117                 // Write GYRO_X high byte address, rest of values will come until we remain clocking -> burst mode
00118                 spi.write(GYRO_XOUT_H | READ);
00119                 // GYRO_X high byte
00120                 bufHigh = spi.write(0x00);
00121                 // GYRO_X low byte
00122                 bufLow = spi.write(0x00);
00123                 // Merge GYRO_X and add it to buffer
00124                 x = (short int)((bufHigh << 8) | bufLow);
00125                 // GYRO_Y high byte
00126                 bufHigh = spi.write(0x00);
00127                 // GYRO_Y low byte
00128                 bufLow = spi.write(0x00);
00129                 // Merge GYRO_Y and add it to buffer
00130                 y = (short int)((bufHigh << 8) | bufLow); 
00131                 // GYRO_Z high byte
00132                 bufHigh = spi.write(0x00);
00133                 // GYRO_Z low byte
00134                 bufLow = spi.write(0x00);
00135                 // Merge GYRO_Z and add it to buffer
00136                 z = (short int)((bufHigh << 8) | bufLow);
00137                 // CS high -> stop communicating
00138                 cs = 1;
00139                 break;
00140             }
00141     }
00142 }