File content as of revision 0:a4131de4bddd:

#include "mbed.h"
#include "LSM303DLHC.h"

LSM303DLHC::LSM303DLHC(I2C &i2c){

    _i2c = &i2c;
//    use setting for _i2c->frequency(400000);

    // HERE GIVES DEVICE DEFAULT
    ACtrl(LP_OFF);      // ACC Normal Power Mode
    ACtrl(ADR3);        // ACC ON and Date Rate 25Hz
    ACtrl(XYZ);         // ACC XYZ Axis Enable
    ACtrl(HPF_ON);      // ACC internal HPF IN USE
    ACtrl(HPF_CF0);     // ACC HPF Cutoff Freq = option 0    
    ACtrl(HPF_NORM_R);  // ACC HPF Model = Normal
    ACtrl(BDU_CONT);    // ACC data continuous
    ACtrl(G4);          // ACC Range +/-4g
    ACtrl(HIGH_R);      // ACC in High Prec
    MCtrl(MDR4);        // MAG DR 15Hz
    MCtrl(GN4);         // MAG GN 4.0Gauss
    MCtrl(MD_CONT);     // MAG ON and MD Continuous
    TCtrl(TEMP_ON);     // TEMP ON
    
    // DEFAULT CALIBRATION PARAMETER
    acc_offset[0] = 0;
    acc_offset[1] = 0;
    acc_offset[2] = 0;
    acc_scale[0] = 1;
    acc_scale[1] = 1;
    acc_scale[2] = 1;
    
    mag_offset[0] = 0;
    mag_offset[1] = 0;
    mag_offset[2] = 0;
    mag_scale[0] = 1;
    mag_scale[1] = 1;
    mag_scale[2] = 1;
    
    temp_offset[0] = 0;
    temp_scale[0] = 1.;
}

void LSM303DLHC::GetAcc(float arr[3]){
    data[0] = OUT_X_L_A | (1 << 7); // MSB=1 to read multiple bytes
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, data, 6);

    int count_x = ((((int)(int8_t)data[1])<<8 | ((uint8_t)data[0])) >> 4);
    int count_y = ((((int)(int8_t)data[3])<<8 | ((uint8_t)data[2])) >> 4);
    int count_z = ((((int)(int8_t)data[5])<<8 | ((uint8_t)data[4])) >> 4);

//    printf("\nA %02X.%02X %02X.%02X %02X.%02X %04x %04x %04x\n", data[1], data[0], data[3], data[2], data[5], data[4], count_x, count_y, count_z);

    arr[0] = acc_scale[0] * (acc_offset[0] + count_x * acc_scale_multiplier);
    arr[1] = acc_scale[1] * (acc_offset[1] + count_y * acc_scale_multiplier);
    arr[2] = acc_scale[2] * (acc_offset[2] + count_z * acc_scale_multiplier);
}

void LSM303DLHC::GetMag(float arr[3]){
    data[0] = OUT_X_H_M;
    _i2c->write(MAG_ADDRESS, data, 1);
    _i2c->read(MAG_ADDRESS, data, 6);
    
    int count_x = ((int)(int8_t)data[0])<<8 | ((uint8_t)data[1]);
    int count_y = ((int)(int8_t)data[4])<<8 | ((uint8_t)data[5]);
    int count_z = ((int)(int8_t)data[2])<<8 | ((uint8_t)data[3]);
    
//    printf("\nM %02X.%02X %02X.%02X %02X.%02X %04x %04x %04x\n", data[1], data[0], data[3], data[2], data[5], data[4], count_x, count_y, count_z);

    arr[0] = mag_scale[0] * (mag_offset[0] + count_x * mag_scale_x_multiplier);
    arr[1] = mag_scale[1] * (mag_offset[1] + count_y * mag_scale_y_multiplier);
    arr[2] = mag_scale[2] * (mag_offset[2] + count_z * mag_scale_z_multiplier);
}

void LSM303DLHC::GetTemp(float arr[1]){
    data[0] = TEMP_OUT_H_M;
    _i2c->write(MAG_ADDRESS, data, 1);
    _i2c->read(MAG_ADDRESS, data, 2);
    
    int count = (((int)(int8_t)data[0]<<8) | ((uint8_t)data[1]))>>4;
    
//    printf("\nT %02X.%02X %04X %d\n", data[0], data[1], count, count);
    
    arr[0] = temp_scale[0] * (temp_offset[0] + count/8.);
}

void LSM303DLHC::ACtrl(ACC_ODR cmd){
    data[0] = CTRL_REG1_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b00001111) | (cmd<<4);
    _i2c->write(ACC_ADDRESS, data, 2);  
}

