Diff: MPU6050.cpp

Revision:

21:ae3ee2d811eb

Parent:

20:0172ae63dd9e

--- a/MPU6050.cpp	Wed Mar 28 22:17:20 2018 +0000
+++ b/MPU6050.cpp	Thu Mar 29 22:06:56 2018 +0000
@@ -58,6 +58,10 @@
 {
     devAddr = MPU6050_DEFAULT_ADDRESS << 1;
     debugSerial.baud(115200);
+    
+    tX[0] = 0; tX[1] = 1; tX[2] = 0;
+    tY[0] = 1; tY[1] = 0; tY[2] = 0;
+    tZ[0] = 0; tZ[1] = 0; tZ[2] = -1;
 }
 
 /** Specific address constructor.
@@ -70,6 +74,10 @@
 {
     devAddr = address << 1;
     debugSerial.baud(115200);
+    
+    tX[0] = 0; tX[1] = 1; tX[2] = 0;
+    tY[0] = 1; tY[1] = 0; tY[2] = 0;
+    tZ[0] = 0; tZ[1] = 0; tZ[2] = -1;
 }
 
 /** Power on and prepare for general usage.
@@ -1948,9 +1956,18 @@
    sampling = on;
 }
 
-void MPU6050::mpu_sample_func(){
+#ifdef MPU9250
+volatile bool magn_valid_9250 = false;
+#endif
+
+void MPU6050::mpu_sample_func(){ 
 #ifdef MPU9250
     i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 21, (uint8_t*)mpu_buffer, &mpureadfin, this);
+    static uint32_t counter = 0;
+    counter += 1;
+    if (counter % 5 == 0) {
+        magn_valid_9250 = true;
+    }
 #else
     i2Cdev.readBytes_nb(devAddr, &mpu_cmd, 14, (uint8_t*)mpu_buffer, &mpureadfin, this);
 #endif
@@ -1959,6 +1976,7 @@
 void MPU6050::start_sampling(){
     sampling = true;
     mpu_cmd = MPU6050_RA_ACCEL_XOUT_H;
+    // sample at 500hz
     mpu_sampling.attach_us(this, &MPU6050::mpu_sample_func, 2000);
 }
 
@@ -3513,10 +3531,6 @@
     setClockSource(MPU60X0_CLOCK_PLL_XGYRO);
     setFullScaleGyroRange(MPU60X0_GYRO_FS_2000);
     setFullScaleAccelRange(MPU60X0_ACCEL_FS_2);
-    
-    tX[0] = 0; tX[1] = 1; tX[2] = 0;
-    tY[0] = 1; tY[1] = 0; tY[2] = 0;
-    tZ[0] = 0; tZ[1] = 0; tZ[2] = 1;  // was -1  transformation is done within lib.
 
     //data = 1000 / rate - 1;
     //setRate(data);    
@@ -3743,7 +3757,7 @@
         Thread::wait(100); // long wait between AK8963 mode changes  
 
         // set AK8963 to 16 bit resolution, 8 Hz update rate
-        if( !writeAKRegister(AK8963_RA_CNTL1,0x12) ){
+        if( !writeAKRegister(AK8963_RA_CNTL1, 0x12) ){
             return -1;
         }
         Thread::wait(100); // long wait between AK8963 mode changes