Diff: LIS2MDL/LIS2MDL.cpp

Revision:

1:4a8ff3cf48ab

Parent:

0:10c0e81df4ba

--- 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