Diff: MPU9250.cpp

Revision:

5:f15d1d9d1561

Parent:

4:79185409730f

Child:

6:c82c48348f8c

diff -r 79185409730f -r f15d1d9d1561 MPU9250.cpp
--- a/MPU9250.cpp	Thu Jun 26 13:03:12 2014 +0000
+++ b/MPU9250.cpp	Sun Jun 29 06:38:21 2014 +0000
@@ -47,7 +47,7 @@
 BITS_DLPF_CFG_2100HZ_NOLPF
 returns 1 if an error occurred
 -----------------------------------------------------------------------------------------------*/
-#define MPU_InitRegNum 14
+#define MPU_InitRegNum 17
 
 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
     uint8_t i = 0;
@@ -62,19 +62,28 @@
         {0x30, MPUREG_INT_PIN_CFG},    //
         //{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
+        {0x20, MPUREG_USER_CTRL},       // I2C Master mode
+        {0x0D, MPUREG_I2C_MST_CTRL}, //  I2C configuration multi-master  IIC 400KHz
+        
+        {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR},  //Set the I2C slave addres of AK8963 and set for write.
         //{0x09, MPUREG_I2C_SLV4_CTRL},
-        {0x81, MPUREG_I2C_MST_DELAY_CTRL} //Enable I2C delay
+        //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
+        {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
+        {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
+        {0x81, MPUREG_I2C_SLV0_CTRL},  //Enable I2C and set 1 byte
+        
+        {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
+        {0x02, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 14bit
+        {0x81, MPUREG_I2C_SLV0_CTRL}  //Enable I2C and set 1 byte
+
+        
     };
     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]);
+        wait(0.001);  //I2C must slow down the write speed, otherwise it won't work
     }
     
     //set_acc_scale(2);
@@ -279,25 +288,35 @@
 }
 unsigned int mpu9250_spi::AK8963_whoami(){
     unsigned int response;
+    WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
     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 
+    WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
+
+    //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
     wait(0.1);
     response=WriteReg(MPUREG_EXT_SENS_DATA_00, 0x00);    //Read I2C 
+    //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
 
     return response;
 }
 void mpu9250_spi::AK8963_read_Magnetometer(){
-    uint8_t response[6];
+    uint8_t response[7];
     int16_t bit_data;
     float data;
+    int i;
+
+    WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
     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
+    WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
 
-    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;
+    wait(0.01);
+    ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
+    //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
+    for(i=0; i<3; i++) {
+        bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+        data=(float)bit_data;
+        Magnetometer[i]=data/Magnetometer_divider;
+    }
 }
 
 /*-----------------------------------------------------------------------------------------------