File content as of revision 1:96a227d1ca7e:

#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;
    
    uint8_t tmp = reg;    
    
    while (!(i2c.write(baseAddress, (char*)&tmp, 1, true) == 0))
        if ((t += retryDelay_secs) > timeout_secs)
            return false;
        else               
            wait(retryDelay_secs);
    
    while (!(i2c.read(baseAddress, (char*)data, length) == 0))
        if ((t += retryDelay_secs) > timeout_secs)
            return false;
        else               
            wait(retryDelay_secs);
            
    return true;
}

inline bool MPU6050::read(Register reg, uint8_t *data, float timeout_secs)
{
    return this->read(reg, data, 1, timeout_secs);
}

bool MPU6050::write(Register reg, uint8_t *data, size_t length, float timeout_secs)
{
    uint8_t tmp[length + 1];
    tmp[0] = reg;    
    memcpy(&tmp[1], data, length);    
    
    for (float t = 0.0f; !(i2c.write(baseAddress, (char*)&tmp[0], length + 1) == 0);)
        if ((t += retryDelay_secs) > timeout_secs)
            return false;
        else               
            wait(retryDelay_secs);
    
    return true;
}

inline bool MPU6050::write(Register reg, uint8_t data, float timeout_secs)
{
    return this->write(reg, &data, 1, timeout_secs);
}

/**
 * AUX_VDDIO register
 */
 
bool MPU6050::getAuxVDDIOLevel(AuxVDDIOLevel *level, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_AUX_VDDIO, &tmp, timeout_secs))
    {
        *level = static_cast<AuxVDDIOLevel>(tmp & AUX_VDDIO_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setAuxVDDIOLevel(AuxVDDIOLevel level, float timeout_secs)
{
    uint8_t tmp;
    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);
}

bool MPU6050::setGyroSampleRateDivider(uint8_t rateDivider, float timeout_secs)
{
    return this->write(REG_SMPLRT_DIV, rateDivider, timeout_secs);
}

/**
 * REG_CONFIG register
 */

bool MPU6050::getExternalFrameSync(ExtFrameSync *frameSync, float timeout_secs)
{    
    uint8_t tmp;
    if (this->read(REG_CONFIG, &tmp, timeout_secs))
    {
        *frameSync = static_cast<ExtFrameSync>(tmp & EXT_SYNC_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setExternalFrameSync(ExtFrameSync frameSync, float timeout_secs)
{ 
    uint8_t tmp;
    return (this->read(REG_CONFIG, &tmp, timeout_secs)) && (this->write(REG_CONFIG, (tmp & (~EXT_SYNC_MASK)) | frameSync, timeout_secs));
}

bool MPU6050::getDLPFBandwidth(DLPFBandwidth *bandwith, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_CONFIG, &tmp, timeout_secs))
    {
        *bandwith = static_cast<DLPFBandwidth>(tmp & DLPF_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setDLPFBandwidth(DLPFBandwidth bandwith, float timeout_secs)
{
    uint8_t tmp;
    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;
    if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs))
    {
        *enabled = ((tmp & GYRO_X_ST_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setGyroXSelfTestEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_GYRO_CONFIG, &tmp))
    {
        tmp &= ~(GYRO_X_ST_MASK);
        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_X_ST_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getGyroYSelfTestEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs))
    {
        *enabled = ((tmp & GYRO_Y_ST_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setGyroYSelfTestEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_GYRO_CONFIG, &tmp))
    {
        tmp &= ~(GYRO_Y_ST_MASK);
        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Y_ST_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getGyroZSelfTestEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs))
    {
        *enabled = ((tmp & GYRO_Z_ST_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setGyroZSelfTestEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_GYRO_CONFIG, &tmp))
    {
        tmp &= ~(GYRO_Z_ST_MASK);
        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Z_ST_MASK) : tmp);
    }
    return false;
}
 
bool MPU6050::getGyroRange(GyroRange *gyroRange, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_GYRO_CONFIG, &tmp, timeout_secs))
    {
        *gyroRange = static_cast<GyroRange>(tmp & GYRO_RANGE_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setGyroRange(GyroRange gyroRange, float timeout_secs)
{
    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)
{
    uint8_t tmp;
    if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs))
    {
        *enabled = ((tmp & ACCEL_X_ST_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setAccelXSelfTestEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_ACCEL_CONFIG, &tmp))
    {
        tmp &= ~(ACCEL_X_ST_MASK);
        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_X_ST_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getAccelYSelfTestEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs))
    {
        *enabled = ((tmp & ACCEL_Y_ST_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setAccelYSelfTestEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_ACCEL_CONFIG, &tmp))
    {
        tmp &= ~(ACCEL_Y_ST_MASK);
        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Y_ST_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getAccelZSelfTestEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs))
    {
        *enabled = ((tmp & ACCEL_Z_ST_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setAccelZSelfTestEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_ACCEL_CONFIG, &tmp))
    {
        tmp &= ~(ACCEL_Z_ST_MASK);
        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Z_ST_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getAccelHPFCutOff(AccelHPFCutOff *frequency, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs))
    {
        *frequency = static_cast<AccelHPFCutOff>(tmp & ACCEL_HPF_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setAccelHPFCutOff(AccelHPFCutOff frequency, float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs)) && (this->write(REG_ACCEL_CONFIG, (tmp & (~ACCEL_HPF_MASK)) | frequency, timeout_secs));
}
 
bool MPU6050::getAccelRange(AccelRange *accelRange, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_ACCEL_CONFIG, &tmp, timeout_secs))
    {
        *accelRange = static_cast<AccelRange>(tmp & ACCEL_RANGE_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setAccelRange(AccelRange accelRange, float timeout_secs)
{
    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)
{
    return this->read(REG_FF_THR, threshold, timeout_secs);
}

bool MPU6050::setFreefallDetectionThreshold(uint8_t threshold, float timeout_secs)
{
    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);
}

bool MPU6050::setFreefallDetectionDuration(uint8_t duration, float timeout_secs)
{
    return this->write(REG_FF_DUR, duration, timeout_secs);
}

/**
 * REG_MOT_THR register
 */
        
bool MPU6050::getMotionDetectionThreshold(uint8_t *threshold, float timeout_secs)
{
    return this->read(REG_MOT_THR, threshold, timeout_secs);
}

bool MPU6050::setMotionDetectionThreshold(uint8_t threshold, float timeout_secs)
{
    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);
}

bool MPU6050::setMotionDetectionDuration(uint8_t duration, float timeout_secs)
{
    return this->write(REG_MOT_DUR, duration, timeout_secs);
}

/**
 * REG_ZRMOT_THR register
 */
        
bool MPU6050::getZeroMotionDetectionThreshold(uint8_t *threshold, float timeout_secs)
{
    return this->read(REG_ZRMOT_THR, threshold, timeout_secs);
}

bool MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold, float timeout_secs)
{
    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);
}

