Mbed driver for the Stmicroelectronics LIS2MDL sensor
Dependents: itracker-mbed-os-example-lis2mdl
Introduction
The LIS2MDL is an ultra-low-power, high-performance 3-axis digital magnetic sensor.
The LIS2MDL has a magnetic field dynamic range of ±50 gauss.
The LIS2MDL includes an I2C serial bus interface that supports standard, fast mode, fast mode plus, and high-speed (100 kHz, 400 kHz, 1 MHz, and 3.4 MHz) and an SPI serial standard interface.
More details in datasheet here: http://www.st.com/resource/en/datasheet/lis2mdl.pdf
This driver assumes the user will deploy the sensor over I2C. SPI update will be committed soon :)
Sample Code:
LIS2mDL sample program
uint8_t MODR = MODR_100Hz; int16_t temp[3] = {0, 0, 0}; float magBias[3] = {0,0,0}, magScale[3] = {0,0,0}; // Attach interrupt to this pin if you want to use magnetometer sensor interrupt // Before that make sure to set the correct interrupt value in the INT_CTRL_REG, INT_THS_L_REG and the INT_THS_H_REG (threshold value) DigitalIn interruptPin(LIS2MDL_intPin); // The itracker has the LIS2MDL sensor on the 13 and 11 I2C pins I2C i2c(p13,p11); // main() method. Runs in its own thread in the OS int main() { //Create LIS2MDL object LIS2MDL sensor(i2c, LIS2MDL_ADDRESS); //Reset the sensor to ensure correct starting config register values sensor.reset(); wait(3); //Intialise the CHIP with the MODR value chosen sensor.init(MODR); //Test the Chip ID. Should return 64 (0x40) sensor.getChipID(); // Calcaulte the offset bias to be used in future reading. See self check for example usage sensor.offsetBias(magBias, magScale); // Read the internal temp sensor sensor.readTemperature(); //Get readings from the sensor; for(int i=0; i<60; i++) { int16_t temp[3] = {0, 0, 0}; sensor.readData(temp); wait(0.1); } }
Diff: LIS2MDL.cpp
- Revision:
- 0:d7138994c637
- Child:
- 1:d7ea67f32b32
diff -r 000000000000 -r d7138994c637 LIS2MDL.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LIS2MDL.cpp Mon Feb 12 04:59:47 2018 +0000 @@ -0,0 +1,225 @@ +/* + Created by Naresh Krishnamoorthy + + The LIS2MDL is a low power magnetometer, here used as 3 DoF solution. + Library may be used freely and without limit with attribution. + +*/ + +#include "LIS2MDL.h" +#include "SEGGER_RTT.h" + +LIS2MDL::LIS2MDL (I2C& p_i2c, uint8_t addr) + : _i2c(p_i2c) +{ + _i2c.frequency(400000); +} + +void LIS2MDL::reset() +{ + // reset device + uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A); + writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL + wait(0.1); + writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL + wait(0.1); // Wait for all registers to reset +} + +void LIS2MDL::offsetBias(float * dest1, float * dest2) +{ + int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0}; + int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0}; + float _mRes = 0.0015f; + + SEGGER_RTT_printf(0, "Calculate mag offset bias: move all around to sample the complete response surface!"); + wait(4); + + for (int ii = 0; ii < 4000; ii++) { + readData(mag_temp); + for (int jj = 0; jj < 3; jj++) { + if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; + if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; + } + wait(0.12); + } + + _mRes = 0.0015f; // fixed sensitivity + // Get hard iron correction + mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts + mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts + mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts + + dest1[0] = (float) mag_bias[0] * _mRes; // save mag biases in G for main program + dest1[1] = (float) mag_bias[1] * _mRes; + dest1[2] = (float) mag_bias[2] * _mRes; + + // Get soft iron correction estimate + mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts + mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts + mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts + + float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2]; + avg_rad /= 3.0f; + + dest2[0] = avg_rad/((float)mag_scale[0]); + dest2[1] = avg_rad/((float)mag_scale[1]); + dest2[2] = avg_rad/((float)mag_scale[2]); + + SEGGER_RTT_printf(0, "Mag Calibration done!"); +} + + +void LIS2MDL::init(uint8_t MODR) +{ + +// enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00) + writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR<<2); + +// enable low pass filter (bit 0 == 1), set to ODR/4 + writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01); + +// enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1) + writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10); + +} + +uint8_t LIS2MDL::getChipID() +{ + uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I); + SEGGER_RTT_printf(0, "Address: %d \n", c); + return c; +} + +uint8_t LIS2MDL::status() +{ + // Read the status register of the altimeter + uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG); + SEGGER_RTT_printf(0, "LIS2MDL status : %d \n", temp); + return temp; +} + +void LIS2MDL::readData(int16_t * destination) +{ + char rawData[6]; // x/y/z mag register data stored here + readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8, &rawData[0]); // Read the 6 raw data registers into data array + + destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; + destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; + + SEGGER_RTT_printf(0, "x: %d y: %d z: %d \n", destination[0], destination[1], destination[2]); +} + +int16_t LIS2MDL::readTemperature() +{ + char rawData[2]; // x/y/z mag register data stored here + readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2, &rawData[0]); // Read the 8 raw data registers into data array + + int16_t temp = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value + SEGGER_RTT_printf(0, "LIS2MDL temp is : %d \n", temp); + return temp; +} + +void LIS2MDL::lis2mdlSelfCheck() +{ + int16_t temp[3] = {0, 0, 0}; + float magTest[3] = {0., 0., 0.}; + float magNom[3] = {0., 0., 0.}; + int32_t sum[3] = {0, 0, 0}; + float _mRes = 0.0015f; + + // first, get average response with self test disabled + for (int ii = 0; ii < 50; ii++) + { + readData(temp); + sum[0] += temp[0]; + sum[1] += temp[1]; + sum[2] += temp[2]; + wait(0.1); + } + + magNom[0] = (float) sum[0] / 50.0f; + magNom[1] = (float) sum[1] / 50.0f; + magNom[2] = (float) sum[2] / 50.0f; + + uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C); + writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test + wait(0.1); // let mag respond + + sum[0] = 0; + sum[1] = 0; + sum[2] = 0; + for (int ii = 0; ii < 50; ii++) + { + readData(temp); + sum[0] += temp[0]; + sum[1] += temp[1]; + sum[2] += temp[2]; + wait(0.1); + } + + magTest[0] = (float) sum[0] / 50.0f; + magTest[1] = (float) sum[1] / 50.0f; + magTest[2] = (float) sum[2] / 50.0f; + + writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode + wait(0.1); // let mag respond + + /*SEGGER_RTT.printf(0, "Mag Self Test: \n"); + SEGGER_RTT.printf(0, "Mx results:"); + SEGGER_RTT.printf(0, " %f ", (magTest[0] - magNom[0]) * _mRes * 1000.0); + SEGGER_RTT.printf(0, " mG \n"); + SEGGER_RTT.printf(0, "My results:"); + SEGGER_RTT.printf(0, " %f \n", (magTest[0] - magNom[0]) * _mRes * 1000.0); + SEGGER_RTT.printf(0, "Mz results:"); + SEGGER_RTT.printf(0, " %f \n", (magTest[1] - magNom[1]) * _mRes * 1000.0); + SEGGER_RTT.printf(0, "Should be between 15 and 500 mG \n");*/ + wait(2.0); // give some time to read the screen +} + + + + +//******************************************* +// I2C read/write functions for the LIS2MDL +//******************************************* + +uint8_t LIS2MDL::readByte(uint8_t address, char subAddress) +{ + char temp[1]; + int ack = 0; + _i2c.start(); + ack = _i2c.write(0x3C); + //SEGGER_RTT_printf(0, "address ACK: %d \n", ack); + ack = _i2c.write(subAddress); + //SEGGER_RTT_printf(0, "sub address ACK: %d \n", ack); + _i2c.start(); + ack = _i2c.write(0x3D); + temp[0] = _i2c.read(0); + _i2c.stop(); + //SEGGER_RTT_printf(0, "readbyte read ACK: %d \n", ack); + return temp[0]; +} + + +void LIS2MDL::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, char * dest) +{ + int ack = 0; + _i2c.start(); + ack = _i2c.write(0x3C); + //SEGGER_RTT_printf(0, "address ACK: %d \n", ack); + ack = _i2c.write(subAddress); + //SEGGER_RTT_printf(0, "subaddr ACK: %d \n", ack); + ack = _i2c.read(address, &dest[0], count); + //SEGGER_RTT_printf(0, "read ACK: %d \n", ack); + _i2c.stop(); +} + +void LIS2MDL::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) +{ + _i2c.start(); + _i2c.write(0x3C); + _i2c.write(subAddress); + _i2c.write(data); + _i2c.stop(); +} \ No newline at end of file