MPU9250.cpp

00001 #include "MPU9250.h"
00002 
00003 MPU9250::MPU9250(PinName MOSI, PinName MISO, PinName SCLK, PinName CS) : spi(MOSI, MISO, SCLK), cs(CS) {
00004     deselect();                         // chip must be deselected first
00005     spi.format(8,0);                    // setup the spi for standard 8 bit data and SPI-Mode 0
00006     spi.frequency(5e6);                 // with a 5MHz clock rate
00007     
00008     /*
00009     last 3 Bits of|Accelerometer(Fs=1kHz) |Gyroscope 
00010     MPU9250_CONFIG|Bandwidth(Hz)|Delay(ms)|Bandwidth(Hz)|Delay(ms)|Fs(kHz)
00011     ------------------------------------------------------------------------- 
00012     0             |260          |0        |256          |0.98     |8 
00013     1             |184          |2.0      |188          |1.9      |1 
00014     2             |94           |3.0      |98           |2.8      |1 
00015     3             |44           |4.9      |42           |4.8      |1 
00016     4             |21           |8.5      |20           |8.3      |1 
00017     5             |10           |13.8     |10           |13.4     |1 
00018     6             |5            |19.0     |5            |18.6     |1 
00019     */
00020     writeRegister8(MPU9250_CONFIG, 0x00);
00021     writeRegister8(MPU9250_GYRO_CONFIG, 0x18);              // scales gyros range to +-2000dps
00022     writeRegister8(MPU9250_ACCEL_CONFIG, 0x08);             // scales accelerometers range to +-4g
00023 }
00024 
00025 uint8_t MPU9250::getWhoami() {
00026     return readRegister8(MPU9250_WHO_AM_I);
00027 }
00028 
00029 float MPU9250::getTemperature() {
00030     int16_t data = readRegister16(MPU9250_TEMP_OUT_H);
00031     return ((data - 21) / 333.87) + 21;                     // formula from register map p.33
00032 }
00033 
00034 void MPU9250::readGyro() {
00035     int16_t rawGyro[3];
00036     readRegister48(MPU9250_GYRO_XOUT_H, rawGyro);
00037     
00038     int16_t offsetGyro[3] = {-31, -16, -12};                // TODO: make better calibration
00039     
00040     for (int i = 0; i < 3; i++)
00041         Gyro[i] = (rawGyro[i] - offsetGyro[i]) * 0.07 * 0.87;        // subtract offset from calibration and multiply unit factor to get degree per second (datasheet p.10)
00042         
00043 
00044 
00045     float tmp = Gyro[0];
00046     Gyro[0] = -Gyro[1];
00047     Gyro[1] = -tmp;
00048     Gyro[2] = -Gyro[2];
00049 }
00050 
00051 void MPU9250::readAcc() {
00052     int16_t rawAcc[3];
00053     readRegister48(MPU9250_ACCEL_XOUT_H, rawAcc);
00054     
00055     int16_t offsetAcc[3] = {-120, -48, -438};                       // TODO: make better calibration
00056     
00057     for (int i = 0; i < 3; i++)
00058         Acc[i] = (rawAcc[i] - offsetAcc[i])/8192.0;                // TODO: didn't care about units because IMU-algorithm just uses vector direction
00059         
00060         
00061         
00062     float tmp = Acc[0];
00063     Acc[0] = -Acc[1];
00064     Acc[1] = -tmp;
00065     Acc[2] = -Acc[2];
00066 }
00067 
00068 // PRIVATE Methods ------------------------------------------------------------------------------------
00069 
00070 
00071 // SPI Interface --------------------------------------------------------------------------------------
00072 uint8_t MPU9250::readRegister8(uint8_t reg) {
00073     uint8_t result;
00074     readRegister(reg, &result, 1);
00075     return result;
00076 }
00077 
00078 uint16_t MPU9250::readRegister16(uint8_t reg) {
00079     uint8_t result[2];
00080     readRegister(reg, result, 2);
00081     return result[0]<<8 | result[1];                        // join 8-Bit pieces to 16-bit short integer
00082 }
00083 
00084 void MPU9250::readRegister48(uint8_t reg, int16_t *buffer) {
00085     uint8_t result[6];
00086     readRegister(reg, result, 6);
00087     buffer[0] = (int16_t) (result[0] << 8 | result[1]);     // join 8-Bit pieces to 16-bit short integers
00088     buffer[1] = (int16_t) (result[2] << 8 | result[3]);
00089     buffer[2] = (int16_t) (result[4] << 8 | result[5]);
00090 }
00091 
00092 void MPU9250::writeRegister8(uint8_t reg, uint8_t buffer) {
00093     writeRegister(reg, &buffer, 1);
00094 }
00095 
00096 void MPU9250::readRegister(uint8_t reg, uint8_t *buffer, int length) {
00097     select();
00098     spi.write(reg | 0x80);                                  // send the register address we want to read and the read flag
00099     for(int i=0; i<length; i++)                             // get data
00100         buffer[i] = spi.write(0x00);
00101     deselect();
00102 }
00103 
00104 void MPU9250::writeRegister(uint8_t reg, uint8_t *buffer, int length) {
00105     select();
00106     spi.write(reg & ~0x80);                                 // send the register address we want to write and the write flag
00107     for(int i=0; i<length; i++)                             // put data
00108         spi.write(buffer[i]);
00109     deselect();
00110 }
00111 
00112 void MPU9250::select() { cs = 0; }                          // set Cable Select pin low to start SPI transaction
00113 void MPU9250::deselect() { cs = 1; }                        // set Cable Select pin high to stop SPI transaction