bool MPU6050::setZeroMotionDetectionDuration(uint8_t duration, float timeout_secs)
{
    return this->write(REG_ZRMOT_DUR, duration, timeout_secs);
}

/**
 * REG_FIFO_EN register
 */
 
bool MPU6050::getTempFIFOEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
    {
        *enabled = ((tmp & TEMP_FIFO_EN_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setTempFIFOEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp))
    {
        tmp &= ~(TEMP_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | TEMP_FIFO_EN_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getGyroXFIFOEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
    {
        *enabled = ((tmp & GYRO_X_FIFO_EN_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setGyroXFIFOEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp))
    {
        tmp &= ~(GYRO_X_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_X_FIFO_EN_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getGyroYFIFOEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
    {
        *enabled = ((tmp & GYRO_Y_FIFO_EN_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setGyroYFIFOEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp))
    {
        tmp &= ~(GYRO_Y_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Y_FIFO_EN_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getGyroZFIFOEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
    {
        *enabled = ((tmp & GYRO_Z_FIFO_EN_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setGyroZFIFOEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp))
    {
        tmp &= ~(GYRO_Z_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Z_FIFO_EN_MASK) : tmp);
    }
    return false;
}

bool MPU6050::getAccelFIFOEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp, timeout_secs))
    {
        *enabled = ((tmp & ACCEL_FIFO_EN_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setAccelFIFOEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_FIFO_EN, &tmp))
    {
        tmp &= ~(ACCEL_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | ACCEL_FIFO_EN_MASK) : tmp);
    }
    return false;
}

/**
 * REG_INT_PIN_CFG register
 */

bool MPU6050::getInterruptLevel(InterruptLevel *level, float timeout_secs)
{    
    uint8_t tmp;
    if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs))
    {
        *level = static_cast<InterruptLevel>(tmp & INT_LEVEL_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptLevel(InterruptLevel level, float timeout_secs)
{    
    uint8_t tmp;
    return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_LEVEL_MASK)) | level, timeout_secs));
}

bool MPU6050::getInterruptDrive(InterruptDrive *drive, float timeout_secs)
{    
    uint8_t tmp;
    if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs))
    {
        *drive = static_cast<InterruptDrive>(tmp & INT_DRIVE_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptDrive(InterruptDrive drive, float timeout_secs)
{    
    uint8_t tmp;
    return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_DRIVE_MASK)) | drive, timeout_secs));
}