void LSM303DLHC::ACtrl(ACC_LPen cmd){
    data[0] = CTRL_REG1_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b11110111) | (cmd<<3);
    _i2c->write(ACC_ADDRESS, data, 2); 
}

void LSM303DLHC::ACtrl(ACC_AXIS cmd){
    data[0] = CTRL_REG1_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b11111000) | (cmd<<0);
    _i2c->write(ACC_ADDRESS, data, 2); 
}

void LSM303DLHC::ACtrl(ACC_HPM cmd){
    data[0] = CTRL_REG2_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b00111111) | (cmd<<6);
    _i2c->write(ACC_ADDRESS, data, 2); 
}

void LSM303DLHC::ACtrl(ACC_HPCF cmd){
    data[0] = CTRL_REG2_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b11001111) | (cmd<<4);
    _i2c->write(ACC_ADDRESS, data, 2); 
}

void LSM303DLHC::ACtrl(ACC_FDS cmd){
    data[0] = CTRL_REG2_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b11110111) | (cmd<<3);
    _i2c->write(ACC_ADDRESS, data, 2); 
}

void LSM303DLHC::ACtrl(ACC_BDU cmd){
    data[0] = CTRL_REG4_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b01111111) | (cmd<<7);
    _i2c->write(ACC_ADDRESS, data, 2); 
}

static const int A_SCALE[4] = {1, 2, 4, 12};

void LSM303DLHC::ACtrl(ACC_FS cmd){
    data[0] = CTRL_REG4_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b11001111) | (cmd<<4);
    _i2c->write(ACC_ADDRESS, data, 2); 
    
    acc_scale_multiplier = A_SCALE[cmd]/1000.;    
}

void LSM303DLHC::ACtrl(ACC_HR cmd){
    data[0] = CTRL_REG4_A;
    _i2c->write(ACC_ADDRESS, data, 1);
    _i2c->read(ACC_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b11110111) | (cmd<<3);
    _i2c->write(ACC_ADDRESS, data, 2); 
}

void LSM303DLHC::TCtrl(TEMP_EN cmd){
    data[0] = CRA_REG_M;
    _i2c->write(MAG_ADDRESS, data, 1);
    _i2c->read(MAG_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b01111111) | (cmd<<7);
    _i2c->write(MAG_ADDRESS, data, 2); 
}

void LSM303DLHC::MCtrl(MAG_DR cmd){
    data[0] = CRA_REG_M;
    _i2c->write(MAG_ADDRESS, data, 1);
    _i2c->read(MAG_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b11100011) | (cmd<<2);
    _i2c->write(MAG_ADDRESS, data, 2); 
}

static const int M_SCALE_XY[8] = {1, 1100, 855, 670, 450, 400, 330, 230 };
static const int M_SCALE_Z[8]  = {1,  980, 760, 600, 400, 355, 295, 205 };

void LSM303DLHC::MCtrl(MAG_GN cmd){
    data[0] = CRB_REG_M;
    _i2c->write(MAG_ADDRESS, data, 1);
    _i2c->read(MAG_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b00011111) | (cmd<<5);
    _i2c->write(MAG_ADDRESS, data, 2);
    
    mag_scale_x_multiplier = 1./M_SCALE_XY[cmd];
    mag_scale_y_multiplier = 1./M_SCALE_XY[cmd];
    mag_scale_z_multiplier = 1./M_SCALE_Z[cmd];
}

void LSM303DLHC::MCtrl(MAG_MD cmd){
    data[0] = MR_REG_M;
    _i2c->write(MAG_ADDRESS, data, 1);
    _i2c->read(MAG_ADDRESS, &data[1], 1);
    data[1] = data[1] & (0b11111100) | (cmd<<0);
    _i2c->write(MAG_ADDRESS, data, 2); 
}

void LSM303DLHC::WriteReg(int sad, char d[2]){
    _i2c->write(sad, d, 2);   
}

void LSM303DLHC::ACal(float offset[3], float scale[3]){
    acc_offset[0] = offset[0];
    acc_offset[1] = offset[1];
    acc_offset[2] = offset[2];
    acc_scale[0] = scale[0];
    acc_scale[1] = scale[1];
    acc_scale[2] = scale[2];
}

void LSM303DLHC::MCal(float offset[3], float scale[3]){
    mag_offset[0] = offset[0];
    mag_offset[1] = offset[1];
    mag_offset[2] = offset[2];
    mag_scale[0] = scale[0];
    mag_scale[1] = scale[1];
    mag_scale[2] = scale[2];
}

void LSM303DLHC::TCal(float offset[1], float scale[1]){
    temp_offset[0] = offset[0];
    temp_scale[0] = scale[0];
}