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@0:d7138994c637, 2018-02-12 (annotated)
- Committer:
- knaresh89
- Date:
- Mon Feb 12 04:59:47 2018 +0000
- Revision:
- 0:d7138994c637
- Child:
- 1:d7ea67f32b32
LIS2MDL sensor driver for mbed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
knaresh89 | 0:d7138994c637 | 1 | /* |
knaresh89 | 0:d7138994c637 | 2 | Created by Naresh Krishnamoorthy |
knaresh89 | 0:d7138994c637 | 3 | |
knaresh89 | 0:d7138994c637 | 4 | The LIS2MDL is a low power magnetometer, here used as 3 DoF solution. |
knaresh89 | 0:d7138994c637 | 5 | Library may be used freely and without limit with attribution. |
knaresh89 | 0:d7138994c637 | 6 | |
knaresh89 | 0:d7138994c637 | 7 | */ |
knaresh89 | 0:d7138994c637 | 8 | |
knaresh89 | 0:d7138994c637 | 9 | #include "LIS2MDL.h" |
knaresh89 | 0:d7138994c637 | 10 | #include "SEGGER_RTT.h" |
knaresh89 | 0:d7138994c637 | 11 | |
knaresh89 | 0:d7138994c637 | 12 | LIS2MDL::LIS2MDL (I2C& p_i2c, uint8_t addr) |
knaresh89 | 0:d7138994c637 | 13 | : _i2c(p_i2c) |
knaresh89 | 0:d7138994c637 | 14 | { |
knaresh89 | 0:d7138994c637 | 15 | _i2c.frequency(400000); |
knaresh89 | 0:d7138994c637 | 16 | } |
knaresh89 | 0:d7138994c637 | 17 | |
knaresh89 | 0:d7138994c637 | 18 | void LIS2MDL::reset() |
knaresh89 | 0:d7138994c637 | 19 | { |
knaresh89 | 0:d7138994c637 | 20 | // reset device |
knaresh89 | 0:d7138994c637 | 21 | uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A); |
knaresh89 | 0:d7138994c637 | 22 | writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL |
knaresh89 | 0:d7138994c637 | 23 | wait(0.1); |
knaresh89 | 0:d7138994c637 | 24 | writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL |
knaresh89 | 0:d7138994c637 | 25 | wait(0.1); // Wait for all registers to reset |
knaresh89 | 0:d7138994c637 | 26 | } |
knaresh89 | 0:d7138994c637 | 27 | |
knaresh89 | 0:d7138994c637 | 28 | void LIS2MDL::offsetBias(float * dest1, float * dest2) |
knaresh89 | 0:d7138994c637 | 29 | { |
knaresh89 | 0:d7138994c637 | 30 | int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0}; |
knaresh89 | 0:d7138994c637 | 31 | int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0}; |
knaresh89 | 0:d7138994c637 | 32 | float _mRes = 0.0015f; |
knaresh89 | 0:d7138994c637 | 33 | |
knaresh89 | 0:d7138994c637 | 34 | SEGGER_RTT_printf(0, "Calculate mag offset bias: move all around to sample the complete response surface!"); |
knaresh89 | 0:d7138994c637 | 35 | wait(4); |
knaresh89 | 0:d7138994c637 | 36 | |
knaresh89 | 0:d7138994c637 | 37 | for (int ii = 0; ii < 4000; ii++) { |
knaresh89 | 0:d7138994c637 | 38 | readData(mag_temp); |
knaresh89 | 0:d7138994c637 | 39 | for (int jj = 0; jj < 3; jj++) { |
knaresh89 | 0:d7138994c637 | 40 | if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; |
knaresh89 | 0:d7138994c637 | 41 | if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; |
knaresh89 | 0:d7138994c637 | 42 | } |
knaresh89 | 0:d7138994c637 | 43 | wait(0.12); |
knaresh89 | 0:d7138994c637 | 44 | } |
knaresh89 | 0:d7138994c637 | 45 | |
knaresh89 | 0:d7138994c637 | 46 | _mRes = 0.0015f; // fixed sensitivity |
knaresh89 | 0:d7138994c637 | 47 | // Get hard iron correction |
knaresh89 | 0:d7138994c637 | 48 | mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts |
knaresh89 | 0:d7138994c637 | 49 | mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts |
knaresh89 | 0:d7138994c637 | 50 | mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts |
knaresh89 | 0:d7138994c637 | 51 | |
knaresh89 | 0:d7138994c637 | 52 | dest1[0] = (float) mag_bias[0] * _mRes; // save mag biases in G for main program |
knaresh89 | 0:d7138994c637 | 53 | dest1[1] = (float) mag_bias[1] * _mRes; |
knaresh89 | 0:d7138994c637 | 54 | dest1[2] = (float) mag_bias[2] * _mRes; |
knaresh89 | 0:d7138994c637 | 55 | |
knaresh89 | 0:d7138994c637 | 56 | // Get soft iron correction estimate |
knaresh89 | 0:d7138994c637 | 57 | mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts |
knaresh89 | 0:d7138994c637 | 58 | mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts |
knaresh89 | 0:d7138994c637 | 59 | mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts |
knaresh89 | 0:d7138994c637 | 60 | |
knaresh89 | 0:d7138994c637 | 61 | float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2]; |
knaresh89 | 0:d7138994c637 | 62 | avg_rad /= 3.0f; |
knaresh89 | 0:d7138994c637 | 63 | |
knaresh89 | 0:d7138994c637 | 64 | dest2[0] = avg_rad/((float)mag_scale[0]); |
knaresh89 | 0:d7138994c637 | 65 | dest2[1] = avg_rad/((float)mag_scale[1]); |
knaresh89 | 0:d7138994c637 | 66 | dest2[2] = avg_rad/((float)mag_scale[2]); |
knaresh89 | 0:d7138994c637 | 67 | |
knaresh89 | 0:d7138994c637 | 68 | SEGGER_RTT_printf(0, "Mag Calibration done!"); |
knaresh89 | 0:d7138994c637 | 69 | } |
knaresh89 | 0:d7138994c637 | 70 | |
knaresh89 | 0:d7138994c637 | 71 | |
knaresh89 | 0:d7138994c637 | 72 | void LIS2MDL::init(uint8_t MODR) |
knaresh89 | 0:d7138994c637 | 73 | { |
knaresh89 | 0:d7138994c637 | 74 | |
knaresh89 | 0:d7138994c637 | 75 | // enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00) |
knaresh89 | 0:d7138994c637 | 76 | writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR<<2); |
knaresh89 | 0:d7138994c637 | 77 | |
knaresh89 | 0:d7138994c637 | 78 | // enable low pass filter (bit 0 == 1), set to ODR/4 |
knaresh89 | 0:d7138994c637 | 79 | writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01); |
knaresh89 | 0:d7138994c637 | 80 | |
knaresh89 | 0:d7138994c637 | 81 | // enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1) |
knaresh89 | 0:d7138994c637 | 82 | writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10); |
knaresh89 | 0:d7138994c637 | 83 | |
knaresh89 | 0:d7138994c637 | 84 | } |
knaresh89 | 0:d7138994c637 | 85 | |
knaresh89 | 0:d7138994c637 | 86 | uint8_t LIS2MDL::getChipID() |
knaresh89 | 0:d7138994c637 | 87 | { |
knaresh89 | 0:d7138994c637 | 88 | uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I); |
knaresh89 | 0:d7138994c637 | 89 | SEGGER_RTT_printf(0, "Address: %d \n", c); |
knaresh89 | 0:d7138994c637 | 90 | return c; |
knaresh89 | 0:d7138994c637 | 91 | } |
knaresh89 | 0:d7138994c637 | 92 | |
knaresh89 | 0:d7138994c637 | 93 | uint8_t LIS2MDL::status() |
knaresh89 | 0:d7138994c637 | 94 | { |
knaresh89 | 0:d7138994c637 | 95 | // Read the status register of the altimeter |
knaresh89 | 0:d7138994c637 | 96 | uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG); |
knaresh89 | 0:d7138994c637 | 97 | SEGGER_RTT_printf(0, "LIS2MDL status : %d \n", temp); |
knaresh89 | 0:d7138994c637 | 98 | return temp; |
knaresh89 | 0:d7138994c637 | 99 | } |
knaresh89 | 0:d7138994c637 | 100 | |
knaresh89 | 0:d7138994c637 | 101 | void LIS2MDL::readData(int16_t * destination) |
knaresh89 | 0:d7138994c637 | 102 | { |
knaresh89 | 0:d7138994c637 | 103 | char rawData[6]; // x/y/z mag register data stored here |
knaresh89 | 0:d7138994c637 | 104 | readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8, &rawData[0]); // Read the 6 raw data registers into data array |
knaresh89 | 0:d7138994c637 | 105 | |
knaresh89 | 0:d7138994c637 | 106 | destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
knaresh89 | 0:d7138994c637 | 107 | destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
knaresh89 | 0:d7138994c637 | 108 | destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
knaresh89 | 0:d7138994c637 | 109 | |
knaresh89 | 0:d7138994c637 | 110 | SEGGER_RTT_printf(0, "x: %d y: %d z: %d \n", destination[0], destination[1], destination[2]); |
knaresh89 | 0:d7138994c637 | 111 | } |
knaresh89 | 0:d7138994c637 | 112 | |
knaresh89 | 0:d7138994c637 | 113 | int16_t LIS2MDL::readTemperature() |
knaresh89 | 0:d7138994c637 | 114 | { |
knaresh89 | 0:d7138994c637 | 115 | char rawData[2]; // x/y/z mag register data stored here |
knaresh89 | 0:d7138994c637 | 116 | readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2, &rawData[0]); // Read the 8 raw data registers into data array |
knaresh89 | 0:d7138994c637 | 117 | |
knaresh89 | 0:d7138994c637 | 118 | int16_t temp = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
knaresh89 | 0:d7138994c637 | 119 | SEGGER_RTT_printf(0, "LIS2MDL temp is : %d \n", temp); |
knaresh89 | 0:d7138994c637 | 120 | return temp; |
knaresh89 | 0:d7138994c637 | 121 | } |
knaresh89 | 0:d7138994c637 | 122 | |
knaresh89 | 0:d7138994c637 | 123 | void LIS2MDL::lis2mdlSelfCheck() |
knaresh89 | 0:d7138994c637 | 124 | { |
knaresh89 | 0:d7138994c637 | 125 | int16_t temp[3] = {0, 0, 0}; |
knaresh89 | 0:d7138994c637 | 126 | float magTest[3] = {0., 0., 0.}; |
knaresh89 | 0:d7138994c637 | 127 | float magNom[3] = {0., 0., 0.}; |
knaresh89 | 0:d7138994c637 | 128 | int32_t sum[3] = {0, 0, 0}; |
knaresh89 | 0:d7138994c637 | 129 | float _mRes = 0.0015f; |
knaresh89 | 0:d7138994c637 | 130 | |
knaresh89 | 0:d7138994c637 | 131 | // first, get average response with self test disabled |
knaresh89 | 0:d7138994c637 | 132 | for (int ii = 0; ii < 50; ii++) |
knaresh89 | 0:d7138994c637 | 133 | { |
knaresh89 | 0:d7138994c637 | 134 | readData(temp); |
knaresh89 | 0:d7138994c637 | 135 | sum[0] += temp[0]; |
knaresh89 | 0:d7138994c637 | 136 | sum[1] += temp[1]; |
knaresh89 | 0:d7138994c637 | 137 | sum[2] += temp[2]; |
knaresh89 | 0:d7138994c637 | 138 | wait(0.1); |
knaresh89 | 0:d7138994c637 | 139 | } |
knaresh89 | 0:d7138994c637 | 140 | |
knaresh89 | 0:d7138994c637 | 141 | magNom[0] = (float) sum[0] / 50.0f; |
knaresh89 | 0:d7138994c637 | 142 | magNom[1] = (float) sum[1] / 50.0f; |
knaresh89 | 0:d7138994c637 | 143 | magNom[2] = (float) sum[2] / 50.0f; |
knaresh89 | 0:d7138994c637 | 144 | |
knaresh89 | 0:d7138994c637 | 145 | uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C); |
knaresh89 | 0:d7138994c637 | 146 | writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test |
knaresh89 | 0:d7138994c637 | 147 | wait(0.1); // let mag respond |
knaresh89 | 0:d7138994c637 | 148 | |
knaresh89 | 0:d7138994c637 | 149 | sum[0] = 0; |
knaresh89 | 0:d7138994c637 | 150 | sum[1] = 0; |
knaresh89 | 0:d7138994c637 | 151 | sum[2] = 0; |
knaresh89 | 0:d7138994c637 | 152 | for (int ii = 0; ii < 50; ii++) |
knaresh89 | 0:d7138994c637 | 153 | { |
knaresh89 | 0:d7138994c637 | 154 | readData(temp); |
knaresh89 | 0:d7138994c637 | 155 | sum[0] += temp[0]; |
knaresh89 | 0:d7138994c637 | 156 | sum[1] += temp[1]; |
knaresh89 | 0:d7138994c637 | 157 | sum[2] += temp[2]; |
knaresh89 | 0:d7138994c637 | 158 | wait(0.1); |
knaresh89 | 0:d7138994c637 | 159 | } |
knaresh89 | 0:d7138994c637 | 160 | |
knaresh89 | 0:d7138994c637 | 161 | magTest[0] = (float) sum[0] / 50.0f; |
knaresh89 | 0:d7138994c637 | 162 | magTest[1] = (float) sum[1] / 50.0f; |
knaresh89 | 0:d7138994c637 | 163 | magTest[2] = (float) sum[2] / 50.0f; |
knaresh89 | 0:d7138994c637 | 164 | |
knaresh89 | 0:d7138994c637 | 165 | writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode |
knaresh89 | 0:d7138994c637 | 166 | wait(0.1); // let mag respond |
knaresh89 | 0:d7138994c637 | 167 | |
knaresh89 | 0:d7138994c637 | 168 | /*SEGGER_RTT.printf(0, "Mag Self Test: \n"); |
knaresh89 | 0:d7138994c637 | 169 | SEGGER_RTT.printf(0, "Mx results:"); |
knaresh89 | 0:d7138994c637 | 170 | SEGGER_RTT.printf(0, " %f ", (magTest[0] - magNom[0]) * _mRes * 1000.0); |
knaresh89 | 0:d7138994c637 | 171 | SEGGER_RTT.printf(0, " mG \n"); |
knaresh89 | 0:d7138994c637 | 172 | SEGGER_RTT.printf(0, "My results:"); |
knaresh89 | 0:d7138994c637 | 173 | SEGGER_RTT.printf(0, " %f \n", (magTest[0] - magNom[0]) * _mRes * 1000.0); |
knaresh89 | 0:d7138994c637 | 174 | SEGGER_RTT.printf(0, "Mz results:"); |
knaresh89 | 0:d7138994c637 | 175 | SEGGER_RTT.printf(0, " %f \n", (magTest[1] - magNom[1]) * _mRes * 1000.0); |
knaresh89 | 0:d7138994c637 | 176 | SEGGER_RTT.printf(0, "Should be between 15 and 500 mG \n");*/ |
knaresh89 | 0:d7138994c637 | 177 | wait(2.0); // give some time to read the screen |
knaresh89 | 0:d7138994c637 | 178 | } |
knaresh89 | 0:d7138994c637 | 179 | |
knaresh89 | 0:d7138994c637 | 180 | |
knaresh89 | 0:d7138994c637 | 181 | |
knaresh89 | 0:d7138994c637 | 182 | |
knaresh89 | 0:d7138994c637 | 183 | //******************************************* |
knaresh89 | 0:d7138994c637 | 184 | // I2C read/write functions for the LIS2MDL |
knaresh89 | 0:d7138994c637 | 185 | //******************************************* |
knaresh89 | 0:d7138994c637 | 186 | |
knaresh89 | 0:d7138994c637 | 187 | uint8_t LIS2MDL::readByte(uint8_t address, char subAddress) |
knaresh89 | 0:d7138994c637 | 188 | { |
knaresh89 | 0:d7138994c637 | 189 | char temp[1]; |
knaresh89 | 0:d7138994c637 | 190 | int ack = 0; |
knaresh89 | 0:d7138994c637 | 191 | _i2c.start(); |
knaresh89 | 0:d7138994c637 | 192 | ack = _i2c.write(0x3C); |
knaresh89 | 0:d7138994c637 | 193 | //SEGGER_RTT_printf(0, "address ACK: %d \n", ack); |
knaresh89 | 0:d7138994c637 | 194 | ack = _i2c.write(subAddress); |
knaresh89 | 0:d7138994c637 | 195 | //SEGGER_RTT_printf(0, "sub address ACK: %d \n", ack); |
knaresh89 | 0:d7138994c637 | 196 | _i2c.start(); |
knaresh89 | 0:d7138994c637 | 197 | ack = _i2c.write(0x3D); |
knaresh89 | 0:d7138994c637 | 198 | temp[0] = _i2c.read(0); |
knaresh89 | 0:d7138994c637 | 199 | _i2c.stop(); |
knaresh89 | 0:d7138994c637 | 200 | //SEGGER_RTT_printf(0, "readbyte read ACK: %d \n", ack); |
knaresh89 | 0:d7138994c637 | 201 | return temp[0]; |
knaresh89 | 0:d7138994c637 | 202 | } |
knaresh89 | 0:d7138994c637 | 203 | |
knaresh89 | 0:d7138994c637 | 204 | |
knaresh89 | 0:d7138994c637 | 205 | void LIS2MDL::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, char * dest) |
knaresh89 | 0:d7138994c637 | 206 | { |
knaresh89 | 0:d7138994c637 | 207 | int ack = 0; |
knaresh89 | 0:d7138994c637 | 208 | _i2c.start(); |
knaresh89 | 0:d7138994c637 | 209 | ack = _i2c.write(0x3C); |
knaresh89 | 0:d7138994c637 | 210 | //SEGGER_RTT_printf(0, "address ACK: %d \n", ack); |
knaresh89 | 0:d7138994c637 | 211 | ack = _i2c.write(subAddress); |
knaresh89 | 0:d7138994c637 | 212 | //SEGGER_RTT_printf(0, "subaddr ACK: %d \n", ack); |
knaresh89 | 0:d7138994c637 | 213 | ack = _i2c.read(address, &dest[0], count); |
knaresh89 | 0:d7138994c637 | 214 | //SEGGER_RTT_printf(0, "read ACK: %d \n", ack); |
knaresh89 | 0:d7138994c637 | 215 | _i2c.stop(); |
knaresh89 | 0:d7138994c637 | 216 | } |
knaresh89 | 0:d7138994c637 | 217 | |
knaresh89 | 0:d7138994c637 | 218 | void LIS2MDL::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) |
knaresh89 | 0:d7138994c637 | 219 | { |
knaresh89 | 0:d7138994c637 | 220 | _i2c.start(); |
knaresh89 | 0:d7138994c637 | 221 | _i2c.write(0x3C); |
knaresh89 | 0:d7138994c637 | 222 | _i2c.write(subAddress); |
knaresh89 | 0:d7138994c637 | 223 | _i2c.write(data); |
knaresh89 | 0:d7138994c637 | 224 | _i2c.stop(); |
knaresh89 | 0:d7138994c637 | 225 | } |