File content as of revision 0:4aa6b3804281:

#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);
    // Enable temperature sensor (bit 7 = 1)
    // Set magnetometer operative mode for x and y axes (bits 6:5)
    // Set magnetometer ODR (bits 4:2)
    //writeByte(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG1, 0x80 | Mopmode << 5 | Modr << 2);
    //i2c_write(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG1, 0x80 | Mopmode << 5 | Modr << 2,1);
    
    //writeByte(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG2, Mscale << 5); // Set magnetometer full scale range
    //i2c_write(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG2, Mscale << 5,1);
    
    //writeByte(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG4, Mopmode << 2); // Set magnetometer operative mode for z axis
    //i2c_write(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG4, Mopmode << 2,1); // Set magnetometer operative mode for z axis
    
    //writeByte(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG5, 0x40); // output registers not updated until both data bytes have been read
    i2c_write(LIS3MDL_ADDRESS, LIS3MDL_CTRL_REG5, 0x40,1); // output registers not updated until both data bytes have been read


    //Put into the correct operating mode 
    //disableLowPower();
    //enableAxisXYZ();
    //setDataRate(2);
    
    return 1;
}

bool LIS3MDL::writeRegister(const uint8_t register_addr, const uint8_t value) {
    //send write call to sensor address
    //send register address to sensor
    //send value to register
    /*
    bool write_status = 0;
    Wire.beginTransmission(_address); //open communication with 
    Wire.write(register_addr);  
    Wire.write(value); 
    Wire.endTransmission(); 
    */
    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;
}

void getMag(int16_t* ax, int16_t* ay, int16_t* az) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool whoAmI(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getTempEnabled(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setTempEnabled(bool enable) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

uint8_t getOperativeMode(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setOperativeMode(uint8_t mode) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

uint8_t getDataRate(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setDataRate(uint8_t rate) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getSelfTestEnabled(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setSelfTestEnabled(bool enable) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

uint8_t getFullScale(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setFullScale(uint8_t sccale) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool reboot(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool softReset(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getLowPowerMode(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setLowPowerMode(bool mode) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getSPIMode(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setSPIMode(bool mode) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

uint8_t getOperatingMode(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setOperatingMode(uint8_t mode) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

uint8_t getOMZ(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setOMZ(uint8_t mode) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getEndian(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setEndian(bool selection) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool disableDataUpdate(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool enableDataUpdate(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getDataUpdateStatus(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getXYZOverrun(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getZOverrun(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getYOverrun(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getXOverrun(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getXYZDataAvailable(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getZDataAvailable(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getYDataAvailabl(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool getXDataAvailable(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool enableXInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool disableXInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool isXIntEnabled(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool enableYInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool disableYInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool isYIntEnabled(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool enableZInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool disableZInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool isZIntEnabled(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setIntHigh(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setIntLow(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool checkIntConfig(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool setInteruptLatch(bool latch) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool checkInteruptLatch(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool enableIntInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool disableIntInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool checkIntInterupt(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}


bool posXThreshold(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool posYThreshold(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool posZThreshold(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool negXThreshold(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool negYThreshold(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool negZThreshold(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool measurementOverflow(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

bool checkInteruptEvent(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}


bool setInteruptThreshold(uint16_t threshold) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}

uint16_t getInteruptThreshold(void) {
    //return (readMaskedRegister(LIS3MDL_CTRL_REG2, LIS3MDL_HPIS2_MASK) != 0);
}





/** Read the all axis registers
 * @see getAxisZ()
 * @see getAxisY()
 * @see getAxisZ()
 */
void LIS3MDL::getMotion(int16_t* ax, int16_t* ay, int16_t* az) {
    *ax = getAxisX();
    *ay = getAxisY();
    *az = getAxisZ();
}

//bool LIS3MDL::tempHasOverrun(void) {
//    uint8_t overrun = readMaskedRegister(LIS3MDL_STATUS_REG_AUX, LIS3MDL_TOR_MASK);
//    return (overrun != 0);
//}

//bool LIS3MDL::tempDataAvailable(void) {
//    uint8_t data = readMaskedRegister(LIS3MDL_STATUS_REG_AUX, LIS3MDL_TDA_MASK);
//    return (data != 0);
//}

//uint16_t LIS3MDL::getTemperature(void) {
//    if(tempDataAvailable()){
//        return readRegisters(LIS3MDL_OUT_TEMP_H, LIS3MDL_OUT_TEMP_L);
//    } else {
//        //if new data isn't available
//        return 0;
//    }
//}

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

bool LIS3MDL::getTempEnabled(void) {
    //return (readMaskedRegister(LIS3MDL_TEMP_CFG_REG, LIS3MDL_TEMP_EN_MASK) != 0);
}

bool LIS3MDL::setTempEnabled(bool enable) {
    //return writeRegister(LIS3MDL_TEMP_CFG_REG, enable ? LIS3MDL_TEMP_EN_MASK : 0);
}

uint8_t LIS3MDL::getDataRate(void) {
    //return readMaskedRegister(LIS3MDL_CTRL_REG1, LIS3MDL_ODR_MASK);
}

bool LIS3MDL::setDataRate(uint8_t rate) {
    if(rate > 9) {
        return 0;
    }
    uint8_t data_rate = rate << 4;
   // return writeMaskedRegister(LIS3MDL_CTRL_REG1, LIS3MDL_ODR_MASK, data_rate);
}