Naresh Krish / LIS2MDL

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);
    }
}

LIS2MDL.cpp

Committer:
knaresh89
Date:
2018-02-12
Revision:
0:d7138994c637
Child:
1:d7ea67f32b32

File content as of revision 0:d7138994c637:

/* 
  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();
}