Diff: MPU9250.cpp

Revision:

12:2b7b98cf595c

Parent:

11:084e8ba240c1

--- a/MPU9250.cpp	Tue Jul 01 13:59:45 2014 +0000
+++ b/MPU9250.cpp	Wed Feb 15 12:33:22 2017 +0000
@@ -256,7 +256,7 @@
 
     bit_data=((int16_t)response[0]<<8)|response[1];
     data=(float)bit_data;
-    Temperature=(data/340)+36.53;
+    //Temperature=(data/340)+36.53;
     deselect();
 }
 
@@ -308,7 +308,7 @@
     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
+    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,3);
@@ -316,7 +316,9 @@
     //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
     for(i=0; i<3; i++) {
         data=response[i];
-        Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
+        Magnetometer_ASA[i]=(((data-128)/256)+1)*Magnetometer_Sensitivity_Scale_Factor;
+        //Magnetometer[i]=data;
+        //Magnetometer_ASA[i]=data;
     }
 }
 void mpu9250_spi::AK8963_read_Magnetometer(){
@@ -336,6 +338,7 @@
         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
         data=(float)bit_data;
         Magnetometer[i]=data*Magnetometer_ASA[i];
+        //Magnetometer[i]=data;
     }
 }
 void mpu9250_spi::read_all(){
@@ -375,6 +378,51 @@
         Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
     }
 }
+void mpu9250_spi::readAngles(){
+    SensorTimer.start(); // start time for measuring sensors
+    //mpu.readGyro(); // reading sensor data
+    //mpu.readAcc();
+    //mpu2.read(); // reading sensor data
+//***************************************************
+    uint8_t response[21];
+    int16_t bit_data;
+    float data;
+    int i;
+
+    //Send I2C command at first
+    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, 0x87); //Read 7 bytes from the magnetometer
+    //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.
+
+    //wait(0.001);
+    ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
+    //Get accelerometer value
+    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;
+    }
+    //Get temperature
+    /*bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+    data=(float)bit_data;
+    Temperature=((data-21)/333.87)+21;*/
+    //Get gyroscop value
+    for(i=4; i<7; i++) {
+        bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+        data=(float)bit_data;
+        gyroscope_data[i-4]=data/gyro_divider;
+    }
+    //Get Magnetometer value
+    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_ASA[i-7];
+    }
+//*******************************************************
+
+    //Filter.compute(dt, mpu2.data_gyro, mpu2.data_acc, mpu2.data_acc);
+}
 
 /*-----------------------------------------------------------------------------------------------
                                 SPI SELECT AND DESELECT