File content as of revision 1:fe199024ddfe:

#include "LIS3MDL.h"


LIS3MDL::LIS3MDL(PinName sda, PinName scl) : i2c(sda, scl) 
{
     _address = LIS3MDL_ADDRESS;//LIS3MDL_DEFAULT_ADDRESS;
}

/* ****
            addr           data        len
i2c.write(LSM6DS0_ADDRESS, data_write, 2);
*/

int LIS3MDL::i2c_write(uint8_t DeviceAddr, uint8_t RegisterAddr,uint8_t value, uint16_t NumByteToWrite)
{
        int ret;
        int TEMP_BUF_SIZE = 5;
        uint8_t tmp[TEMP_BUF_SIZE];

        if(NumByteToWrite >= TEMP_BUF_SIZE) return -2;

        /* First, send device address. Then, send data and STOP condition */
        tmp[0] = RegisterAddr;
        tmp[1] = value;
        //memcpy(tmp+1, pBuffer, NumByteToWrite);

        ret = i2c.write(DeviceAddr, (const char*)tmp, NumByteToWrite+1, false);

        if(ret) return -1;
        return 0;
}

int LIS3MDL::i2c_read(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* pBuffer, uint16_t NumByteToRead) {
        int ret;

        /* Send device address, with no STOP condition */
        ret = i2c.write(DeviceAddr, (const char*)&RegisterAddr, 1, true);
        if(!ret) {
            /* Read data, with STOP condition  */
            ret = i2c.read(DeviceAddr, (char*)pBuffer, NumByteToRead, false);
        }

        if(ret) return -1;
        return 0;
    }

bool LIS3MDL::init(void) {
 
    
    // Choose device mode (bits 1:0 = 00 = continuous data read, 01 = single conversion, 10 & 11 = default power down)
    //writeByte(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG3, 0x00); // Enable continuous data read mode (bits 1:0 = 00)
    i2c_write(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG3,0x00,1);
    i2c_write(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG5, 0x40,1); // output registers not updated until both data bytes have been read


    return 1;
}

bool LIS3MDL::writeRegister(const uint8_t register_addr, const uint8_t value) {
 
    i2c_write(_address,register_addr,value,1);

    return 1; //returns whether the write succeeded or failed
}

uint8_t LIS3MDL::readRegister(const uint8_t register_addr)
{
    uint8_t reg_val = 0;
    i2c_read(_address, register_addr, &reg_val, 1);
    
    return reg_val;
}

uint16_t LIS3MDL::readRegisters(const uint8_t msb_register, const uint8_t lsb_register) {
    uint8_t msb = readRegister(msb_register);
    uint8_t lsb = readRegister(lsb_register);
    return (((int16_t)msb) << 8) | lsb;
}

bool LIS3MDL::writeMaskedRegister(const uint8_t register_addr, const uint8_t mask, const bool value) {
    uint8_t data = readRegister(register_addr);
    uint8_t combo;
    if(value) {
        combo = (mask | data);
    } else {
        combo = ((~mask) & data);
    }
    return writeRegister(register_addr, combo);
}

bool LIS3MDL::writeMaskedRegister(const int register_addr, const int mask, const int value) {
    uint8_t data = readRegister(register_addr);
    uint8_t masked_value = (data | (mask & value)); //Not sure if right...
    return writeRegister(register_addr, masked_value);
}

uint8_t LIS3MDL::readMaskedRegister(const uint8_t register_addr, const uint8_t mask) {
    uint8_t data = readRegister(register_addr);
    return (data & mask);
}


/** Read the X axis registers
 * @see LIS3MDL_OUT_X_H
 * @see LIS3MDL_OUT_X_L
 */
int16_t LIS3MDL::getAxisX(void) {
    return readRegisters(LIS3MDL_OUT_X_H, LIS3MDL_OUT_X_L);
}

float LIS3MDL::getAxisX_mag()
{
    float res = 0;
    float factor = 6842;
    res = (int16_t)getAxisX();
    res = res / factor;
    
    return res;
}


/** Read the Y axis registers
 * @see LIS3MDL_OUT_Y_H
 * @see LIS3MDL_OUT_Y_L
 */
int16_t LIS3MDL::getAxisY(void) {
    return readRegisters(LIS3MDL_OUT_Y_H, LIS3MDL_OUT_Y_L);
}

float LIS3MDL::getAxisY_mag()
{
    float res = 0;
    float factor = 6842;
    res = (int16_t)getAxisY();
    res = res / factor;
    
    return res;
}

/** Read the Z axis registers
 * @see LIS3MDL_OUT_Z_H
 * @see LIS3MDL_OUT_Z_L
 */
int16_t LIS3MDL::getAxisZ(void) {
    return readRegisters(LIS3MDL_OUT_Z_H, LIS3MDL_OUT_Z_L);
}

int16_t LIS3MDL::getTemperature(void) {
    return readRegisters(LIS3MDL_TEMP_OUT_H,LIS3MDL_TEMP_OUT_L);
}


float LIS3MDL::getHeading()
{
    float xH = getAxisX_mag();
    float yH = getAxisY_mag();
    //float xH = (float)mag_axes[0]; //milliGauss
    //float yH = (float)mag_axes[1]; //milliGauss
    xH *=1000; //milliGauss
    yH *=1000; //milliGauss
    
    xH = xH - 190; //taratura manuale
    yH = yH + 550;
    
    float heading = atan(yH/xH)*LIS3MDL_Rad2Degree;
    
    
    if(xH<0){heading=180-heading;}
    else if(xH>0  && yH<0){heading=-heading;}
    else if(xH>0  && yH>0){heading=360-heading; }
    else if(xH==0 && yH<0){heading=90; }
    else if(xH==0 && yH>0){heading=270; }
    
    heading = (360-heading)+180;
    
    if(heading<0){ heading +=360; }
    else if(heading>360){ heading -=360; }

    
    return heading;
}


bool LIS3MDL::whoAmI(void) {
    return (LIS3MDL_I_AM_MASK == readRegister(LIS3MDL_WHO_AM_I));
}