LSM303DLHC.cpp

//------------------------------------------------------------------------------
//This file is part of embKernel. 
//See license.txt for the full license governing this code.
//------------------------------------------------------------------------------

#include "LSM303DLHC.hpp"

LSM303DLHC::LSM303DLHC(DrvI2cMaster* i2c) {
    mI2c = i2c;
}

LSM303DLHC::~LSM303DLHC() {
}

void LSM303DLHC::init() {
    writeMagRegister(LSM303DLHC_CRA_REG_M, LSM303DLHC_TEMPSENSOR_DISABLE | LSM303DLHC_ODR_30_HZ);
    writeMagRegister(LSM303DLHC_CRB_REG_M, LSM303DLHC_FS_8_1_GA);
    writeMagRegister(LSM303DLHC_MR_REG_M, LSM303DLHC_CONTINUOS_CONVERSION);
    writeAccRegister(LSM303DLHC_CTRL_REG1_A, LSM303DLHC_NORMAL_MODE | LSM303DLHC_ODR_50_HZ | LSM303DLHC_AXES_ENABLE); //0x47
    writeAccRegister(LSM303DLHC_CTRL_REG2_A, LSM303DLHC_HPM_NORMAL_MODE | LSM303DLHC_HPFCF_16 | LSM303DLHC_HPF_AOI1_DISABLE | LSM303DLHC_HPF_AOI2_DISABLE); //0x90
    writeAccRegister(LSM303DLHC_CTRL_REG4_A, LSM303DLHC_FULLSCALE_2G | LSM303DLHC_BlockUpdate_Continous | LSM303DLHC_BLE_LSB | LSM303DLHC_HR_ENABLE); //0x08
}

bool LSM303DLHC::readAccXyz(float* x, float* y, float* z) {
    int16_t xyz[3];
    if (!readAccRegisters(LSM303DLHC_OUT_X_L_A, (uint8_t*) xyz, sizeof(xyz))) {
        return false;
    }
    *x = (float) xyz[0] / 16000.0;
    *y = (float) xyz[1] / 16000.0;
    *z = (float) xyz[2] / 16000.0;

    return true;
}

bool LSM303DLHC::writeMagRegister(uint8_t reg, uint8_t value) {
    mI2c->setSlaveAddr(MAG_I2C_ADDRESS);
    uint8_t buffer[] = { reg, value };
    return mI2c->readWrite(buffer, sizeof(buffer), 0, 0);
}

bool LSM303DLHC::readMagRegisters(uint8_t reg, uint8_t* buffer, size_t len) {
    if (len > 1) {
        reg |= 0x80;
    }
    mI2c->setSlaveAddr(MAG_I2C_ADDRESS);
    return mI2c->readWrite(&reg, 1, buffer, len);
}

bool LSM303DLHC::writeAccRegister(uint8_t reg, uint8_t value) {
    mI2c->setSlaveAddr(ACC_I2C_ADDRESS);
    uint8_t buffer[] = { reg, value };
    return mI2c->readWrite(buffer, sizeof(buffer), 0, 0);
}

bool LSM303DLHC::readAccRegisters(uint8_t reg, uint8_t* buffer, size_t len) {
    if (len > 1) {
        reg |= 0x80;
    }
    mI2c->setSlaveAddr(ACC_I2C_ADDRESS);
    return mI2c->readWrite(&reg, 1, buffer, len);
}