Diff: MPU9250.cpp

Revision:

11:084e8ba240c1

Parent:

10:d27b3298e9e0

--- a/MPU9250.cpp	Tue Jul 01 11:09:17 2014 +0000
+++ b/MPU9250.cpp	Tue Jul 01 13:59:45 2014 +0000
@@ -55,7 +55,7 @@
         {0x80, MPUREG_PWR_MGMT_1},     // Reset Device
         {0x01, MPUREG_PWR_MGMT_1},     // Clock Source
         {0x00, MPUREG_PWR_MGMT_2},     // Enable Acc & Gyro
-        {0x01, MPUREG_CONFIG},         // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
+        {low_pass_filter, MPUREG_CONFIG},         // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
         {0x18, MPUREG_GYRO_CONFIG},    // +-2000dps
         {0x08, MPUREG_ACCEL_CONFIG},   // +-4G
         {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
@@ -68,14 +68,14 @@
         {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
+
         {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
         {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
         {0x81, MPUREG_I2C_SLV0_CTRL}  //Enable I2C and set 1 byte
-
         
     };
     spi.format(8,0);
@@ -85,14 +85,13 @@
         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);
+    set_gyro_scale(250);
     
-    //set_acc_scale(2);
-    //set_gyro_scale(250);
-    Magnetometer_Sensitivity_Scale_Factor=0.6;
-    
+    //AK8963_calib_Magnetometer();  //Can't load this function here , strange problem?
     return 0;
 }
-
 /*-----------------------------------------------------------------------------------------------
                                 ACCELEROMETER SCALE
 usage: call this function at startup, after initialization, to set the right range for the
@@ -287,18 +286,38 @@
     set_acc_scale(temp_scale);
 }
 uint8_t mpu9250_spi::AK8963_whoami(){
-    uint8_t response[2];
+    uint8_t 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, 0x82); //Read 1 byte from the magnetometer
+    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.001);
+    response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00);    //Read I2C 
+    //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
+    //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
+
+    return response;
+}
+void mpu9250_spi::AK8963_calib_Magnetometer(){
+    uint8_t response[3];
+    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_ASAX); //I2C slave 0 register address from where to begin data transfer
+    WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
 
     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
     wait(0.001);
     //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00);    //Read I2C 
-    ReadRegs(MPUREG_EXT_SENS_DATA_00,response,2);
+    ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
+    
     //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
-
-    return response[0];
+    for(i=0; i<3; i++) {
+        data=response[i];
+        Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
+    }
 }
 void mpu9250_spi::AK8963_read_Magnetometer(){
     uint8_t response[7];
@@ -316,7 +335,7 @@
     for(i=0; i<3; i++) {
         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
         data=(float)bit_data;
-        Magnetometer[i]=data*Magnetometer_Sensitivity_Scale_Factor;
+        Magnetometer[i]=data*Magnetometer_ASA[i];
     }
 }
 void mpu9250_spi::read_all(){
@@ -353,7 +372,7 @@
     for(i=7; i<10; i++) {
         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
         data=(float)bit_data;
-        Magnetometer[i-7]=data*Magnetometer_Sensitivity_Scale_Factor;
+        Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
     }
 }