Felix Rüdiger / MPU6050_lib

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

MPU6050.cpp

Committer:
fruediger
Date:
2015-06-30
Revision:
0:95916b07e8be
Child:
1:96a227d1ca7e

File content as of revision 0:95916b07e8be:

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

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

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[0] = 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[0] = 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[0] = 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[0] = ((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[0] = ((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[0] = ((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[0] = 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[0] = ((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[0] = ((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[0] = ((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[0] = 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[0] = 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[0] = ((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[0] = ((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[0] = ((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[0] = ((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[0] = ((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;
}