File content as of revision 4:5a52b575fbc6:

#include "MPU6050.h"

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);
}
 
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));
}

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);
}

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));
}

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, timeout_secs))
    {
        tmp &= ~(GYRO_X_ST_MASK);
        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_X_ST_MASK) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(GYRO_Y_ST_MASK);
        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Y_ST_MASK) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(GYRO_Z_ST_MASK);
        return this->write(REG_GYRO_CONFIG, enabled ? (tmp | GYRO_Z_ST_MASK) : tmp, timeout_secs);
    }
    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));
}
 
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, timeout_secs))
    {
        tmp &= ~(ACCEL_X_ST_MASK);
        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_X_ST_MASK) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(ACCEL_Y_ST_MASK);
        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Y_ST_MASK) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(ACCEL_Z_ST_MASK);
        return this->write(REG_ACCEL_CONFIG, enabled ? (tmp | ACCEL_Z_ST_MASK) : tmp, timeout_secs);
    }
    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));
}
        
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);
}

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);
}
        
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);
}

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);
}
        
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);
}

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);
}
 
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, timeout_secs))
    {
        tmp &= ~(TEMP_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | TEMP_FIFO_EN_MASK) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(GYRO_X_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_X_FIFO_EN_MASK) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(GYRO_Y_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Y_FIFO_EN_MASK) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(GYRO_Z_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | GYRO_Z_FIFO_EN_MASK) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(ACCEL_FIFO_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | ACCEL_FIFO_EN_MASK) : tmp, timeout_secs);
    }
    return false;
}

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, timeout_secs))
    {
        tmp &= ~(INT_FSYNC_EN_MASK);
        return this->write(REG_FIFO_EN, enabled ? (tmp | INT_FSYNC_EN_MASK) : tmp, timeout_secs);
    }
    return false;
}

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, timeout_secs))
    {
        tmp &= ~(INT_FREEFALL);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FREEFALL) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(INT_MOTION);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_MOTION) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(INT_ZERO_MOTION);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_ZERO_MOTION) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(INT_FIFO_OVERFLOW);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_FIFO_OVERFLOW) : tmp, timeout_secs);
    }
    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, timeout_secs))
    {
        tmp &= ~(INT_DATA_READY);
        return this->write(REG_INT_ENABLE, enabled ? (tmp | INT_DATA_READY) : tmp, timeout_secs);
    }
    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;
}

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;
}

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;
}

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;
}

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;
}

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));
}

bool MPU6050::getAccelerometerPowerOnDelay(PowerOnDelay* delay, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs))
    {
        *delay = static_cast<PowerOnDelay>(tmp & POWER_ON_DELAY_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setAccelerometerPowerOnDelay(PowerOnDelay delay, float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) && (this->write(REG_MOT_DETECT_CTRL, (tmp & (~POWER_ON_DELAY_MASK)) | delay, timeout_secs));
}

bool MPU6050::getFreefallDetectionDecrement(FreefallDetectionDecrement* dec,  float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs))
    {
        *dec = static_cast<FreefallDetectionDecrement>(tmp & FF_DETECTION_DEC_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setFreefallDetectionDecrement(FreefallDetectionDecrement dec, float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) && (this->write(REG_MOT_DETECT_CTRL, (tmp & (~FF_DETECTION_DEC_MASK)) | dec, timeout_secs));
}

bool MPU6050::getMotionDetectionDecrement(MotionDetectionDecrement* dec, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs))
    {
        *dec = static_cast<MotionDetectionDecrement>(tmp & MOT_DETECTION_DEC_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setMotionDetectionDecrement(MotionDetectionDecrement dec, float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_MOT_DETECT_CTRL, &tmp, timeout_secs)) && (this->write(REG_MOT_DETECT_CTRL, (tmp & (~MOT_DETECTION_DEC_MASK)) | dec, timeout_secs));
}

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

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

bool MPU6050::resetFIFO(float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_USER_CTRL, &tmp) && this->write(REG_USER_CTRL, tmp | FIFO_RESET_MASK));
}

bool MPU6050::resetSensors(float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_USER_CTRL, &tmp) && this->write(REG_USER_CTRL, tmp | SIG_COND_RESET_MASK));
}

bool MPU6050::reset(float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_PWR_MGMT_1, &tmp) && this->write(REG_PWR_MGMT_1, tmp | RESET_MASK));
}

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

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

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

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

bool MPU6050::getTemperatureSensorEnabled(bool *enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs))
    {
        // this is an inverted state
        *enabled = ((tmp & TEMP_EN_MASK) == 0);
        return true;
    }
    
    return false;
}

bool MPU6050::setTemperatureSensorEnabled(bool enabled, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs))
    {
        tmp &= ~(TEMP_EN_MASK);
        //this is an incverted state
        return this->write(REG_PWR_MGMT_1, enabled ? tmp : (tmp | TEMP_EN_MASK), timeout_secs);
    }
    return false;
}

bool MPU6050::getClockSource(ClockSource* clockSource, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs))
    {
        *clockSource = static_cast<ClockSource>(tmp & CLOCK_SOURCE_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setClockSource(ClockSource clockSource, float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_PWR_MGMT_1, &tmp, timeout_secs)) && (this->write(REG_PWR_MGMT_1, (tmp & (~CLOCK_SOURCE_MASK)) | clockSource, timeout_secs));
}

bool MPU6050::getWakeFrequency(WakeFrequency* wakeFrequency, float timeout_secs)
{
    uint8_t tmp;
    if (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs))
    {
        *wakeFrequency = static_cast<WakeFrequency>(tmp & WAKE_FREQ_MASK);
        return true;
    }
    
    return false;
}

bool MPU6050::setWakeFrequency(WakeFrequency wakeFrequency, float timeout_secs)
{
    uint8_t tmp;
    return (this->read(REG_PWR_MGMT_2, &tmp, timeout_secs)) && (this->write(REG_PWR_MGMT_2, (tmp & (~WAKE_FREQ_MASK)) | wakeFrequency, timeout_secs));
}

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

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

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

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

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

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

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

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

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

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

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

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

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

bool MPU6050::readFIFO(uint8_t *data, float timeout_secs)
{
    return this->read(REG_FIFO_R_W, data, timeout_secs);
}

bool MPU6050::writeFIFO(uint8_t data, float timeout_secs)
{
    return this->write(REG_FIFO_R_W, data, timeout_secs);
}

bool MPU6050::readFIFO(uint8_t *data, size_t lenght, float timeout_secs)
{
    return this->read(REG_FIFO_R_W, data, lenght, timeout_secs);
}

bool MPU6050::writeFIFO(uint8_t *data, size_t lenght, float timeout_secs)
{
    return this->write(REG_FIFO_R_W, data, lenght, timeout_secs);
}

bool MPU6050::getDeviceId(uint8_t *id, float timeout_secs)
{
    
    uint8_t tmp;
    if (this->read(REG_WHO_AM_I, &tmp, timeout_secs))
    {
        *id = tmp & DEVICE_ID_MASK;
        return true;
    }
    
    return false;
}