Diff: MPU9250.cpp

Revision:

0:768d2e151834

Child:

1:f738165e54f0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.cpp	Sat Jun 21 11:52:23 2014 +0000
@@ -0,0 +1,336 @@
+/*CODED by Qiyong Mu on 21/06/2014
+
+*/
+
+#include <mbed.h>
+#include <mpu9250.h>
+
+mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
+
+unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
+{
+    unsigned int temp_val;
+    select();
+    spi.write(WriteAddr);
+    temp_val=spi.write(WriteData);
+    deselect();
+    wait_us(50);
+    return temp_val;
+}
+void mpu9250_spi::ReadRegs( unsigned int ReadAddr, unsigned int *ReadBuf, unsigned int Bytes )
+{
+    unsigned int  i = 0;
+
+    select();
+    spi.write(ReadAddr | READ_FLAG);
+    for(i=0; i<Bytes; i++)
+        ReadBuf[i] = spi.write(0x00);
+    deselect();
+    wait_us(50);
+}
+
+/*
+void mpu9250_spi::ReadReg( u8 ReadAddr, u8 *ReadData )
+{
+    select();
+    response=spi.write(MPUREG_USER_CTRL);
+    response=spi.write(BIT_I2C_IF_DIS);
+    deselect();
+  IMU_CSM = 0;
+  SPI_WriteByte(SPIx, 0x80 | ReadAddr);
+  *ReadData = SPI_ReadByte(SPIx);
+  IMU_CSM = 1;
+}
+*/
+/*-----------------------------------------------------------------------------------------------
+                                    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
+-----------------------------------------------------------------------------------------------*/
+#define MPU_InitRegNum 10
+
+bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
+    uint8_t i = 0;
+    uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
+        {0x80, MPUREG_PWR_MGMT_1},     // Reset Device
+        {0x01, MPUREG_PWR_MGMT_1},     // Clock Source
+        {0x00, MPUREG_PWR_MGMT_2},     // Enable Acc & Gyro
+        {0x07, MPUREG_CONFIG},         //
+        {0x18, MPUREG_GYRO_CONFIG},    // +-2000dps
+        {0x08, MPUREG_ACCEL_CONFIG},   // +-4G
+        {0x00, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates
+        {0x30, MPUREG_INT_PIN_CFG},    //
+        {0x40, MPUREG_I2C_MST_CTRL},   // I2C Speed 348 kHz
+        {0x20, MPUREG_USER_CTRL},      // Enable AUX
+    };
+    spi.format(8,0);
+    spi.frequency(1000000);
+
+    for(i=0; i<MPU_InitRegNum; i++) {
+        WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
+    }
+    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 mpu9250_spi::set_acc_scale(int scale){
+    unsigned int temp_scale;
+    WriteReg(MPUREG_ACCEL_CONFIG, scale);
+    
+    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;   
+    }
+    temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
+    
+    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 mpu9250_spi::set_gyro_scale(int scale){
+    unsigned int temp_scale;
+    WriteReg(MPUREG_GYRO_CONFIG, scale);
+    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;   
+    }
+    temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
+    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
+mpu9250 which should be 104 when in SPI mode.
+returns the I2C address (104)
+-----------------------------------------------------------------------------------------------*/
+unsigned int mpu9250_spi::whoami(){
+    unsigned int response;
+    response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
+    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 mpu9250_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 mpu9250_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 mpu9250_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 mpu9250_spi::calib_acc(int axis){
+    uint8_t responseH,responseL,calib_data;
+    int temp_scale;
+    //READ CURRENT ACC SCALE
+    temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
+    set_acc_scale(BITS_FS_8G);
+    //ENABLE SELF TEST
+    temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
+
+    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 mpu9250 communication bus
+-----------------------------------------------------------------------------------------------*/
+void mpu9250_spi::select() {
+    //Set CS low to start transmission (interrupts conversion)
+    cs = 0;
+}
+void mpu9250_spi::deselect() {
+    //Set CS high to stop transmission (restarts conversion)
+    cs = 1;
+}
\ No newline at end of file