bool MPU6050::getInterruptLatch(InterruptLatch *latch, float timeout_secs)
{    
    uint8_t tmp;
    if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs))
    {
        *latch = static_cast<InterruptLatch>(tmp & INT_LATCH_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptLatch(InterruptLatch latch, float timeout_secs)
{    
    uint8_t tmp;
    return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_LATCH_MASK)) | latch, timeout_secs));
}

bool MPU6050::getInterruptLatchClear(InterruptClear *latchClear, float timeout_secs)
{    
    uint8_t tmp;
    if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs))
    {
        *latchClear = static_cast<InterruptClear>(tmp & INT_CLEAR_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptLatchClear(InterruptClear latchClear, float timeout_secs)
{    
    uint8_t tmp;
    return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_CLEAR_MASK)) | latchClear, timeout_secs));
}

bool MPU6050::getInterruptFSyncLevel(InterruptFSyncLevel *fsyncLevel, float timeout_secs)
{    
    uint8_t tmp;
    if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs))
    {
        *fsyncLevel = static_cast<InterruptFSyncLevel>(tmp & INT_FSYNC_LEVEL_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptFSyncLevel(InterruptFSyncLevel fsyncLevel, float timeout_secs)
{    
    uint8_t tmp;
    return (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs)) && (this->write(REG_INT_PIN_CFG, (tmp & (~INT_FSYNC_LEVEL_MASK)) | fsyncLevel, timeout_secs));
}

bool MPU6050::getInterruptFSyncEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_PIN_CFG, &tmp, timeout_secs))
    {
        *enabled = ((tmp & INT_FSYNC_EN_MASK) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptFSyncEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_PIN_CFG, &tmp))
    {
        tmp &= ~(INT_FSYNC_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | INT_FSYNC_EN_MASK) : tmp);
    }
    return false;
}

/**
 * REG_INT_ENABLE register
 */

bool MPU6050::getInterruptsEnabled(Interrupt *interruptSet, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
    {
        *interruptSet = static_cast<Interrupt>(tmp);
        return true;
    }
    return false;
}

bool MPU6050::setInterruptsEnabled(Interrupt interruptSet, float timeout_secs)
{
    uint8_t tmp = interruptSet;
    return this->write(REG_INT_ENABLE, tmp, timeout_secs);
}

bool MPU6050::getInterruptFreefallEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
    {
        *enabled = ((tmp & INT_FREEFALL) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptFreefallEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp))
    {
        tmp &= ~(INT_FREEFALL);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FREEFALL) : tmp);
    }
    return false;
}

bool MPU6050::getInterruptMotionEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
    {
        *enabled = ((tmp & INT_MOTION) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptMotionEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp))
    {
        tmp &= ~(INT_MOTION);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_MOTION) : tmp);
    }
    return false;
}

bool MPU6050::getInterruptZeroMotionEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
    {
        *enabled = ((tmp & INT_ZERO_MOTION) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptZeroMotionEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp))
    {
        tmp &= ~(INT_ZERO_MOTION);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_ZERO_MOTION) : tmp);
    }
    return false;
}

bool MPU6050::getInterruptFIFOOverflowEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
    {
        *enabled = ((tmp & INT_FIFO_OVERFLOW) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptFIFOOverflowEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp))
    {
        tmp &= ~(INT_FIFO_OVERFLOW);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FIFO_OVERFLOW) : tmp);
    }
    return false;
}

bool MPU6050::getInterruptDataReadyEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp, timeout_secs))
    {
        *enabled = ((tmp & INT_DATA_READY) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setInterruptDataReadyEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_ENABLE, &tmp))
    {
        tmp &= ~(INT_DATA_READY);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_DATA_READY) : tmp);
    }
    return false;
}

bool MPU6050::getInterruptStatuses(Interrupt *interruptSet, float timeout_secs)
{    
    uint8_t tmp;
    if (this->read(REG_INT_STATUS, &tmp, timeout_secs))
    {
        *interruptSet = static_cast<Interrupt>(tmp);
        return true;
    }
    return false;
}

