Diff: MPU6050.cpp

Revision:

13:a74f2d622b54

Parent:

12:e32a6beb0a41

Child:

15:09f072efa71e

--- a/MPU6050.cpp	Tue Mar 27 22:31:21 2018 +0000
+++ b/MPU6050.cpp	Wed Mar 28 20:25:13 2018 +0000
@@ -40,7 +40,7 @@
 ===============================================
 */
 
-#include "MODI2C.h"
+#include "mbed.h"
 #include "MPU6050.h"
 #include "rtos.h"
 
@@ -1915,12 +1915,24 @@
  */
 uint32_t mpureadfin(uint32_t param){
     MPU6050* ins = (MPU6050*)param;
+#ifdef MPU9250
     ins->ax_cache = (((int16_t)ins->mpu_buffer[0]) << 8) | ins->mpu_buffer[1];
     ins->ay_cache = (((int16_t)ins->mpu_buffer[2]) << 8) | ins->mpu_buffer[3];
     ins->az_cache = (((int16_t)ins->mpu_buffer[4]) << 8) | ins->mpu_buffer[5];
     ins->gx_cache = (((int16_t)ins->mpu_buffer[8]) << 8) | ins->mpu_buffer[9];
     ins->gy_cache = (((int16_t)ins->mpu_buffer[10]) << 8) | ins->mpu_buffer[11];
     ins->gz_cache = (((int16_t)ins->mpu_buffer[12]) << 8) | ins->mpu_buffer[13];
+    ins->hx_cache = (((int16_t)ins->mpu_buffer[15]) << 8) | ins->mpu_buffer[14];
+    ins->hy_cache = (((int16_t)ins->mpu_buffer[17]) << 8) | ins->mpu_buffer[16];
+    ins->hz_cache = (((int16_t)ins->mpu_buffer[19]) << 8) | ins->mpu_buffer[18];
+#else
+    ins->ax_cache = (((int16_t)ins->mpu_buffer[0]) << 8) | ins->mpu_buffer[1];
+    ins->ay_cache = (((int16_t)ins->mpu_buffer[2]) << 8) | ins->mpu_buffer[3];
+    ins->az_cache = (((int16_t)ins->mpu_buffer[4]) << 8) | ins->mpu_buffer[5];
+    ins->gx_cache = (((int16_t)ins->mpu_buffer[8]) << 8) | ins->mpu_buffer[9];
+    ins->gy_cache = (((int16_t)ins->mpu_buffer[10]) << 8) | ins->mpu_buffer[11];
+    ins->gz_cache = (((int16_t)ins->mpu_buffer[12]) << 8) | ins->mpu_buffer[13];
+#endif
     return 0;
 }
 
@@ -1933,7 +1945,11 @@
 }
 
 void MPU6050::mpu_sample_func(){
+#ifdef MPU9250
+    i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 21, (uint8_t*)mpu_buffer, &mpureadfin, this);
+#else
     i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 14, (uint8_t*)mpu_buffer, &mpureadfin, this);
+#endif
 }
 
 void MPU6050::start_sampling(){
@@ -3480,6 +3496,7 @@
 // =============================================================================
 // =============================================================================
 
+#ifdef MPU9250
 
 /** initialize 9250.
  * 
@@ -3498,7 +3515,6 @@
 }
 
 void MPU6050::initialize9250MasterMode(){
-    #include "AK8963.h"
     
     uint8_t buff[3];
     uint8_t data[7];
@@ -3819,19 +3835,16 @@
 {
     uint8_t buff[21];
     int16_t axx, ayy, azz, gxx, gyy, gzz;
-    readRegister(MPU60X0_RA_ACCEL_XOUT_H, sizeof(buff), &buff[0]); // grab the data from the MPU9250
 
-    axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
-    ayy = (((int16_t)buff[2]) << 8) | buff[3];
-    azz = (((int16_t)buff[4]) << 8) | buff[5];
-
-    gxx = (((int16_t)buff[8]) << 8) | buff[9];
-    gyy = (((int16_t)buff[10]) << 8) | buff[11];
-    gzz = (((int16_t)buff[12]) << 8) | buff[13];
-
-    *hx = (((int16_t)buff[15]) << 8) | buff[14];  
-    *hy = (((int16_t)buff[17]) << 8) | buff[16];
-    *hz = (((int16_t)buff[19]) << 8) | buff[18];
+    axx = ax_cache;
+    ayy = ay_cache;
+    azz = az_cache;
+    gxx = gx_cache;
+    gyy = gy_cache;
+    gzz = gz_cache;
+    *hx = hx_cache;
+    *hy = hy_cache;
+    *hz = hz_cache;
 
     *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
     *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
@@ -3864,36 +3877,6 @@
 
 }
 
-/* get accelerometer, magnetometer, and temperature data given pointers to store values, return data as counts */
-void MPU6050::get9250Motion10Counts(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* hx, int16_t* hy, int16_t* hz, int16_t* t){
-    uint8_t buff[21];
-    int16_t axx, ayy, azz, gxx, gyy, gzz;
-
-    readRegister(MPU60X0_RA_ACCEL_XOUT_H, sizeof(buff), &buff[0]); // grab the data from the MPU9250
-
-    axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
-    ayy = (((int16_t)buff[2]) << 8) | buff[3];
-    azz = (((int16_t)buff[4]) << 8) | buff[5];
-
-    *t = (((int16_t)buff[6]) << 8) | buff[7];
-
-    gxx = (((int16_t)buff[8]) << 8) | buff[9];
-    gyy = (((int16_t)buff[10]) << 8) | buff[11];
-    gzz = (((int16_t)buff[12]) << 8) | buff[13];
-
-    *hx = (((int16_t)buff[15]) << 8) | buff[14];
-    *hy = (((int16_t)buff[17]) << 8) | buff[16];
-    *hz = (((int16_t)buff[19]) << 8) | buff[18];
-
-    *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
-    *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
-    *az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;
-
-    *gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz;
-    *gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
-    *gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
-}
-
 void MPU60X0::readRegister(uint8_t subAddress, uint8_t count, uint8_t* dest) {
     i2Cdev.readBytes(devAddr, subAddress, count, dest);
 }
@@ -3949,9 +3932,8 @@
     
 }
 
+#endif // MPU9250
 
 
 
 
-
-