Revision 12:e526897d15f8, committed 2017-07-25

Comitter:

d15321854

Date:

Tue Jul 25 09:13:00 2017 +0000

Parent:

11:084e8ba240c1

Commit message:

123

Changed in this revision

diff -r 084e8ba240c1 -r e526897d15f8 MPU9250.cpp
--- a/MPU9250.cpp	Tue Jul 01 13:59:45 2014 +0000
+++ b/MPU9250.cpp	Tue Jul 25 09:13:00 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

diff -r 084e8ba240c1 -r e526897d15f8 MPU9250.h
--- a/MPU9250.h	Tue Jul 01 13:59:45 2014 +0000
+++ b/MPU9250.h	Tue Jul 25 09:13:00 2017 +0000
@@ -7,7 +7,6 @@
 #define mpu9250_h
 #include "mbed.h"
 
-
 class mpu9250_spi
 {
     SPI& spi;
@@ -33,7 +32,7 @@
     uint8_t  AK8963_whoami();
     void AK8963_read_Magnetometer();
     void read_all();
-
+    void readAngles();
 
     
     float acc_divider;
@@ -42,16 +41,24 @@
     int calib_data[3];
     float Magnetometer_ASA[3];
 
+    
     float accelerometer_data[3];
     float Temperature;
     float gyroscope_data[3];
     float Magnetometer[3];
     
+    //float dt ;//= 0;
+    //float dt_sensors ;//= 0;
   private:
     PinName _CS_pin;
     PinName _SO_pin;
     PinName _SCK_pin;
     float _error;
+    
+    Timer LoopTimer;               // local time to calculate processing speed for entire loop
+    Timer SensorTimer;             // local time to calculate processing speed for just reading sensors
+    uint8_t Mscale;                 //= 10.*4912./32760.0;   10.*4912./32760.0;  MFS_16BITS
+    uint8_t Mmode ;                 //= 0x02; 
 };
 
 #endif