Copied from STs implementation.
Diff: lis3mdl.cpp
- Revision:
- 1:898554a35638
- Parent:
- 0:32f3441153b5
diff -r 32f3441153b5 -r 898554a35638 lis3mdl.cpp --- a/lis3mdl.cpp Thu Oct 06 16:45:48 2016 +0000 +++ b/lis3mdl.cpp Thu Oct 06 23:28:08 2016 +0000 @@ -48,27 +48,18 @@ Init(); } /** - * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in mgauss. + * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in microTeslas. * @param float *pfData * @retval None. */ -void LIS3MDL::GetAxes(AxesRaw_TypeDef *pData) +void LIS3MDL::readMag(void) { - uint8_t tempReg = 0x00; - int16_t pDataRaw[3]; + float sensitivity = 0; - int ret; - - GetAxesRaw(pDataRaw); + - ret = _i2c.i2c_read(&tempReg, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1); - - if (ret == 0) - { - tempReg &= LIS3MDL_M_FS_MASK; - - switch(tempReg) - { + GetAxesRaw(); +/* case LIS3MDL_M_FS_4: sensitivity = 0.14; break; @@ -82,11 +73,11 @@ sensitivity = 0.58; break; } - } - - pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity); - pData->AXIS_Y = (int32_t)(pDataRaw[1] * sensitivity); - pData->AXIS_Z = (int32_t)(pDataRaw[2] * sensitivity); + }*/ + sensitivity = 0.12207; + mx = mx_raw * sensitivity; + my = my_raw * sensitivity; + mz = mz_raw * sensitivity; } @@ -95,31 +86,18 @@ * @param float *pfData * @retval None. */ -void LIS3MDL::GetAxesRaw(int16_t *pData) +void LIS3MDL::GetAxesRaw() { - uint8_t tempReg[2] = {0,0}; - int ret; - - pData[0] = pData[1] = pData[2] = 0; + char data[6]; + + char subAddressXL = LIS3MDL_M_OUT_X_L_M; - ret = _i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_X_L_M + 0x80, 2); - - if (ret == 0) - { - pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); - ret = _i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_Y_L_M + 0x80, 2); - } - - if (ret == 0) - { - pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); - ret = _i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_Z_L_M + 0x80, 2); - } - - if (ret == 0) - { - pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); - } + _i2c.write(LIS3MDL_M_MEMS_ADDRESS, &subAddressXL, 1, true); + _i2c.read(LIS3MDL_M_MEMS_ADDRESS, data, 6); + + mx_raw = data[0] | (data[1] << 8); + my_raw = data[2] | (data[3] << 8); + mz_raw = data[4] | (data[5] << 8); } /** @@ -129,14 +107,16 @@ */ uint8_t LIS3MDL::ReadID(void) { - uint8_t tmp=0x00; + char data[1]; int ret; + char subAddress = LIS3MDL_M_WHO_AM_I_ADDR; /* Read WHO I AM register */ - ret = _i2c.i2c_read(&tmp, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_WHO_AM_I_ADDR, 1); + _i2c.write(LIS3MDL_M_MEMS_ADDRESS, &subAddress, 1, true); + ret=_i2c.read(LIS3MDL_M_MEMS_ADDRESS, data, 1); /* Return the ID */ - return ((ret == 0) ? (uint8_t)tmp : 0); + return ((ret == 0) ? (uint8_t)data[0] : 0); } /** @@ -146,62 +126,43 @@ */ void LIS3MDL::Init() { - uint8_t tmp1 = 0x00; - int ret; + char tmp1[2]; /****** Magnetic sensor *******/ - - ret = _i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG3_M, 1); - + /* Conversion mode selection */ - if (ret == 0) - { - tmp1 &= ~(LIS3MDL_M_MD_MASK); - tmp1 |= LIS3MDL_M_MD_CONTINUOUS; - ret = _i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG3_M, 1); - } + tmp1[0] = LIS3MDL_M_CTRL_REG3_M; + tmp1[1] = 0x00; + + _i2c.write(LIS3MDL_M_MEMS_ADDRESS, tmp1, 2); + - if (ret == 0) - { - ret = _i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG1_M, 1); - } + //OM=UHP for XY + //FAST_ODR enabled - 155Hz + tmp1[0] = LIS3MDL_M_CTRL_REG1_M; + tmp1[1] = 0x8E; + + _i2c.write(LIS3MDL_M_MEMS_ADDRESS, tmp1, 2); - if (ret == 0) - { - /* Output data rate selection */ - tmp1 &= ~(LIS3MDL_M_DO_MASK); - tmp1 |= LIS3MDL_M_DO_80; + /* Full scale selection */ + tmp1[0] = LIS3MDL_M_CTRL_REG2_M; + tmp1[1] = 0x00; - /* X and Y axes Operative mode selection */ - tmp1 &= ~(LIS3MDL_M_OM_MASK); - tmp1 |= LIS3MDL_M_OM_HP; - - ret = _i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG1_M, 1); - } + _i2c.write(LIS3MDL_M_MEMS_ADDRESS, tmp1, 2); - if (ret == 0) - { - ret = _i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1); - } - - if (ret == 0) - { - /* Full scale selection */ - tmp1 &= ~(LIS3MDL_M_FS_MASK); - tmp1 |= LIS3MDL_M_FS_4; + /*Fast Z*/ + tmp1[0] = LIS3MDL_M_CTRL_REG4_M; + tmp1[1] = 0x0C; - ret = _i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1); - } + _i2c.write(LIS3MDL_M_MEMS_ADDRESS, tmp1, 2); /******************************/ - if (ret == 0) - { if(ReadID() == I_AM_LIS3MDL_M) { LIS3MDLInitialized = 1; } - } + return; }