Diff: MPU6050.cpp

Revision:

2:32b13cc64cb0

Parent:

1:96a227d1ca7e

Child:

3:a6e53ab2c8c0

diff -r 96a227d1ca7e -r 32b13cc64cb0 MPU6050.cpp
--- a/MPU6050.cpp	Thu Jul 02 00:39:12 2015 +0000
+++ b/MPU6050.cpp	Mon Jul 13 15:23:35 2015 +0000
@@ -1,9 +1,5 @@
 #include "MPU6050.h"
 
-/**
- * Read and write registers
- */
-
 bool MPU6050::read(Register reg, uint8_t *data, size_t length, float timeout_secs)
 {  
     float t = 0.0f;
@@ -49,10 +45,6 @@
 {
     return this->write(reg, &data, 1, timeout_secs);
 }
-
-/**
- * AUX_VDDIO register
- */
  
 bool MPU6050::getAuxVDDIOLevel(AuxVDDIOLevel *level, float timeout_secs)
 {
@@ -72,10 +64,6 @@
     return (this->read(REG_AUX_VDDIO, &tmp, timeout_secs)) && (this->write(REG_AUX_VDDIO, (tmp & (~AUX_VDDIO_MASK)) | level, timeout_secs));
 }
 
-/**
- * REG_SMPLRT_DIV register
- */
-
 bool MPU6050::getGyroSampleRateDivider(uint8_t *rateDivider, float timeout_secs)
 {
     return this->read(REG_SMPLRT_DIV, rateDivider, timeout_secs);
@@ -86,10 +74,6 @@
     return this->write(REG_SMPLRT_DIV, rateDivider, timeout_secs);
 }
 
-/**
- * REG_CONFIG register
- */
-
 bool MPU6050::getExternalFrameSync(ExtFrameSync *frameSync, float timeout_secs)
 {    
     uint8_t tmp;
@@ -126,10 +110,6 @@
     return (this->read(REG_CONFIG, &tmp, timeout_secs)) && (this->write(REG_CONFIG, (tmp & (~DLPF_MASK)) | bandwith, timeout_secs));
 }
 
-/**
- * REG_GYRO_CONFIG register
- */  
-
 bool MPU6050::getGyroXSelfTestEnabled(bool *enabled, float timeout_secs)
 {
     uint8_t tmp;
@@ -145,10 +125,10 @@
 bool MPU6050::setGyroXSelfTestEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_GYRO_CONFIG, &tmp))
+    if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs))
     {
         tmp &= ~(GYRO_X_ST_MASK);
-        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_X_ST_MASK) : tmp);
+        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_X_ST_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -168,10 +148,10 @@
 bool MPU6050::setGyroYSelfTestEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_GYRO_CONFIG, &tmp))
+    if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs))
     {
         tmp &= ~(GYRO_Y_ST_MASK);
-        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Y_ST_MASK) : tmp);
+        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Y_ST_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -191,10 +171,10 @@
 bool MPU6050::setGyroZSelfTestEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_GYRO_CONFIG, &tmp))
+    if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs))
     {
         tmp &= ~(GYRO_Z_ST_MASK);
-        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Z_ST_MASK) : tmp);
+        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Z_ST_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -216,10 +196,6 @@
     uint8_t tmp;
     return (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs)) && (this->write(REG_GYRO_CONFIG, (tmp & (~GYRO_RANGE_MASK)) | gyroRange, timeout_secs));
 }
-
-/**
- * REG_ACCEL_CONFIG register
- */
  
 bool MPU6050::getAccelXSelfTestEnabled(bool *enabled, float timeout_secs)
 {
@@ -236,10 +212,10 @@
 bool MPU6050::setAccelXSelfTestEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_ACCEL_CONFIG, &tmp))
+    if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs))
     {
         tmp &= ~(ACCEL_X_ST_MASK);
-        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_X_ST_MASK) : tmp);
+        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_X_ST_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -259,10 +235,10 @@
 bool MPU6050::setAccelYSelfTestEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_ACCEL_CONFIG, &tmp))
+    if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs))
     {
         tmp &= ~(ACCEL_Y_ST_MASK);
-        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Y_ST_MASK) : tmp);
+        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Y_ST_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -282,10 +258,10 @@
 bool MPU6050::setAccelZSelfTestEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_ACCEL_CONFIG, &tmp))
