Diff: MPU6050.cpp

Revision:

15:09f072efa71e

Parent:

13:a74f2d622b54

Child:

16:6be55d010301

--- a/MPU6050.cpp	Wed Mar 28 21:06:57 2018 +0000
+++ b/MPU6050.cpp	Wed Mar 28 21:23:40 2018 +0000
@@ -3509,11 +3509,25 @@
     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);    
 }
 
+/** Disable MPU-6050 standby mode. Really only used for MPU-9250
+ * @param 
+ * @see 
+ * @see MPU60X0_RA_PWR_MGMT_2
+ * @see 
+ */
+void MPU6050::setStandbyDisable() {
+    i2Cdev.writeByte(devAddr, MPU60X0_RA_PWR_MGMT_2, 0x00);
+}
+
 void MPU6050::initialize9250MasterMode(){
     
     uint8_t buff[3];
@@ -3594,7 +3608,7 @@
     if(  buff[0] != 72 ){
         debugSerial.printf("%d", buff[0]);
         debugSerial.printf(", ");
-        debugSerial.println("AK does not match");
+        debugSerial.printf("AK does not match\n");
     }
 
     /* get the magnetometer calibration */
@@ -3722,13 +3736,13 @@
         if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
             return -1;
         }
-        delay(100); // long wait between AK8963 mode changes  
+        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) ){
             return -1;
         }
-        delay(100); // long wait between AK8963 mode changes     
+        Thread::wait(100); // long wait between AK8963 mode changes     
 
         // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
         readAKRegisters(AK8963_RA_HXL,sizeof(data),&data[0]);
@@ -3877,12 +3891,12 @@
 
 }
 
-void MPU60X0::readRegister(uint8_t subAddress, uint8_t count, uint8_t* dest) {
+void MPU6050::readRegister(uint8_t subAddress, uint8_t count, uint8_t* dest) {
     i2Cdev.readBytes(devAddr, subAddress, count, dest);
 }
 
 /* writes a register to the AK8963 given a register address and data */
-bool MPU60X0::writeAKRegister(uint8_t subAddress, uint8_t data) {
+bool MPU6050::writeAKRegister(uint8_t subAddress, uint8_t data) {
     uint8_t count = 1;
     uint8_t buff[1];
     
@@ -3903,7 +3917,7 @@
 
 
 /* reads registers from the AK8963 */
-void MPU60X0::readAKRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest) {
+void MPU6050::readAKRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest) {
     
     writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, magDevAddr | I2C_READ_FLAG); // set slave 0 to the AK8963 and set for read
     writeRegister(MPU60X0_RA_I2C_SLV0_REG,  subAddress); // set the register to the desired AK8963 sub address
@@ -3913,7 +3927,7 @@
 
 }
 
-bool MPU60X0::writeRegister(uint8_t subAddress, uint8_t data) {
+bool MPU6050::writeRegister(uint8_t subAddress, uint8_t data) {
     uint8_t buff[1];
 
     i2Cdev.writeByte(devAddr, subAddress, data);