Based on https://os.mbed.com/users/Airium/code/module_LSM303DLHC/ Main difference is the usage of the I2C - now it is passed as parameter making cooperative use easier. LSM303DLHC Full Driver: Readings For Acc, Mag and Temp; Class Method for frequently-used 13 sensor parameters; Class Method to direct modify registers; Support Calibration (offset scale);
LSM303DLHC.cpp
- Committer:
- Pythia
- Date:
- 2020-02-03
- Revision:
- 1:4ee6df2df73a
- Parent:
- 0:a4131de4bddd
File content as of revision 1:4ee6df2df73a:
#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.; } int LSM303DLHC::GetAcc(float arr[3]){ int result; data[0] = OUT_X_L_A | (1 << 7); // MSB=1 to read multiple bytes result = _i2c->write(ACC_ADDRESS, data, 1); if (0 != result) { printf("\nI2C write issue (%02x)%d\n", MAG_ADDRESS, result); return result; } result = _i2c->read(ACC_ADDRESS, data, 6); if (0 != result) { printf("\nI2C read issue (%02x)%d\n", MAG_ADDRESS, result); return result; } 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); float a_x = acc_scale[0] * (acc_offset[0] + count_x * acc_scale_multiplier); float a_y = acc_scale[1] * (acc_offset[1] + count_y * acc_scale_multiplier); float a_z = acc_scale[2] * (acc_offset[2] + count_z * acc_scale_multiplier); arr[0] = a_x; arr[1] = a_y; arr[2] = a_z; // printf("\nA %02X.%02X %02X.%02X %02X.%02X %04x %04x %04x %7.3f %7.3f %7.3f \n", data[1], data[0], data[3], data[2], data[5], data[4], count_x, count_y, count_z, a_x, a_y, a_z); return 0; } int LSM303DLHC::GetMag(float arr[3]){ int result; data[0] = OUT_X_H_M; result = _i2c->write(MAG_ADDRESS, data, 1); if (0 != result) { printf("\nI2C write issue (%02x)%d\n", MAG_ADDRESS, result); return result; } result = _i2c->read(MAG_ADDRESS, data, 6); if (0 != result) { printf("\nI2C read issue (%02x)%d\n", MAG_ADDRESS, result); return result; } 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]); float m_x = mag_scale[0] * (mag_offset[0] + count_x * mag_scale_x_multiplier); float m_y = mag_scale[1] * (mag_offset[1] + count_y * mag_scale_y_multiplier); float m_z = mag_scale[2] * (mag_offset[2] + count_z * mag_scale_z_multiplier); arr[0] = m_x; arr[1] = m_y; arr[2] = m_z; // printf("\nM %02X.%02X %02X.%02X %02X.%02X %04x %04x %04x %7.3f %7.3f %7.3f \n", data[1], data[0], data[3], data[2], data[5], data[4], count_x, count_y, count_z, m_x, m_y, m_z); return 0; } int LSM303DLHC::GetTemp(float arr[1]){ int result; data[0] = TEMP_OUT_H_M; result = _i2c->write(MAG_ADDRESS, data, 1); if (0 != result) { printf("\nI2C write issue (%02x)%d\n", MAG_ADDRESS, result); return result; } result = _i2c->read(MAG_ADDRESS, data, 2); if (0 != result) { printf("\nI2C read issue (%02x)%d\n", MAG_ADDRESS, result); return result; } int count = (((int)(int8_t)data[0]<<8) | ((uint8_t)data[1]))>>4; float temp = count / 8.; // printf("\nT %02X.%02X %04X %d %7.3f\n", data[0], data[1], count, count, temp); arr[0] = temp_scale[0] * (temp_offset[0] + temp); return 0; } 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]; }