+    if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs))
     {
         tmp &= ~(ACCEL_Z_ST_MASK);
-        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Z_ST_MASK) : tmp);
+        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Z_ST_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -325,10 +301,6 @@
     uint8_t tmp;
     return (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) && (this->write(REG_ACCEL_CONFIG, (tmp & (~ACCEL_RANGE_MASK)) | accelRange, timeout_secs));
 }
-
-/**
- * REG_FF_THR register
- */
         
 bool MPU6050::getFreefallDetectionThreshold(uint8_t *threshold, float timeout_secs)
 {
@@ -340,10 +312,6 @@
     return this->write(REG_FF_THR, threshold, timeout_secs);
 }
 
-/**
- * REG_FF_DUR register
- */
-
 bool MPU6050::getFreefallDetectionDuration(uint8_t *duration, float timeout_secs)
 {
     return this->read(REG_FF_DUR, duration, timeout_secs);
@@ -353,10 +321,6 @@
 {
     return this->write(REG_FF_DUR, duration, timeout_secs);
 }
-
-/**
- * REG_MOT_THR register
- */
         
 bool MPU6050::getMotionDetectionThreshold(uint8_t *threshold, float timeout_secs)
 {
@@ -368,10 +332,6 @@
     return this->write(REG_MOT_THR, threshold, timeout_secs);
 }
 
-/**
- * REG_MOT_DUR register
- */
-
 bool MPU6050::getMotionDetectionDuration(uint8_t *duration, float timeout_secs)
 {
     return this->read(REG_MOT_DUR, duration, timeout_secs);
@@ -381,10 +341,6 @@
 {
     return this->write(REG_MOT_DUR, duration, timeout_secs);
 }
-
-/**
- * REG_ZRMOT_THR register
- */
         
 bool MPU6050::getZeroMotionDetectionThreshold(uint8_t *threshold, float timeout_secs)
 {
@@ -396,10 +352,6 @@
     return this->write(REG_ZRMOT_THR, threshold, timeout_secs);
 }
 
-/**
- * REG_ZRMOT_DUR register
- */
-
 bool MPU6050::getZeroMotionDetectionDuration(uint8_t *duration, float timeout_secs)
 {
     return this->read(REG_ZRMOT_DUR, duration, timeout_secs);
@@ -409,10 +361,6 @@
 {
     return this->write(REG_ZRMOT_DUR, duration, timeout_secs);
 }
-
-/**
- * REG_FIFO_EN register
- */
  
 bool MPU6050::getTempFIFOEnabled(bool *enabled, float timeout_secs)
 {
@@ -429,10 +377,10 @@
 bool MPU6050::setTempFIFOEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_FIFO_EN, &tmp))
+    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
     {
         tmp &= ~(TEMP_FIFO_EN_MASK);
-        return this->write(REG_FIFO_EN, enabled ? (tmp | TEMP_FIFO_EN_MASK) : tmp);
+        return this->write(REG_FIFO_EN, enabled ? (tmp | TEMP_FIFO_EN_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -452,10 +400,10 @@
 bool MPU6050::setGyroXFIFOEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_FIFO_EN, &tmp))
+    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
     {
         tmp &= ~(GYRO_X_FIFO_EN_MASK);
-        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_X_FIFO_EN_MASK) : tmp);
+        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_X_FIFO_EN_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -475,10 +423,10 @@
 bool MPU6050::setGyroYFIFOEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_FIFO_EN, &tmp))
+    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
     {
         tmp &= ~(GYRO_Y_FIFO_EN_MASK);
-        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Y_FIFO_EN_MASK) : tmp);
+        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Y_FIFO_EN_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -498,10 +446,10 @@
 bool MPU6050::setGyroZFIFOEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_FIFO_EN, &tmp))
+    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
     {
         tmp &= ~(GYRO_Z_FIFO_EN_MASK);
-        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Z_FIFO_EN_MASK) : tmp);
+        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Z_FIFO_EN_MASK) : tmp, timeout_secs);
     }
     return false;
 }
@@ -521,18 +469,14 @@
 bool MPU6050::setAccelFIFOEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_FIFO_EN, &tmp))
+    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
     {
         tmp &= ~(ACCEL_FIFO_EN_MASK);
-        return this->write(REG_FIFO_EN, enabled ? (tmp | ACCEL_FIFO_EN_MASK) : tmp);
+        return this->write(REG_FIFO_EN, enabled ? (tmp | ACCEL_FIFO_EN_MASK) : tmp, timeout_secs);
     }
     return false;
 }
 
