LIS3MDL I2C Mbed class with compass heading function
LIS3MDL.cpp
- Committer:
- gpmbed
- Date:
- 2021-04-13
- Revision:
- 1:fe199024ddfe
- Parent:
- 0:4aa6b3804281
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, ®_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)); }