bool MPU6050::getInterruptFreefallStatus(bool *status, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_STATUS, &tmp, timeout_secs))
    {
        *status = ((tmp & INT_FREEFALL) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::getInterruptMotionStatus(bool *status, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_STATUS, &tmp, timeout_secs))
    {
        *status = ((tmp & INT_MOTION) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::getInterruptZeroMotionStatus(bool *status, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_STATUS, &tmp, timeout_secs))
    {
        *status = ((tmp & INT_ZERO_MOTION) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::getInterruptFIFOOverflowStatus(bool *status, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_STATUS, &tmp, timeout_secs))
    {
        *status = ((tmp & INT_FIFO_OVERFLOW) != 0);
        return true;
    }
    
    return false;
}

bool MPU6050::getInterruptDataReadyStatus(bool *status, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_INT_STATUS, &tmp, timeout_secs))
    {
        *status = ((tmp & INT_DATA_READY) != 0);
        return true;
    }
    
    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];
    if (this->read(REG_ACCEL_XOUT_H, tmp, 6, timeout_secs))
    {
        *ax = (((int16_t)tmp[0]) << 8) | tmp[1];
        *ay = (((int16_t)tmp[2]) << 8) | tmp[3];
        *az = (((int16_t)tmp[4]) << 8) | tmp[5];
        
        return true;
    }
    
    return false;
}

bool MPU6050::getAccelerationX(int16_t *ax, float timeout_secs)
{
    uint8_t tmp[2];
    if (this->read(REG_ACCEL_XOUT_H, tmp, 2, timeout_secs))
    {
        *ax = (((int16_t)tmp[0]) << 8) | tmp[1];        
        return true;
    }
    
    return false;
}

bool MPU6050::getAccelerationY(int16_t *ay, float timeout_secs)
{
    uint8_t tmp[2];
    if (this->read(REG_ACCEL_YOUT_H, tmp, 2, timeout_secs))
    {
        *ay = (((int16_t)tmp[0]) << 8) | tmp[1];        
        return true;
    }
    
    return false;
}

bool MPU6050::getAccelerationZ(int16_t *az, float timeout_secs)
{
    uint8_t tmp[2];
    if (this->read(REG_ACCEL_ZOUT_H, tmp, 2, timeout_secs))
    {
        *az = (((int16_t)tmp[0]) << 8) | tmp[1];        
        return true;
    }
    
    return false;
}

/** Temperature */

bool MPU6050::getTemperature(int16_t *temp, float timeout_secs)
{
    uint8_t tmp[2];
    if (this->read(REG_TEMP_OUT_H, tmp, 2, timeout_secs))
    {
        *temp = (((int16_t)tmp[0]) << 8) | tmp[1];        
        return true;
    }
    
    return false;
}

/** Rotation speed */

bool MPU6050::getRotationSpeed(int16_t *ox, int16_t *oy, int16_t *oz, float timeout_secs)
{
    uint8_t tmp[6];
    if (this->read(REG_GYRO_XOUT_H, tmp, 6, timeout_secs))
    {
        *ox = (((int16_t)tmp[0]) << 8) | tmp[1];
        *oy = (((int16_t)tmp[2]) << 8) | tmp[3];
        *oz = (((int16_t)tmp[4]) << 8) | tmp[5];
        
        return true;
    }
    
    return false;
}

bool MPU6050::getRotationSpeedX(int16_t *ox, float timeout_secs)
{
    uint8_t tmp[2];
    if (this->read(REG_GYRO_XOUT_H, tmp, 2, timeout_secs))
    {
        *ox = (((int16_t)tmp[0]) << 8) | tmp[1];        
        return true;
    }
    
    return false;
}

bool MPU6050::getRotationSpeedY(int16_t *oy, float timeout_secs)
{
    uint8_t tmp[2];
    if (this->read(REG_GYRO_YOUT_H, tmp, 2, timeout_secs))
    {
        *oy = (((int16_t)tmp[0]) << 8) | tmp[1];        
        return true;
    }
    
    return false;
}

bool MPU6050::getRotationSpeedZ(int16_t *oz, float timeout_secs)
{
    uint8_t tmp[2];
    if (this->read(REG_GYRO_ZOUT_H, tmp, 2, timeout_secs))
    {
        *oz = (((int16_t)tmp[0]) << 8) | tmp[1];        
        return true;
    }
    
    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];
    if (this->read(REG_GYRO_XOUT_H, tmp, 14, timeout_secs))
    {
        *ax = (((int16_t)tmp[0]) << 8) | tmp[1];
        *ay = (((int16_t)tmp[2]) << 8) | tmp[3];
        *az = (((int16_t)tmp[4]) << 8) | tmp[5];
        *temp = (((int16_t)tmp[6]) << 8) | tmp[7];
        *ox = (((int16_t)tmp[8]) << 8) | tmp[9];
        *oy = (((int16_t)tmp[10]) << 8) | tmp[11];
        *oz = (((int16_t)tmp[12]) << 8) | tmp[13];
        
        return true;
    }
    
    return false;
}