-/**
- * REG_INT_PIN_CFG register
- */
-
 bool MPU6050::getInterruptLevel(InterruptLevel *level, float timeout_secs)
 {    
     uint8_t tmp;
@@ -638,18 +582,14 @@
 bool MPU6050::setInterruptFSyncEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_INT_PIN_CFG, &tmp))
+    if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs))
     {
         tmp &= ~(INT_FSYNC_EN_MASK);
-        return this->write(REG_FIFO_EN, enabled ? (tmp | INT_FSYNC_EN_MASK) : tmp);
+        return this->write(REG_FIFO_EN, enabled ? (tmp | INT_FSYNC_EN_MASK) : tmp, timeout_secs);
     }
     return false;
 }
 
-/**
- * REG_INT_ENABLE register
- */
-
 bool MPU6050::getInterruptsEnabled(Interrupt *interruptSet, float timeout_secs)
 {
     uint8_t tmp;
@@ -682,10 +622,10 @@
 bool MPU6050::setInterruptFreefallEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_INT_ENABLE, &tmp))
+    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
     {
         tmp &= ~(INT_FREEFALL);
-        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FREEFALL) : tmp);
+        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FREEFALL) : tmp, timeout_secs);
     }
     return false;
 }
@@ -705,10 +645,10 @@
 bool MPU6050::setInterruptMotionEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_INT_ENABLE, &tmp))
+    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
     {
         tmp &= ~(INT_MOTION);
-        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_MOTION) : tmp);
+        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_MOTION) : tmp, timeout_secs);
     }
     return false;
 }
@@ -728,10 +668,10 @@
 bool MPU6050::setInterruptZeroMotionEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_INT_ENABLE, &tmp))
+    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
     {
         tmp &= ~(INT_ZERO_MOTION);
-        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_ZERO_MOTION) : tmp);
+        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_ZERO_MOTION) : tmp, timeout_secs);
     }
     return false;
 }
@@ -751,10 +691,10 @@
 bool MPU6050::setInterruptFIFOOverflowEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_INT_ENABLE, &tmp))
+    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
     {
         tmp &= ~(INT_FIFO_OVERFLOW);
-        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FIFO_OVERFLOW) : tmp);
+        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FIFO_OVERFLOW) : tmp, timeout_secs);
     }
     return false;
 }
@@ -774,10 +714,10 @@
 bool MPU6050::setInterruptDataReadyEnabled(bool enabled, float timeout_secs)
 {
     uint8_t tmp;
-    if (this->read(REG_INT_ENABLE, &tmp))
+    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
     {
         tmp &= ~(INT_DATA_READY);
-        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_DATA_READY) : tmp);
+        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_DATA_READY) : tmp, timeout_secs);
     }
     return false;
 }
@@ -853,25 +793,6 @@
     return false;
 }
 
-/**
- * REG_ACCEL_XOUT_H 
- * REG_ACCEL_XOUT_L 
- * REG_ACCEL_YOUT_H 
- * REG_ACCEL_YOUT_L 
- * REG_ACCEL_ZOUT_H 
- * REG_ACCEL_ZOUT_L 
- * REG_TEMP_OUT_H 
- * REG_TEMP_OUT_L 
- * REG_GYRO_XOUT_H 
- * REG_GYRO_XOUT_L 
- * REG_GYRO_YOUT_H 
- * REG_GYRO_YOUT_L
- * REG_GYRO_ZOUT_H  
- * REG_GYRO_ZOUT_L registers
- */
-         
-/** Acceleration */
-
 bool MPU6050::getAcceleration(int16_t *ax, int16_t *ay, int16_t *az, float timeout_secs)
 {
     uint8_t tmp[6];
@@ -923,8 +844,6 @@
     return false;
 }
 
-/** Temperature */
-
 bool MPU6050::getTemperature(int16_t *temp, float timeout_secs)
 {
     uint8_t tmp[2];
@@ -937,8 +856,6 @@
     return false;
 }
 
-/** Rotation speed */
-
 bool MPU6050::getRotationSpeed(int16_t *ox, int16_t *oy, int16_t *oz, float timeout_secs)
 {
     uint8_t tmp[6];
@@ -990,8 +907,6 @@
     return false;
 }
 
-/** All three */
-
 bool MPU6050::getMotionAndTemperature(int16_t *ax, int16_t *ay, int16_t *az, int16_t *ox, int16_t *oy, int16_t *oz, int16_t *temp, float timeout_secs)
 {
     uint8_t tmp[14];
@@ -1009,4 +924,142 @@
     }
     
     return false;
