Diff: lis3mdl.cpp

Revision:

1:898554a35638

Parent:

0:32f3441153b5

--- a/lis3mdl.cpp	Thu Oct 06 16:45:48 2016 +0000
+++ b/lis3mdl.cpp	Thu Oct 06 23:28:08 2016 +0000
@@ -48,27 +48,18 @@
         Init();
 }
 /**
- * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in mgauss.
+ * @brief Read data from LIS3MDL Magnetic sensor and calculate Magnetic in microTeslas.
  * @param float *pfData
  * @retval None.
  */
-void LIS3MDL::GetAxes(AxesRaw_TypeDef *pData)
+void LIS3MDL::readMag(void)
 {
-    uint8_t tempReg = 0x00;
-    int16_t pDataRaw[3];
+   
     float sensitivity = 0;
-    int ret;
-
-    GetAxesRaw(pDataRaw);
+   
 
-    ret = _i2c.i2c_read(&tempReg, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
-
-    if (ret == 0)
-    {
-      tempReg &= LIS3MDL_M_FS_MASK;
-
-      switch(tempReg)
-      {
+    GetAxesRaw();
+/*
         case LIS3MDL_M_FS_4:
           sensitivity = 0.14;
           break;
@@ -82,11 +73,11 @@
           sensitivity = 0.58;
           break;
       }
-    }
-
-    pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity);
-    pData->AXIS_Y = (int32_t)(pDataRaw[1] * sensitivity);
-    pData->AXIS_Z = (int32_t)(pDataRaw[2] * sensitivity);
+    }*/
+    sensitivity = 0.12207;
+    mx = mx_raw * sensitivity;
+    my = my_raw * sensitivity;
+    mz = mz_raw * sensitivity;
   
 }
 
@@ -95,31 +86,18 @@
  * @param float *pfData
  * @retval None.
  */
-void LIS3MDL::GetAxesRaw(int16_t *pData)
+void LIS3MDL::GetAxesRaw()
 {
-    uint8_t tempReg[2] = {0,0};
-    int ret;
-
-    pData[0] = pData[1] = pData[2] = 0;
+    char data[6];
+    
+    char subAddressXL = LIS3MDL_M_OUT_X_L_M;
     
-    ret = _i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_X_L_M + 0x80, 2);
-
-    if (ret == 0)
-    {
-      pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-      ret = _i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_Y_L_M + 0x80, 2);
-    }
-    
-    if (ret == 0)
-    {
-      pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-      ret = _i2c.i2c_read(&tempReg[0], LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_OUT_Z_L_M + 0x80, 2);
-    }
-    
-    if (ret == 0)
-    {
-      pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
-    }
+    _i2c.write(LIS3MDL_M_MEMS_ADDRESS, &subAddressXL, 1, true);
+    _i2c.read(LIS3MDL_M_MEMS_ADDRESS, data, 6);
+ 
+    mx_raw = data[0] | (data[1] << 8);
+    my_raw = data[2] | (data[3] << 8);
+    mz_raw = data[4] | (data[5] << 8);
 }
 
 /**
@@ -129,14 +107,16 @@
  */
 uint8_t LIS3MDL::ReadID(void)
 {
-    uint8_t tmp=0x00;
+    char data[1];
     int ret;
+    char subAddress = LIS3MDL_M_WHO_AM_I_ADDR;
     
     /* Read WHO I AM register */
-    ret = _i2c.i2c_read(&tmp, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_WHO_AM_I_ADDR, 1);
+    _i2c.write(LIS3MDL_M_MEMS_ADDRESS, &subAddress, 1, true);
+    ret=_i2c.read(LIS3MDL_M_MEMS_ADDRESS, data, 1);
 
     /* Return the ID */
-    return ((ret == 0) ? (uint8_t)tmp : 0);
+    return ((ret == 0) ? (uint8_t)data[0] : 0);
 }
 
 /**
@@ -146,62 +126,43 @@
  */
 void LIS3MDL::Init() {
     
-    uint8_t tmp1 = 0x00;
-    int ret;
+    char tmp1[2];
     
     /****** Magnetic sensor *******/
-
-    ret = _i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG3_M, 1);
-
+ 
     /* Conversion mode selection */
-    if (ret == 0)
-    {
-      tmp1 &= ~(LIS3MDL_M_MD_MASK);
-      tmp1 |= LIS3MDL_M_MD_CONTINUOUS;
-      ret = _i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG3_M, 1);
-    }
+      tmp1[0] = LIS3MDL_M_CTRL_REG3_M;
+      tmp1[1] = 0x00;
+      
+      _i2c.write(LIS3MDL_M_MEMS_ADDRESS, tmp1, 2);
+   
 
-    if (ret == 0)
-    {
-      ret = _i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG1_M, 1);
-    }
+        //OM=UHP for XY
+        //FAST_ODR enabled - 155Hz
+      tmp1[0] = LIS3MDL_M_CTRL_REG1_M;
+      tmp1[1] = 0x8E;
+
+      _i2c.write(LIS3MDL_M_MEMS_ADDRESS, tmp1, 2);
     
-    if (ret == 0)
-    {
-      /* Output data rate selection */
-      tmp1 &= ~(LIS3MDL_M_DO_MASK);
-      tmp1 |= LIS3MDL_M_DO_80;
+      /* Full scale selection */
+      tmp1[0] = LIS3MDL_M_CTRL_REG2_M;
+      tmp1[1] = 0x00;
 
-      /* X and Y axes Operative mode selection */
-      tmp1 &= ~(LIS3MDL_M_OM_MASK);
-      tmp1 |= LIS3MDL_M_OM_HP;
-
-      ret = _i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG1_M, 1);
-    }
+      _i2c.write(LIS3MDL_M_MEMS_ADDRESS, tmp1, 2);
     
-    if (ret == 0)
-    {
-      ret = _i2c.i2c_read(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
-    }
-    
-    if (ret == 0)
-    {
-      /* Full scale selection */
-      tmp1 &= ~(LIS3MDL_M_FS_MASK);
-      tmp1 |= LIS3MDL_M_FS_4;
+      /*Fast Z*/
+      tmp1[0] = LIS3MDL_M_CTRL_REG4_M;
+      tmp1[1] = 0x0C;
 
-      ret = _i2c.i2c_write(&tmp1, LIS3MDL_M_MEMS_ADDRESS, LIS3MDL_M_CTRL_REG2_M, 1);
-    }
+      _i2c.write(LIS3MDL_M_MEMS_ADDRESS, tmp1, 2);
     
     /******************************/
     
-    if (ret == 0)
-    {
       if(ReadID() == I_AM_LIS3MDL_M)
       {
           LIS3MDLInitialized = 1;
       }
-    }
+    
 
     return;
 }