Naresh Krish / Mbed OS itracker-mbed-os-example-lis2mdl

Sample program on how to use the LIS2MDL sensor on the RAKWirelss iTracker module

Dependencies:   LIS2MDL

LIS2MDL/LIS2MDL.cpp@0:10c0e81df4ba, 2018-02-12 (annotated)

Committer:
knaresh89
Date:
Mon Feb 12 04:58:22 2018 +0000
Revision:
0:10c0e81df4ba
Sample app for the LIS2MDL sensor on the RAKWireless iTracker module

Who changed what in which revision?

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