Naresh Krish
/
itracker-mbed-os-example-lis2mdl
Sample program on how to use the LIS2MDL sensor on the RAKWirelss iTracker module
LIS2MDL/LIS2MDL.cpp
- Committer:
- knaresh89
- Date:
- 2018-02-12
- Revision:
- 0:10c0e81df4ba
File content as of revision 0:10c0e81df4ba:
/* 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(); }