Diff: MPU9250.cpp

Revision:

4:79185409730f

Parent:

3:f4fa24cc247d

Child:

5:f15d1d9d1561

--- a/MPU9250.cpp	Wed Jun 25 15:41:05 2014 +0000
+++ b/MPU9250.cpp	Thu Jun 26 13:03:12 2014 +0000
@@ -47,7 +47,7 @@
 BITS_DLPF_CFG_2100HZ_NOLPF
 returns 1 if an error occurred
 -----------------------------------------------------------------------------------------------*/
-#define MPU_InitRegNum 10
+#define MPU_InitRegNum 14
 
 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
     uint8_t i = 0;
@@ -60,8 +60,15 @@
         {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
+        //{0x40, MPUREG_I2C_MST_CTRL},   // I2C Speed 348 kHz
+        //{0x20, MPUREG_USER_CTRL},      // Enable AUX
+        {0xCD, MPUREG_I2C_MST_CTRL}, //  Enables multi-master  IIC 400KHz
+        {0x30, MPUREG_USER_CTRL},       // Enable AUX and make SPI only    
+        {AK8963_I2C_ADDR|READ_FLAG, MPUREG_I2C_SLV0_ADDR},  //Set the I2C slave addres of AK8963 and set for read.
+        {AK8963_WIA, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
+        {0x81, MPUREG_I2C_SLV0_CTRL},  //Enable I2C and set bytes
+        //{0x09, MPUREG_I2C_SLV4_CTRL},
+        {0x81, MPUREG_I2C_MST_DELAY_CTRL} //Enable I2C delay
     };
     spi.format(8,0);
     spi.frequency(1000000);
@@ -69,6 +76,11 @@
     for(i=0; i<MPU_InitRegNum; i++) {
         WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
     }
+    
+    //set_acc_scale(2);
+    //set_gyro_scale(250);
+    Magnetometer_divider=2;
+    
     return 0;
 }
 
@@ -185,28 +197,20 @@
 0 -> X axis
 1 -> Y axis
 2 -> Z axis
-returns the value in Gs
 -----------------------------------------------------------------------------------------------*/
 void mpu9250_spi::read_acc()
 {
-    uint8_t response[2];
+    uint8_t response[6];
     int16_t bit_data;
     float data;
-
-    ReadRegs(MPUREG_ACCEL_XOUT_H,response,2);
-    bit_data=((int16_t)response[0]<<8)|response[1];
-    data=(float)bit_data;
-    accelerometer_data[0]=data/acc_divider;
-
-    ReadRegs(MPUREG_ACCEL_YOUT_H,response,2);
-    bit_data=((int16_t)response[0]<<8)|response[1];
-    data=(float)bit_data;
-    accelerometer_data[1]=data/acc_divider;
-
-    ReadRegs(MPUREG_ACCEL_ZOUT_H,response,2);
-    bit_data=((int16_t)response[0]<<8)|response[1];
-    data=(float)bit_data;
-    accelerometer_data[2]=data/acc_divider;
+    int i;
+    ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
+    for(i=0; i<3; i++) {
+        bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+        data=(float)bit_data;
+        accelerometer_data[i]=data/acc_divider;
+    }
+    
 }
 
 /*-----------------------------------------------------------------------------------------------
@@ -215,28 +219,20 @@
 0 -> X axis
 1 -> Y axis
 2 -> Z axis
-returns the value in Degrees per second
 -----------------------------------------------------------------------------------------------*/
 void mpu9250_spi::read_rot()
 {
-    uint8_t response[2];
+    uint8_t response[6];
     int16_t bit_data;
     float data;
-
-    ReadRegs(MPUREG_GYRO_XOUT_H,response,2);
-    bit_data=((int16_t)response[0]<<8)|response[1];
-    data=(float)bit_data;
-    gyroscope_data[0]=data/gyro_divider;
+    int i;
+    ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
+    for(i=0; i<3; i++) {
+        bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+        data=(float)bit_data;
+        gyroscope_data[i]=data/gyro_divider;
+    }
 
-    ReadRegs(MPUREG_GYRO_YOUT_H,response,2);
-    bit_data=((int16_t)response[0]<<8)|response[1];
-    data=(float)bit_data;
-    gyroscope_data[1]=data/gyro_divider;
-
-    ReadRegs(MPUREG_GYRO_ZOUT_H,response,2);
-    bit_data=((int16_t)response[0]<<8)|response[1];
-    data=(float)bit_data;
-    gyroscope_data[2]=data/gyro_divider;
 }
 
 /*-----------------------------------------------------------------------------------------------
@@ -281,6 +277,28 @@
 
     set_acc_scale(temp_scale);
 }
+unsigned int mpu9250_spi::AK8963_whoami(){
+    unsigned int response;
+    WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
+    WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
+    response=WriteReg(MPUREG_EXT_SENS_DATA_00, 0x00);    //Read I2C 
+    wait(0.1);
+    response=WriteReg(MPUREG_EXT_SENS_DATA_00, 0x00);    //Read I2C 
+
+    return response;
+}
+void mpu9250_spi::AK8963_read_Magnetometer(){
+    uint8_t response[6];
+    int16_t bit_data;
+    float data;
+    WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
+    WriteReg(MPUREG_I2C_SLV0_CTRL, 0x86);    //Enable I2C and set bytes
+
+    ReadRegs(MPUREG_EXT_SENS_DATA_00,response,6);
+    bit_data=((int16_t)response[0]<<8)|response[1];
+    data=(float)bit_data;
+    Magnetometer[0]=data/Magnetometer_divider;
+}
 
 /*-----------------------------------------------------------------------------------------------
                                 SPI SELECT AND DESELECT