+}
+
+bool MPU6050::getMotionAndTemperature(MPU6050SensorReading *sensorReading, float timeout_secs)
+{  
+    uint8_t tmp;
+    #define SWAP(a, b) tmp = a; a = b; b = tmp;
+    
+    if (this->read(REG_GYRO_XOUT_H, (uint8_t*)&converter, sizeof(converter), timeout_secs))
+    {
+        SWAP(converter.reg.ax_h,    converter.reg.ax_l);
+        SWAP(converter.reg.ay_h,    converter.reg.ay_l);
+        SWAP(converter.reg.az_h,    converter.reg.az_l);
+        SWAP(converter.reg.temp_h,  converter.reg.temp_l);
+        SWAP(converter.reg.ox_h,    converter.reg.ox_l);
+        SWAP(converter.reg.oy_h,    converter.reg.oy_l);
+        SWAP(converter.reg.oz_h,    converter.reg.oz_l);
+        
+        *sensorReading = converter.value;
+        
+        return true;
+    }
+    
+    return false;
+    
+    #undef SWAP
+}
+
+bool MPU6050::getMotionStatus(Motion *motionSet, float timeout_secs)
+{    
+    uint8_t tmp;
+    if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs))
+    {
+        *motionSet = static_cast<Motion>(tmp);
+        return true;
+    }
+    return false;
+}
+
+bool MPU6050::getNegativeXMotionDetected(bool *detected, float timeout_secs)
+{
+    uint8_t tmp;
+    if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs))
+    {
+        *detected = ((tmp & MOT_NEGATIVE_X) != 0);
+        return true;
+    }
+    
+    return false;
+}
+
+bool MPU6050::getPositiveXMotionDetected(bool *detected, float timeout_secs)
+{
+    uint8_t tmp;
+    if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs))
+    {
+        *detected = ((tmp & MOT_POSITIVE_X) != 0);
+        return true;
+    }
+    
+    return false;
+}
+
+bool MPU6050::getNegativeYMotionDetected(bool *detected, float timeout_secs)
+{
+    uint8_t tmp;
+    if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs))
+    {
+        *detected = ((tmp & MOT_NEGATIVE_Y) != 0);
+        return true;
+    }
+    
+    return false;
+}
+
+bool MPU6050::getPositiveYMotionDetected(bool *detected, float timeout_secs)
+{
+    uint8_t tmp;
+    if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs))
+    {
+        *detected = ((tmp & MOT_POSITIVE_Y) != 0);
+        return true;
+    }
+    
+    return false;
+}
+
+bool MPU6050::getNegativeZMotionDetected(bool *detected, float timeout_secs)
+{
+    uint8_t tmp;
+    if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs))
+    {
+        *detected = ((tmp & MOT_NEGATIVE_Z) != 0);
+        return true;
+    }
+    
+    return false;
+}
+
+bool MPU6050::getPositiveZMotionDetected(bool *detected, float timeout_secs)
+{
+    uint8_t tmp;
+    if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs))
+    {
+        *detected = ((tmp & MOT_POSITIVE_Z) != 0);
+        return true;
+    }
+    
+    return false;
+}
+
+bool MPU6050::getZeroMotionDetected(bool *detected, float timeout_secs)
+{
+    uint8_t tmp;
+    if (this->read(REG_MOT_DETECT_STATUS, &tmp, timeout_secs))
+    {
+        *detected = ((tmp & MOT_ZERO) != 0);
+        return true;
+    }
+    
+    return false;
+}
+
+bool MPU6050::resetGyroscopeSignalPath(float timeout_secs)
+{
+    uint8_t tmp;
+    return (this->read(REG_SIGNAL_PATH_RESET, &tmp) && this->write(REG_SIGNAL_PATH_RESET, tmp | GYRO_PATH_RESET_MASK));
+}
+
+bool MPU6050::resetAccelerometerSignalPath(float timeout_secs)
+{
+    uint8_t tmp;
+    return (this->read(REG_SIGNAL_PATH_RESET, &tmp) && this->write(REG_SIGNAL_PATH_RESET, tmp | ACCEL_PATH_RESET_MASK));
+}
+
+bool MPU6050::resetTemperatureSignalPath(float timeout_secs)
+{
+    uint8_t tmp;
+    return (this->read(REG_SIGNAL_PATH_RESET, &tmp) && this->write(REG_SIGNAL_PATH_RESET, tmp | TEMP_PATH_RESET_MASK));
 }
\ No newline at end of file