Naresh Krish
/
itracker-mbed-os-example-lis2mdl
Sample program on how to use the LIS2MDL sensor on the RAKWirelss iTracker module
Diff: LIS2MDL/LIS2MDL.cpp
- Revision:
- 1:4a8ff3cf48ab
- Parent:
- 0:10c0e81df4ba
diff -r 10c0e81df4ba -r 4a8ff3cf48ab LIS2MDL/LIS2MDL.cpp --- a/LIS2MDL/LIS2MDL.cpp Mon Feb 12 04:58:22 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,225 +0,0 @@ -/* - 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