Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: BLE_Nano_MPU6050Service
Diff: MPU6050.cpp
- Revision:
- 2:32b13cc64cb0
- Parent:
- 1:96a227d1ca7e
- Child:
- 3:a6e53ab2c8c0
--- 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