git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9627 35acf78f-673a-0410-8e92-d51de3d6d3f4

12 files changed, 6129 insertions, 6129 deletions

diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index d36399d91..3e9c93640 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -1,482 +1,482 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    l3gd20.c
- * @brief   L3GD20 MEMS interface module code.
- *
- * @addtogroup l3gd20
- * @{
- */
-
-#include "hal.h"
-#include "l3gd20.h"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-#define  L3GD20_SENS_250DPS                 ((float)0.00875f)
-#define  L3GD20_SENS_500DPS                 ((float)0.01750f)
-#define  L3GD20_SENS_2000DPS                ((float)0.07000f)
-
-#define  L3GD20_DI                          ((uint8_t)0xFF)
-#define  L3GD20_DI_0                        ((uint8_t)0x01)
-#define  L3GD20_DI_1                        ((uint8_t)0x02)
-#define  L3GD20_DI_2                        ((uint8_t)0x04)
-#define  L3GD20_DI_3                        ((uint8_t)0x08)
-#define  L3GD20_DI_4                        ((uint8_t)0x10)
-#define  L3GD20_DI_5                        ((uint8_t)0x20)
-#define  L3GD20_DI_6                        ((uint8_t)0x40)
-#define  L3GD20_DI_7                        ((uint8_t)0x80)
-
-#define  L3GD20_AD                          ((uint8_t)0x3F)
-#define  L3GD20_AD_0                        ((uint8_t)0x01)
-#define  L3GD20_AD_1                        ((uint8_t)0x02)
-#define  L3GD20_AD_2                        ((uint8_t)0x04)
-#define  L3GD20_AD_3                        ((uint8_t)0x08)
-#define  L3GD20_AD_4                        ((uint8_t)0x10)
-#define  L3GD20_AD_5                        ((uint8_t)0x20)
-#define  L3GD20_MS                          ((uint8_t)0x40)
-#define  L3GD20_RW                          ((uint8_t)0x80)
-
-#define  L3GD20_AD_WHO_AM_I                 ((uint8_t)0x0F)
-#define  L3GD20_AD_CTRL_REG1                ((uint8_t)0x20)
-#define  L3GD20_AD_CTRL_REG2                ((uint8_t)0x21)
-#define  L3GD20_AD_CTRL_REG3                ((uint8_t)0x22)
-#define  L3GD20_AD_CTRL_REG4                ((uint8_t)0x23)
-#define  L3GD20_AD_CTRL_REG5                ((uint8_t)0x24)
-#define  L3GD20_AD_REFERENCE                ((uint8_t)0x25)
-#define  L3GD20_AD_OUT_TEMP                 ((uint8_t)0x26)
-#define  L3GD20_AD_STATUS_REG               ((uint8_t)0x27)
-#define  L3GD20_AD_OUT_X_L                  ((uint8_t)0x28)
-#define  L3GD20_AD_OUT_X_H                  ((uint8_t)0x29)
-#define  L3GD20_AD_OUT_Y_L                  ((uint8_t)0x2A)
-#define  L3GD20_AD_OUT_Y_H                  ((uint8_t)0x2B)
-#define  L3GD20_AD_OUT_Z_L                  ((uint8_t)0x2C)
-#define  L3GD20_AD_OUT_Z_H                  ((uint8_t)0x2D)
-#define  L3GD20_AD_FIFO_CTRL_REG            ((uint8_t)0x2E)
-#define  L3GD20_AD_FIFO_SRC_REG             ((uint8_t)0x2F)
-#define  L3GD20_AD_INT1_CFG                 ((uint8_t)0x30)
-#define  L3GD20_AD_INT1_SRC                 ((uint8_t)0x31)
-#define  L3GD20_AD_INT1_TSH_XH              ((uint8_t)0x32)
-#define  L3GD20_AD_INT1_TSH_XL              ((uint8_t)0x33)
-#define  L3GD20_AD_INT1_TSH_YH              ((uint8_t)0x34)
-#define  L3GD20_AD_INT1_TSH_YL              ((uint8_t)0x35)
-#define  L3GD20_AD_INT1_TSH_ZH              ((uint8_t)0x36)
-#define  L3GD20_AD_INT1_TSH_ZL              ((uint8_t)0x37)
-#define  L3GD20_AD_INT1_DURATION            ((uint8_t)0x38)
-
-#define  L3GD20_CTRL_REG4_FS_MASK           ((uint8_t)0x30)
-
-/*===========================================================================*/
-/* Driver exported variables.                                                */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local variables and types.                                         */
-/*===========================================================================*/
-
-/**
- * @brief  L3GD20 Power Mode
- */
-typedef enum {
-  L3GD20_PM_POWER_DOWN   = 0x00,    /**< Power down enabled.                */          
-  L3GD20_PM_SLEEP_NORMAL = 0x08     /**< Normal operation mode.             */          
-}l3gd20_pm_t;
-
-/*===========================================================================*/
-/* Driver local functions.                                                   */
-/*===========================================================================*/
-
-#if (L3GD20_USE_SPI) || defined(__DOXYGEN__)
-/**
- * @brief   Reads a generic register value using SPI.
- * @pre     The SPI interface must be initialized and the driver started.
- *
- * @param[in] spip      pointer to the SPI interface
- * @param[in] reg       register number
- * @return              register value.
- */
-static uint8_t l3gd20SPIReadRegister(SPIDriver *spip, uint8_t reg) {
-  uint8_t txbuf[2] = {L3GD20_RW | reg, 0xFF};
-  uint8_t rxbuf[2] = {0x00, 0x00};
-  spiSelect(spip);
-  spiExchange(spip, 2, txbuf, rxbuf);
-  spiUnselect(spip);
-  return rxbuf[1];
-}
-
-/**
- * @brief   Writes a value into a generic register using SPI.
- * @pre     The SPI interface must be initialized and the driver started.
- *
- * @param[in] spip      pointer to the SPI interface
- * @param[in] reg       register number
- * @param[in] value     register value.
- */
-static void l3gd20SPIWriteRegister(SPIDriver *spip, uint8_t reg,
-                                   uint8_t value) {
-
-  switch (reg) {
-    default:
-      /* Reserved register must not be written, according to the datasheet
-       * this could permanently damage the device.
-       */
-      osalDbgAssert(FALSE, "l3gd20SPIWriteRegister(), reserved register");
-    case L3GD20_AD_WHO_AM_I:
-    case L3GD20_AD_OUT_TEMP :
-    case L3GD20_AD_STATUS_REG:
-    case L3GD20_AD_OUT_X_L:
-    case L3GD20_AD_OUT_X_H:
-    case L3GD20_AD_OUT_Y_L:
-    case L3GD20_AD_OUT_Y_H:
-    case L3GD20_AD_OUT_Z_L:
-    case L3GD20_AD_OUT_Z_H:
-    case L3GD20_AD_FIFO_SRC_REG:
-    case L3GD20_AD_INT1_SRC:
-    /* Read only registers cannot be written, the command is ignored.*/
-      return;
-    case L3GD20_AD_CTRL_REG1:
-    case L3GD20_AD_CTRL_REG2:
-    case L3GD20_AD_CTRL_REG3:
-    case L3GD20_AD_CTRL_REG4:
-    case L3GD20_AD_CTRL_REG5:
-    case L3GD20_AD_REFERENCE:
-    case L3GD20_AD_FIFO_CTRL_REG:
-    case L3GD20_AD_INT1_CFG:
-    case L3GD20_AD_INT1_TSH_XH:
-    case L3GD20_AD_INT1_TSH_XL:
-    case L3GD20_AD_INT1_TSH_YH:
-    case L3GD20_AD_INT1_TSH_YL:
-    case L3GD20_AD_INT1_TSH_ZH:
-    case L3GD20_AD_INT1_TSH_ZL:
-    case L3GD20_AD_INT1_DURATION:
-      spiSelect(spip);
-      uint8_t txbuf[2] = {reg, value};
-      spiSend(spip, 2, txbuf);
-      spiUnselect(spip);
-  }
-}
-#endif /* L3GD20_USE_SPI */
-
-/*
- * Interface implementation.
- */
-static size_t get_axes_number(void *ip) {
-
-  osalDbgCheck(ip != NULL);
-  return L3GD20_NUMBER_OF_AXES;
-}
-
-static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {
-  int16_t tmp;
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-
-  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
-              "read_raw(), invalid state");
-
-#if L3GD20_USE_SPI
-  osalDbgAssert((((L3GD20Driver *)ip)->config->spip->state == SPI_READY),
-                "read_raw(), channel not ready");
-#if	L3GD20_SHARED_SPI
-  spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
-  spiStart(((L3GD20Driver *)ip)->config->spip,
-           ((L3GD20Driver *)ip)->config->spicfg);
-#endif /* L3GD20_SHARED_SPI */   
-  if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_X){
-    tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
-                                  L3GD20_AD_OUT_X_L);
-    tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
-                                   L3GD20_AD_OUT_X_H) << 8;
-    axes[0] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[0];
-  }
-  if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_Y){
-    tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
-                                  L3GD20_AD_OUT_Y_L);
-    tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
-                                   L3GD20_AD_OUT_Y_H) << 8;
-    axes[1] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[1];
-  }
-  if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_Z){
-    tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
-                                  L3GD20_AD_OUT_Z_L);
-    tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
-                                   L3GD20_AD_OUT_Z_H) << 8;
-    axes[2] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[2];
-  }
-#if	L3GD20_SHARED_SPI
-  spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
-#endif /* L3GD20_SHARED_SPI */   
-#endif /* L3GD20_USE_SPI */ 
-  return MSG_OK;
-}
-
-static msg_t read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[L3GD20_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-
-  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
-              "read_cooked(), invalid state");
-
-  msg = read_raw(ip, raw);
-  for(i = 0; i < L3GD20_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] * ((L3GD20Driver *)ip)->sensitivity[i];
-  }
-  return msg;
-}
-
-static msg_t sample_bias(void *ip) {
-  uint32_t i, j;
-  int32_t raw[L3GD20_NUMBER_OF_AXES];
-  int32_t buff[L3GD20_NUMBER_OF_AXES] = {0, 0, 0};
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
-                "sample_bias(), invalid state");
-
-  for(i = 0; i < L3GD20_BIAS_ACQ_TIMES; i++){
-    read_raw(ip, raw);
-    for(j = 0; j < L3GD20_NUMBER_OF_AXES; j++){
-      buff[j] += raw[j];
-    }
-    osalThreadSleepMicroseconds(L3GD20_BIAS_SETTLING_uS);
-  }
-
-  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++){
-    ((L3GD20Driver *)ip)->bias[i] = buff[i] / L3GD20_BIAS_ACQ_TIMES;
-  }
-  return MSG_OK;
-}
-
-static msg_t set_bias(void *ip, int32_t *bp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (bp !=NULL));
-
-  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||
-                (((L3GD20Driver *)ip)->state == L3GD20_STOP),
-                "set_bias(), invalid state");
-  
-  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
-    ((L3GD20Driver *)ip)->bias[i] = bp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t reset_bias(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||
-                (((L3GD20Driver *)ip)->state == L3GD20_STOP),
-              "reset_bias(), invalid state");
-
-  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-    ((L3GD20Driver *)ip)->bias[i] = 0;
-  return MSG_OK;
-}
-
-static msg_t set_sensivity(void *ip, float *sp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (sp !=NULL));
-
-  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
-                "set_sensivity(), invalid state");
-  
-  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
-    ((L3GD20Driver *)ip)->sensitivity[i] = sp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t reset_sensivity(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
-                "reset_sensivity(), invalid state");
-
-  if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_250DPS)
-    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_250DPS;
-  else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_500DPS)
-	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_500DPS;
-  else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_2000DPS)
-	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_2000DPS;
-  else {
-    osalDbgAssert(FALSE, "reset_sensivity(), full scale issue");
-    return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
-static msg_t get_temperature(void *ip, float* tempp) {
-	
-#if L3GD20_USE_SPI
-  osalDbgAssert((((L3GD20Driver *)ip)->config->spip->state == SPI_READY),
-                "read_raw(), channel not ready");
-#if	L3GD20_SHARED_SPI
-  spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
-  spiStart(((L3GD20Driver *)ip)->config->spip,
-           ((L3GD20Driver *)ip)->config->spicfg);
-#endif /* L3GD20_SHARED_SPI */   
-  *tempp = (int8_t)l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
-                                       L3GD20_AD_OUT_TEMP);
-#if	L3GD20_SHARED_SPI
-  spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
-#endif /* L3GD20_SHARED_SPI */   
-#endif /* L3GD20_USE_SPI */ 
-  return MSG_OK;
-}
-
-static const struct BaseSensorVMT vmt_basesensor = {
-  get_axes_number, read_raw, read_cooked
-};
-
-static const struct BaseGyroscopeVMT vmt_basegyroscope = {
-  get_axes_number, read_raw, read_cooked,
-  sample_bias, set_bias, reset_bias,
-  set_sensivity, reset_sensivity
-};
-
-static const struct L3GD20VMT vmt_l3gd20 = {
-  get_axes_number, read_raw, read_cooked,
-  sample_bias, set_bias, reset_bias,
-  set_sensivity, reset_sensivity, get_temperature
-};
-
-/*===========================================================================*/
-/* Driver exported functions.                                                */
-/*===========================================================================*/
-
-/**
- * @brief   Initializes an instance.
- *
- * @param[out] devp     pointer to the @p L3GD20Driver object
- *
- * @init
- */
-void l3gd20ObjectInit(L3GD20Driver *devp) {
-  uint32_t i;
-  devp->vmt_basesensor = &vmt_basesensor;
-  devp->vmt_basegyroscope = &vmt_basegyroscope;
-  devp->vmt_l3gd20 = &vmt_l3gd20;
-  devp->config = NULL;
-  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-    devp->bias[i] = 0;
-  devp->state  = L3GD20_STOP;
-}
-
-/**
- * @brief   Configures and activates L3GD20 Complex Driver peripheral.
- *
- * @param[in] devp      pointer to the @p L3GD20Driver object
- * @param[in] config    pointer to the @p L3GD20Config object
- *
- * @api
- */
-void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
-  uint32_t i;
-  osalDbgCheck((devp != NULL) && (config != NULL));
-
-  osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY),
-              "l3gd20Start(), invalid state");			  
-
-  devp->config = config;
-  
-#if L3GD20_USE_SPI
-#if	L3GD20_SHARED_SPI
-  spiAcquireBus((devp)->config->spip);
-#endif /* L3GD20_SHARED_SPI */
-  spiStart((devp)->config->spip,
-           (devp)->config->spicfg);
-  l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,
-                         devp->config->axesenabling |
-                         L3GD20_PM_SLEEP_NORMAL |
-                         devp->config->outputdatarate);
-  l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG4,
-                         devp->config->fullscale |
-                         devp->config->blockdataupdate |
-                         devp->config->endianness);
-#if	L3GD20_SHARED_SPI
-  spiReleaseBus((devp)->config->spip);
-#endif /* L3GD20_SHARED_SPI */  
-#endif /* L3GD20_USE_SPI */
-  
-  /* Storing sensitivity information according to full scale value */
-  if(devp->config->fullscale == L3GD20_FS_250DPS)
-    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = L3GD20_SENS_250DPS;
-  else if(devp->config->fullscale == L3GD20_FS_500DPS)
-	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = L3GD20_SENS_500DPS;
-  else if(devp->config->fullscale == L3GD20_FS_2000DPS)
-	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = L3GD20_SENS_2000DPS;
-  else
-    osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
-  /* This is the Gyroscope transient recovery time */
-  osalThreadSleepMilliseconds(10);
-
-  devp->state = L3GD20_READY;
-} 
-
-/**
- * @brief   Deactivates the L3GD20 Complex Driver peripheral.
- *
- * @param[in] devp       pointer to the @p L3GD20Driver object
- *
- * @api
- */
-void l3gd20Stop(L3GD20Driver *devp) {
-
-  osalDbgCheck(devp != NULL);
-
-  osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY),
-                "l3gd20Stop(), invalid state");
-
-#if (L3GD20_USE_SPI)
-  if (devp->state == L3GD20_STOP) {
-#if	L3GD20_SHARED_SPI
-    spiAcquireBus((devp)->config->spip);
-    spiStart((devp)->config->spip,
-             (devp)->config->spicfg);
-#endif /* L3GD20_SHARED_SPI */
-    l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,
-                           L3GD20_PM_POWER_DOWN | L3GD20_AE_DISABLED);
-    spiStop((devp)->config->spip);
-#if	L3GD20_SHARED_SPI
-    spiReleaseBus((devp)->config->spip);
-#endif /* L3GD20_SHARED_SPI */    
-  }			  
-#endif /* L3GD20_USE_SPI */
-  devp->state = L3GD20_STOP;
-}
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    l3gd20.c

+ * @brief   L3GD20 MEMS interface module code.

+ *

+ * @addtogroup l3gd20

+ * @{

+ */

+

+#include "hal.h"

+#include "l3gd20.h"

+

+/*===========================================================================*/

+/* Driver local definitions.                                                 */

+/*===========================================================================*/

+

+#define  L3GD20_SENS_250DPS                 ((float)0.00875f)

+#define  L3GD20_SENS_500DPS                 ((float)0.01750f)

+#define  L3GD20_SENS_2000DPS                ((float)0.07000f)

+

+#define  L3GD20_DI                          ((uint8_t)0xFF)

+#define  L3GD20_DI_0                        ((uint8_t)0x01)

+#define  L3GD20_DI_1                        ((uint8_t)0x02)

+#define  L3GD20_DI_2                        ((uint8_t)0x04)

+#define  L3GD20_DI_3                        ((uint8_t)0x08)

+#define  L3GD20_DI_4                        ((uint8_t)0x10)

+#define  L3GD20_DI_5                        ((uint8_t)0x20)

+#define  L3GD20_DI_6                        ((uint8_t)0x40)

+#define  L3GD20_DI_7                        ((uint8_t)0x80)

+

+#define  L3GD20_AD                          ((uint8_t)0x3F)

+#define  L3GD20_AD_0                        ((uint8_t)0x01)

+#define  L3GD20_AD_1                        ((uint8_t)0x02)

+#define  L3GD20_AD_2                        ((uint8_t)0x04)

+#define  L3GD20_AD_3                        ((uint8_t)0x08)

+#define  L3GD20_AD_4                        ((uint8_t)0x10)

+#define  L3GD20_AD_5                        ((uint8_t)0x20)

+#define  L3GD20_MS                          ((uint8_t)0x40)

+#define  L3GD20_RW                          ((uint8_t)0x80)

+

+#define  L3GD20_AD_WHO_AM_I                 ((uint8_t)0x0F)

+#define  L3GD20_AD_CTRL_REG1                ((uint8_t)0x20)

+#define  L3GD20_AD_CTRL_REG2                ((uint8_t)0x21)

+#define  L3GD20_AD_CTRL_REG3                ((uint8_t)0x22)

+#define  L3GD20_AD_CTRL_REG4                ((uint8_t)0x23)

+#define  L3GD20_AD_CTRL_REG5                ((uint8_t)0x24)

+#define  L3GD20_AD_REFERENCE                ((uint8_t)0x25)

+#define  L3GD20_AD_OUT_TEMP                 ((uint8_t)0x26)

+#define  L3GD20_AD_STATUS_REG               ((uint8_t)0x27)

+#define  L3GD20_AD_OUT_X_L                  ((uint8_t)0x28)

+#define  L3GD20_AD_OUT_X_H                  ((uint8_t)0x29)

+#define  L3GD20_AD_OUT_Y_L                  ((uint8_t)0x2A)

+#define  L3GD20_AD_OUT_Y_H                  ((uint8_t)0x2B)

+#define  L3GD20_AD_OUT_Z_L                  ((uint8_t)0x2C)

+#define  L3GD20_AD_OUT_Z_H                  ((uint8_t)0x2D)

+#define  L3GD20_AD_FIFO_CTRL_REG            ((uint8_t)0x2E)

+#define  L3GD20_AD_FIFO_SRC_REG             ((uint8_t)0x2F)

+#define  L3GD20_AD_INT1_CFG                 ((uint8_t)0x30)

+#define  L3GD20_AD_INT1_SRC                 ((uint8_t)0x31)

+#define  L3GD20_AD_INT1_TSH_XH              ((uint8_t)0x32)

+#define  L3GD20_AD_INT1_TSH_XL              ((uint8_t)0x33)

+#define  L3GD20_AD_INT1_TSH_YH              ((uint8_t)0x34)

+#define  L3GD20_AD_INT1_TSH_YL              ((uint8_t)0x35)

+#define  L3GD20_AD_INT1_TSH_ZH              ((uint8_t)0x36)

+#define  L3GD20_AD_INT1_TSH_ZL              ((uint8_t)0x37)

+#define  L3GD20_AD_INT1_DURATION            ((uint8_t)0x38)

+

+#define  L3GD20_CTRL_REG4_FS_MASK           ((uint8_t)0x30)

+

+/*===========================================================================*/

+/* Driver exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local variables and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @brief  L3GD20 Power Mode

+ */

+typedef enum {

+  L3GD20_PM_POWER_DOWN   = 0x00,    /**< Power down enabled.                */          

+  L3GD20_PM_SLEEP_NORMAL = 0x08     /**< Normal operation mode.             */          

+}l3gd20_pm_t;

+

+/*===========================================================================*/

+/* Driver local functions.                                                   */

+/*===========================================================================*/

+

+#if (L3GD20_USE_SPI) || defined(__DOXYGEN__)

+/**

+ * @brief   Reads a generic register value using SPI.

+ * @pre     The SPI interface must be initialized and the driver started.

+ *

+ * @param[in] spip      pointer to the SPI interface

+ * @param[in] reg       register number

+ * @return              register value.

+ */

+static uint8_t l3gd20SPIReadRegister(SPIDriver *spip, uint8_t reg) {

+  uint8_t txbuf[2] = {L3GD20_RW | reg, 0xFF};

+  uint8_t rxbuf[2] = {0x00, 0x00};

+  spiSelect(spip);

+  spiExchange(spip, 2, txbuf, rxbuf);

+  spiUnselect(spip);

+  return rxbuf[1];

+}

+

+/**

+ * @brief   Writes a value into a generic register using SPI.

+ * @pre     The SPI interface must be initialized and the driver started.

+ *

+ * @param[in] spip      pointer to the SPI interface

+ * @param[in] reg       register number

+ * @param[in] value     register value.

+ */

+static void l3gd20SPIWriteRegister(SPIDriver *spip, uint8_t reg,

+                                   uint8_t value) {

+

+  switch (reg) {

+    default:

+      /* Reserved register must not be written, according to the datasheet

+       * this could permanently damage the device.

+       */

+      osalDbgAssert(FALSE, "l3gd20SPIWriteRegister(), reserved register");

+    case L3GD20_AD_WHO_AM_I:

+    case L3GD20_AD_OUT_TEMP :

+    case L3GD20_AD_STATUS_REG:

+    case L3GD20_AD_OUT_X_L:

+    case L3GD20_AD_OUT_X_H:

+    case L3GD20_AD_OUT_Y_L:

+    case L3GD20_AD_OUT_Y_H:

+    case L3GD20_AD_OUT_Z_L:

+    case L3GD20_AD_OUT_Z_H:

+    case L3GD20_AD_FIFO_SRC_REG:

+    case L3GD20_AD_INT1_SRC:

+    /* Read only registers cannot be written, the command is ignored.*/

+      return;

+    case L3GD20_AD_CTRL_REG1:

+    case L3GD20_AD_CTRL_REG2:

+    case L3GD20_AD_CTRL_REG3:

+    case L3GD20_AD_CTRL_REG4:

+    case L3GD20_AD_CTRL_REG5:

+    case L3GD20_AD_REFERENCE:

+    case L3GD20_AD_FIFO_CTRL_REG:

+    case L3GD20_AD_INT1_CFG:

+    case L3GD20_AD_INT1_TSH_XH:

+    case L3GD20_AD_INT1_TSH_XL:

+    case L3GD20_AD_INT1_TSH_YH:

+    case L3GD20_AD_INT1_TSH_YL:

+    case L3GD20_AD_INT1_TSH_ZH:

+    case L3GD20_AD_INT1_TSH_ZL:

+    case L3GD20_AD_INT1_DURATION:

+      spiSelect(spip);

+      uint8_t txbuf[2] = {reg, value};

+      spiSend(spip, 2, txbuf);

+      spiUnselect(spip);

+  }

+}

+#endif /* L3GD20_USE_SPI */

+

+/*

+ * Interface implementation.

+ */

+static size_t get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return L3GD20_NUMBER_OF_AXES;

+}

+

+static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {

+  int16_t tmp;

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),

+              "read_raw(), invalid state");

+

+#if L3GD20_USE_SPI

+  osalDbgAssert((((L3GD20Driver *)ip)->config->spip->state == SPI_READY),

+                "read_raw(), channel not ready");

+#if	L3GD20_SHARED_SPI

+  spiAcquireBus(((L3GD20Driver *)ip)->config->spip);

+  spiStart(((L3GD20Driver *)ip)->config->spip,

+           ((L3GD20Driver *)ip)->config->spicfg);

+#endif /* L3GD20_SHARED_SPI */   

+  if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_X){

+    tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,

+                                  L3GD20_AD_OUT_X_L);

+    tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,

+                                   L3GD20_AD_OUT_X_H) << 8;

+    axes[0] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[0];

+  }

+  if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_Y){

+    tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,

+                                  L3GD20_AD_OUT_Y_L);

+    tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,

+                                   L3GD20_AD_OUT_Y_H) << 8;

+    axes[1] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[1];

+  }

+  if(((L3GD20Driver *)ip)->config->axesenabling & L3GD20_AE_Z){

+    tmp = l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,

+                                  L3GD20_AD_OUT_Z_L);

+    tmp += l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,

+                                   L3GD20_AD_OUT_Z_H) << 8;

+    axes[2] = (int32_t)tmp + ((L3GD20Driver *)ip)->bias[2];

+  }

+#if	L3GD20_SHARED_SPI

+  spiReleaseBus(((L3GD20Driver *)ip)->config->spip);

+#endif /* L3GD20_SHARED_SPI */   

+#endif /* L3GD20_USE_SPI */ 

+  return MSG_OK;

+}

+

+static msg_t read_cooked(void *ip, float axes[]) {

+  uint32_t i;

+  int32_t raw[L3GD20_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),

+              "read_cooked(), invalid state");

+

+  msg = read_raw(ip, raw);

+  for(i = 0; i < L3GD20_NUMBER_OF_AXES ; i++){

+    axes[i] = raw[i] * ((L3GD20Driver *)ip)->sensitivity[i];

+  }

+  return msg;

+}

+

+static msg_t sample_bias(void *ip) {

+  uint32_t i, j;

+  int32_t raw[L3GD20_NUMBER_OF_AXES];

+  int32_t buff[L3GD20_NUMBER_OF_AXES] = {0, 0, 0};

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),

+                "sample_bias(), invalid state");

+

+  for(i = 0; i < L3GD20_BIAS_ACQ_TIMES; i++){

+    read_raw(ip, raw);

+    for(j = 0; j < L3GD20_NUMBER_OF_AXES; j++){

+      buff[j] += raw[j];

+    }

+    osalThreadSleepMicroseconds(L3GD20_BIAS_SETTLING_uS);

+  }

+

+  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++){

+    ((L3GD20Driver *)ip)->bias[i] = buff[i] / L3GD20_BIAS_ACQ_TIMES;

+  }

+  return MSG_OK;

+}

+

+static msg_t set_bias(void *ip, int32_t *bp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (bp !=NULL));

+

+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||

+                (((L3GD20Driver *)ip)->state == L3GD20_STOP),

+                "set_bias(), invalid state");

+  

+  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {

+    ((L3GD20Driver *)ip)->bias[i] = bp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t reset_bias(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||

+                (((L3GD20Driver *)ip)->state == L3GD20_STOP),

+              "reset_bias(), invalid state");

+

+  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)

+    ((L3GD20Driver *)ip)->bias[i] = 0;

+  return MSG_OK;

+}

+

+static msg_t set_sensivity(void *ip, float *sp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (sp !=NULL));

+

+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),

+                "set_sensivity(), invalid state");

+  

+  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {

+    ((L3GD20Driver *)ip)->sensitivity[i] = sp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t reset_sensivity(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),

+                "reset_sensivity(), invalid state");

+

+  if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_250DPS)

+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)

+      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_250DPS;

+  else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_500DPS)

+	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)

+      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_500DPS;

+  else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_2000DPS)

+	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)

+      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_2000DPS;

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), full scale issue");

+    return MSG_RESET;

+  }

+  return MSG_OK;

+}

+

+static msg_t get_temperature(void *ip, float* tempp) {

+	

+#if L3GD20_USE_SPI

+  osalDbgAssert((((L3GD20Driver *)ip)->config->spip->state == SPI_READY),

+                "read_raw(), channel not ready");

+#if	L3GD20_SHARED_SPI

+  spiAcquireBus(((L3GD20Driver *)ip)->config->spip);

+  spiStart(((L3GD20Driver *)ip)->config->spip,

+           ((L3GD20Driver *)ip)->config->spicfg);

+#endif /* L3GD20_SHARED_SPI */   

+  *tempp = (int8_t)l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,

+                                       L3GD20_AD_OUT_TEMP);

+#if	L3GD20_SHARED_SPI

+  spiReleaseBus(((L3GD20Driver *)ip)->config->spip);

+#endif /* L3GD20_SHARED_SPI */   

+#endif /* L3GD20_USE_SPI */ 

+  return MSG_OK;

+}

+

+static const struct BaseSensorVMT vmt_basesensor = {

+  get_axes_number, read_raw, read_cooked

+};

+

+static const struct BaseGyroscopeVMT vmt_basegyroscope = {

+  get_axes_number, read_raw, read_cooked,

+  sample_bias, set_bias, reset_bias,

+  set_sensivity, reset_sensivity

+};

+

+static const struct L3GD20VMT vmt_l3gd20 = {

+  get_axes_number, read_raw, read_cooked,

+  sample_bias, set_bias, reset_bias,

+  set_sensivity, reset_sensivity, get_temperature

+};

+

+/*===========================================================================*/

+/* Driver exported functions.                                                */

+/*===========================================================================*/

+

+/**

+ * @brief   Initializes an instance.

+ *

+ * @param[out] devp     pointer to the @p L3GD20Driver object

+ *

+ * @init

+ */

+void l3gd20ObjectInit(L3GD20Driver *devp) {

+  uint32_t i;

+  devp->vmt_basesensor = &vmt_basesensor;

+  devp->vmt_basegyroscope = &vmt_basegyroscope;

+  devp->vmt_l3gd20 = &vmt_l3gd20;

+  devp->config = NULL;

+  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)

+    devp->bias[i] = 0;

+  devp->state  = L3GD20_STOP;

+}

+

+/**

+ * @brief   Configures and activates L3GD20 Complex Driver peripheral.

+ *

+ * @param[in] devp      pointer to the @p L3GD20Driver object

+ * @param[in] config    pointer to the @p L3GD20Config object

+ *

+ * @api

+ */

+void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {

+  uint32_t i;

+  osalDbgCheck((devp != NULL) && (config != NULL));

+

+  osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY),

+              "l3gd20Start(), invalid state");			  

+

+  devp->config = config;

+  

+#if L3GD20_USE_SPI

+#if	L3GD20_SHARED_SPI

+  spiAcquireBus((devp)->config->spip);

+#endif /* L3GD20_SHARED_SPI */

+  spiStart((devp)->config->spip,

+           (devp)->config->spicfg);

+  l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,

+                         devp->config->axesenabling |

+                         L3GD20_PM_SLEEP_NORMAL |

+                         devp->config->outputdatarate);

+  l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG4,

+                         devp->config->fullscale |

+                         devp->config->blockdataupdate |

+                         devp->config->endianness);

+#if	L3GD20_SHARED_SPI

+  spiReleaseBus((devp)->config->spip);

+#endif /* L3GD20_SHARED_SPI */  

+#endif /* L3GD20_USE_SPI */

+  

+  /* Storing sensitivity information according to full scale value */

+  if(devp->config->fullscale == L3GD20_FS_250DPS)

+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = L3GD20_SENS_250DPS;

+  else if(devp->config->fullscale == L3GD20_FS_500DPS)

+	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = L3GD20_SENS_500DPS;

+  else if(devp->config->fullscale == L3GD20_FS_2000DPS)

+	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = L3GD20_SENS_2000DPS;

+  else

+    osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");

+  /* This is the Gyroscope transient recovery time */

+  osalThreadSleepMilliseconds(10);

+

+  devp->state = L3GD20_READY;

+} 

+

+/**

+ * @brief   Deactivates the L3GD20 Complex Driver peripheral.

+ *

+ * @param[in] devp       pointer to the @p L3GD20Driver object

+ *

+ * @api

+ */

+void l3gd20Stop(L3GD20Driver *devp) {

+

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY),

+                "l3gd20Stop(), invalid state");

+

+#if (L3GD20_USE_SPI)

+  if (devp->state == L3GD20_STOP) {

+#if	L3GD20_SHARED_SPI

+    spiAcquireBus((devp)->config->spip);

+    spiStart((devp)->config->spip,

+             (devp)->config->spicfg);

+#endif /* L3GD20_SHARED_SPI */

+    l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,

+                           L3GD20_PM_POWER_DOWN | L3GD20_AE_DISABLED);

+    spiStop((devp)->config->spip);

+#if	L3GD20_SHARED_SPI

+    spiReleaseBus((devp)->config->spip);

+#endif /* L3GD20_SHARED_SPI */    

+  }			  

+#endif /* L3GD20_USE_SPI */

+  devp->state = L3GD20_STOP;

+}

+/** @} */

diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h
index f798d0c25..5e7011a76 100644
--- a/os/ex/ST/l3gd20.h
+++ b/os/ex/ST/l3gd20.h
@@ -1,330 +1,330 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    l3gd20.h
- * @brief   L3GD20 MEMS interface module header.
- *
- * @{
- */
-
-#ifndef _L3GD20_H_
-#define _L3GD20_H_
-
-#include "hal_gyroscope.h"
-
-/*===========================================================================*/
-/* Driver constants.                                                         */
-/*===========================================================================*/
-
-/**
- * @brief   L3GD20 number of axes.
- */
-#define  L3GD20_NUMBER_OF_AXES              ((size_t)   3U)
-
-/*===========================================================================*/
-/* Driver pre-compile time settings.                                         */
-/*===========================================================================*/
-
-/**
- * @name    Configuration options
- * @{
- */
-/**
- * @brief   L3GD20 SPI interface switch.
- * @details If set to @p TRUE the support for SPI is included.
- * @note    The default is @p TRUE.
- */
-#if !defined(L3GD20_USE_SPI) || defined(__DOXYGEN__)
-#define L3GD20_USE_SPI                      TRUE
-#endif
-
-/**
- * @brief   L3GD20 I2C interface switch.
- * @details If set to @p TRUE the support for I2C is included.
- * @note    The default is @p FALSE.
- */
-#if !defined(L3GD20_USE_I2C) || defined(__DOXYGEN__)
-#define L3GD20_USE_I2C                      FALSE
-#endif
-
-/**
- * @brief   L3GD20 shared SPI switch.
- * @details If set to @p TRUE the device acquires SPI bus ownership
- *          on each transaction.
- * @note    The default is @p FALSE. Requires SPI_USE_MUTUAL_EXCLUSION
- */
-#if !defined(L3GD20_SHARED_SPI) || defined(__DOXYGEN__)
-#define L3GD20_SHARED_SPI                   FALSE
-#endif
-
-/**
- * @brief   Number of acquisitions for bias removal
- * @details This is the number of acquisitions performed to compute the
- *          bias. A repetition is required in order to remove noise.
- */
-#if !defined(L3GD20_BIAS_ACQ_TIMES) || defined(__DOXYGEN__)
-#define L3GD20_BIAS_ACQ_TIMES               50
-#endif
-
-/**
- * @brief   Settling time for bias removal
- * @details This is the time between each bias acquisition.
- */
-#if !defined(L3GD20_BIAS_SETTLING_uS) || defined(__DOXYGEN__)
-#define L3GD20_BIAS_SETTLING_uS             5000
-#endif
-/** @} */
-
-/*===========================================================================*/
-/* Derived constants and error checks.                                       */
-/*===========================================================================*/
-
-#if !(L3GD20_USE_SPI ^ L3GD20_USE_I2C)
-#error "L3GD20_USE_SPI and L3GD20_USE_I2C cannot be both true or both false"
-#endif
-
-#if L3GD20_USE_SPI && !HAL_USE_SPI
-#error "L3GD20_USE_SPI requires HAL_USE_SPI"
-#endif
-
-#if L3GD20_USE_I2C && !HAL_USE_I2C
-#error "L3GD20_USE_I2C requires HAL_USE_I2C"
-#endif
-
-#if L3GD20_SHARED_SPI && !SPI_USE_MUTUAL_EXCLUSION
-#error "L3GD20_SHARED_SPI requires SPI_USE_MUTUAL_EXCLUSION"
-#endif
-
-/*===========================================================================*/
-/* Driver data structures and types.                                         */
-/*===========================================================================*/
-
-/**
- * @name    L3GD20 data structures and types
- * @{
- */
-/**
- * @brief   L3GD20 full scale
- */
-typedef enum {
-  L3GD20_FS_250DPS = 0x00,          /**< Full scale 250 degree per second.  */
-  L3GD20_FS_500DPS = 0x10,          /**< Full scale 500 degree per second.  */
-  L3GD20_FS_2000DPS = 0x20          /**< Full scale 2000 degree per second. */
-}l3gd20_fs_t;
-
-/**
- * @brief   L3GD20 output data rate and bandwidth
- */
-typedef enum {
-  L3GD20_ODR_95HZ_FC_12_5 = 0x00,   /**< ODR 95 Hz, BW 12.5 Hz.             */
-  L3GD20_ODR_95HZ_FC_25 = 0x10,     /**< ODR 95 Hz, BW 25Hz.                */
-  L3GD20_ODR_190HZ_FC_12_5 = 0x40,  /**< ODR 190 Hz, BW 12.5 Hz.            */
-  L3GD20_ODR_190HZ_FC_25 = 0x50,    /**< ODR 190 Hz, BW 25 Hz.              */
-  L3GD20_ODR_190HZ_FC_50 = 0x60,    /**< ODR 190 Hz, BW 50 Hz.              */
-  L3GD20_ODR_190HZ_FC_70 = 0x70,    /**< ODR 190 Hz, BW 70 Hz.              */
-  L3GD20_ODR_380HZ_FC_20 = 0x80,    /**< ODR 380 Hz, BW 20 Hz.              */
-  L3GD20_ODR_380HZ_FC_25 = 0x90,    /**< ODR 380 Hz, BW 25 Hz.              */
-  L3GD20_ODR_380HZ_FC_50 = 0xA0,    /**< ODR 380 Hz, BW 50 Hz.              */
-  L3GD20_ODR_380HZ_FC_100 = 0xB0,   /**< ODR 380 Hz, BW 100 Hz.             */
-  L3GD20_ODR_760HZ_FC_30 = 0xC0,    /**< ODR 760 Hz, BW 30 Hz.              */
-  L3GD20_ODR_760HZ_FC_35 = 0xD0,    /**< ODR 760 Hz, BW 35 Hz.              */
-  L3GD20_ODR_760HZ_FC_50 = 0xE0,    /**< ODR 760 Hz, BW 50 Hz.              */
-  L3GD20_ODR_760HZ_FC_100 = 0xF0    /**< ODR 760 Hz, BW 100 Hz.             */
-}l3gd20_odr_t;
-
-/**
- * @brief   L3GD20 axes enabling
- */
-typedef enum {
-  L3GD20_AE_DISABLED = 0x00,        /**< All axes disabled.                 */
-  L3GD20_AE_X = 0x01,               /**< Only X-axis enabled.               */
-  L3GD20_AE_Y = 0x02,               /**< Only Y-axis enabled.               */
-  L3GD20_AE_XY = 0x03,              /**< X and Y axes enabled.              */
-  L3GD20_AE_Z = 0x04,               /**< Only Z-axis enabled.               */
-  L3GD20_AE_XZ = 0x05,              /**< X and Z axes enabled.              */
-  L3GD20_AE_YZ = 0x06,              /**< Y and Z axes enabled.              */
-  L3GD20_AE_XYZ = 0x07              /**< All axes enabled.                  */
-}l3gd20_ae_t;
-
-/**
- * @brief   L3GD20 block data update
- */
-typedef enum {
-  L3GD20_BDU_CONTINUOUS = 0x00,     /**< Block data continuously updated.   */
-  L3GD20_BDU_BLOCKED = 0x80         /**< Block data updated after reading.  */
-}l3gd20_bdu_t;
-
-/**
- * @brief   L3GD20 endianness
- */
-typedef enum {
-  L3GD20_END_LITTLE = 0x00,         /**< Little endian.                     */
-  L3GD20_END_BIG = 0x40             /**< Big endian.                        */
-}l3gd20_end_t;
-
-/**
- * @brief   Driver state machine possible states.
- */
-typedef enum {
-  L3GD20_UNINIT = 0,                /**< Not initialized.                   */
-  L3GD20_STOP = 1,                  /**< Stopped.                           */
-  L3GD20_READY = 2,                 /**< Ready.                             */
-} l3gd20_state_t;
-
-/**
- * @brief   L3GD20 configuration structure.
- */
-typedef struct {
-
-#if (L3GD20_USE_SPI) || defined(__DOXYGEN__)
-  /**
-   * @brief SPI driver associated to this L3GD20.
-   */
-  SPIDriver                 *spip;
-  /**
-   * @brief SPI configuration associated to this L3GD20.
-   */
-  const SPIConfig           *spicfg;
-#endif /* L3GD20_USE_SPI */
-#if (L3GD20_USE_I2C) || defined(__DOXYGEN__)
-  /**
-   * @brief I2C driver associated to this L3GD20.
-   */
-  I2CDriver                 *i2cp;
-  /**
-   * @brief I2C configuration associated to this L3GD20.
-   */
-  const I2CConfig           *i2ccfg;
-#endif /* L3GD20_USE_I2C */
-  /**
-   * @brief L3GD20 full scale value.
-   */
-  l3gd20_fs_t               fullscale;
-  /**
-   * @brief L3GD20 output data rate selection.
-   */
-  l3gd20_odr_t              outputdatarate;
-  /**
-   * @brief L3GD20 axes enabling.
-   */
-  l3gd20_ae_t               axesenabling;
-  /**
-   * @brief L3GD20 block data update.
-   */
-  l3gd20_bdu_t              blockdataupdate;
-  /**
-   * @brief L3GD20 endianness.
-   */
-  l3gd20_end_t              endianness;
-} L3GD20Config;
-
-/**
- * @brief   Structure representing a L3GD20 driver.
- */
-typedef struct L3GD20Driver L3GD20Driver;
-
-/**
- * @brief   @p L3GD20 specific methods.
- */
-#define _l3gd20_methods                                                     \
-  _base_gyroscope_methods                                                   \
-  /* Retrieve the temperature of L3GD20 chip.*/                             \
-  msg_t (*get_temperature)(void *instance, float* temperature);
-
-
-/**
- * @extends BaseGyroscopeVMT
- *
- * @brief   @p L3GD20 virtual methods table.
- */
-struct L3GD20VMT {
-  _l3gd20_methods
-};
-
-/**
- * @brief   @p L3GD20Driver specific data.
- */
-#define _l3gd20_data                                                        \
-  _base_gyroscope_data                                                      \
-  /* Driver state.*/                                                        \
-  l3gd20_state_t            state;                                          \
-  /* Current configuration data.*/                                          \
-  const L3GD20Config        *config;                                        \
-  /* Current sensitivity.*/                                                 \
-  float                     sensitivity[L3GD20_NUMBER_OF_AXES];             \
-  /* Bias data.*/                                                           \
-  int32_t                   bias[L3GD20_NUMBER_OF_AXES];
-
-/**
- * @extends BaseGyroscope
- *
- * @brief   L3GD20 3-axis gyroscope class.
- * @details This class extends @p BaseGyroscope by adding physical
- *          driver implementation.
- */
-struct L3GD20Driver {
-  /** @brief BaseSensor Virtual Methods Table. */
-  const struct BaseSensorVMT *vmt_basesensor;
-  /** @brief BaseGyroscope Virtual Methods Table. */
-  const struct BaseGyroscopeVMT *vmt_basegyroscope;
-  /** @brief L3GD20 Virtual Methods Table. */
-  const struct L3GD20VMT *vmt_l3gd20;
-  _l3gd20_data
-};
-/** @} */
-
-/*===========================================================================*/
-/* Driver macros.                                                            */
-/*===========================================================================*/
-
-/**
- * @brief   Get current MEMS temperature.
- * @detail  This information is very useful especially for high accuracy IMU
- *
- * @param[in] ip        pointer to a @p BaseGyroscope class.
- * @param[out] temp     the MEMS temperature as single precision floating.
- *
- * @return              The operation status.
- * @retval MSG_OK       if the function succeeded.
- * @retval MSG_RESET    if one or more errors occurred.
- * @api
- */
-#define gyroscopeGetTemp(ip, tpp)                                           \
-        (ip)->vmt_l3gd20->get_temperature(ip, tpp)
-
-/*===========================================================================*/
-/* External declarations.                                                    */
-/*===========================================================================*/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-  void l3gd20ObjectInit(L3GD20Driver *devp);
-  void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config);
-  void l3gd20Stop(L3GD20Driver *devp);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _L3GD20_H_ */
-
-/** @} */
-
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    l3gd20.h

+ * @brief   L3GD20 MEMS interface module header.

+ *

+ * @{

+ */

+

+#ifndef _L3GD20_H_

+#define _L3GD20_H_

+

+#include "hal_gyroscope.h"

+

+/*===========================================================================*/

+/* Driver constants.                                                         */

+/*===========================================================================*/

+

+/**

+ * @brief   L3GD20 number of axes.

+ */

+#define  L3GD20_NUMBER_OF_AXES              ((size_t)   3U)

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    Configuration options

+ * @{

+ */

+/**

+ * @brief   L3GD20 SPI interface switch.

+ * @details If set to @p TRUE the support for SPI is included.

+ * @note    The default is @p TRUE.

+ */

+#if !defined(L3GD20_USE_SPI) || defined(__DOXYGEN__)

+#define L3GD20_USE_SPI                      TRUE

+#endif

+

+/**

+ * @brief   L3GD20 I2C interface switch.

+ * @details If set to @p TRUE the support for I2C is included.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(L3GD20_USE_I2C) || defined(__DOXYGEN__)

+#define L3GD20_USE_I2C                      FALSE

+#endif

+

+/**

+ * @brief   L3GD20 shared SPI switch.

+ * @details If set to @p TRUE the device acquires SPI bus ownership

+ *          on each transaction.

+ * @note    The default is @p FALSE. Requires SPI_USE_MUTUAL_EXCLUSION

+ */

+#if !defined(L3GD20_SHARED_SPI) || defined(__DOXYGEN__)

+#define L3GD20_SHARED_SPI                   FALSE

+#endif

+

+/**

+ * @brief   Number of acquisitions for bias removal

+ * @details This is the number of acquisitions performed to compute the

+ *          bias. A repetition is required in order to remove noise.

+ */

+#if !defined(L3GD20_BIAS_ACQ_TIMES) || defined(__DOXYGEN__)

+#define L3GD20_BIAS_ACQ_TIMES               50

+#endif

+

+/**

+ * @brief   Settling time for bias removal

+ * @details This is the time between each bias acquisition.

+ */

+#if !defined(L3GD20_BIAS_SETTLING_uS) || defined(__DOXYGEN__)

+#define L3GD20_BIAS_SETTLING_uS             5000

+#endif

+/** @} */

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+#if !(L3GD20_USE_SPI ^ L3GD20_USE_I2C)

+#error "L3GD20_USE_SPI and L3GD20_USE_I2C cannot be both true or both false"

+#endif

+

+#if L3GD20_USE_SPI && !HAL_USE_SPI

+#error "L3GD20_USE_SPI requires HAL_USE_SPI"

+#endif

+

+#if L3GD20_USE_I2C && !HAL_USE_I2C

+#error "L3GD20_USE_I2C requires HAL_USE_I2C"

+#endif

+

+#if L3GD20_SHARED_SPI && !SPI_USE_MUTUAL_EXCLUSION

+#error "L3GD20_SHARED_SPI requires SPI_USE_MUTUAL_EXCLUSION"

+#endif

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    L3GD20 data structures and types

+ * @{

+ */

+/**

+ * @brief   L3GD20 full scale

+ */

+typedef enum {

+  L3GD20_FS_250DPS = 0x00,          /**< Full scale 250 degree per second.  */

+  L3GD20_FS_500DPS = 0x10,          /**< Full scale 500 degree per second.  */

+  L3GD20_FS_2000DPS = 0x20          /**< Full scale 2000 degree per second. */

+}l3gd20_fs_t;

+

+/**

+ * @brief   L3GD20 output data rate and bandwidth

+ */

+typedef enum {

+  L3GD20_ODR_95HZ_FC_12_5 = 0x00,   /**< ODR 95 Hz, BW 12.5 Hz.             */

+  L3GD20_ODR_95HZ_FC_25 = 0x10,     /**< ODR 95 Hz, BW 25Hz.                */

+  L3GD20_ODR_190HZ_FC_12_5 = 0x40,  /**< ODR 190 Hz, BW 12.5 Hz.            */

+  L3GD20_ODR_190HZ_FC_25 = 0x50,    /**< ODR 190 Hz, BW 25 Hz.              */

+  L3GD20_ODR_190HZ_FC_50 = 0x60,    /**< ODR 190 Hz, BW 50 Hz.              */

+  L3GD20_ODR_190HZ_FC_70 = 0x70,    /**< ODR 190 Hz, BW 70 Hz.              */

+  L3GD20_ODR_380HZ_FC_20 = 0x80,    /**< ODR 380 Hz, BW 20 Hz.              */

+  L3GD20_ODR_380HZ_FC_25 = 0x90,    /**< ODR 380 Hz, BW 25 Hz.              */

+  L3GD20_ODR_380HZ_FC_50 = 0xA0,    /**< ODR 380 Hz, BW 50 Hz.              */

+  L3GD20_ODR_380HZ_FC_100 = 0xB0,   /**< ODR 380 Hz, BW 100 Hz.             */

+  L3GD20_ODR_760HZ_FC_30 = 0xC0,    /**< ODR 760 Hz, BW 30 Hz.              */

+  L3GD20_ODR_760HZ_FC_35 = 0xD0,    /**< ODR 760 Hz, BW 35 Hz.              */

+  L3GD20_ODR_760HZ_FC_50 = 0xE0,    /**< ODR 760 Hz, BW 50 Hz.              */

+  L3GD20_ODR_760HZ_FC_100 = 0xF0    /**< ODR 760 Hz, BW 100 Hz.             */

+}l3gd20_odr_t;

+

+/**

+ * @brief   L3GD20 axes enabling

+ */

+typedef enum {

+  L3GD20_AE_DISABLED = 0x00,        /**< All axes disabled.                 */

+  L3GD20_AE_X = 0x01,               /**< Only X-axis enabled.               */

+  L3GD20_AE_Y = 0x02,               /**< Only Y-axis enabled.               */

+  L3GD20_AE_XY = 0x03,              /**< X and Y axes enabled.              */

+  L3GD20_AE_Z = 0x04,               /**< Only Z-axis enabled.               */

+  L3GD20_AE_XZ = 0x05,              /**< X and Z axes enabled.              */

+  L3GD20_AE_YZ = 0x06,              /**< Y and Z axes enabled.              */

+  L3GD20_AE_XYZ = 0x07              /**< All axes enabled.                  */

+}l3gd20_ae_t;

+

+/**

+ * @brief   L3GD20 block data update

+ */

+typedef enum {

+  L3GD20_BDU_CONTINUOUS = 0x00,     /**< Block data continuously updated.   */

+  L3GD20_BDU_BLOCKED = 0x80         /**< Block data updated after reading.  */

+}l3gd20_bdu_t;

+

+/**

+ * @brief   L3GD20 endianness

+ */

+typedef enum {

+  L3GD20_END_LITTLE = 0x00,         /**< Little endian.                     */

+  L3GD20_END_BIG = 0x40             /**< Big endian.                        */

+}l3gd20_end_t;

+

+/**

+ * @brief   Driver state machine possible states.

+ */

+typedef enum {

+  L3GD20_UNINIT = 0,                /**< Not initialized.                   */

+  L3GD20_STOP = 1,                  /**< Stopped.                           */

+  L3GD20_READY = 2,                 /**< Ready.                             */

+} l3gd20_state_t;

+

+/**

+ * @brief   L3GD20 configuration structure.

+ */

+typedef struct {

+

+#if (L3GD20_USE_SPI) || defined(__DOXYGEN__)

+  /**

+   * @brief SPI driver associated to this L3GD20.

+   */

+  SPIDriver                 *spip;

+  /**

+   * @brief SPI configuration associated to this L3GD20.

+   */

+  const SPIConfig           *spicfg;

+#endif /* L3GD20_USE_SPI */

+#if (L3GD20_USE_I2C) || defined(__DOXYGEN__)

+  /**

+   * @brief I2C driver associated to this L3GD20.

+   */

+  I2CDriver                 *i2cp;

+  /**

+   * @brief I2C configuration associated to this L3GD20.

+   */

+  const I2CConfig           *i2ccfg;

+#endif /* L3GD20_USE_I2C */

+  /**

+   * @brief L3GD20 full scale value.

+   */

+  l3gd20_fs_t               fullscale;

+  /**

+   * @brief L3GD20 output data rate selection.

+   */

+  l3gd20_odr_t              outputdatarate;

+  /**

+   * @brief L3GD20 axes enabling.

+   */

+  l3gd20_ae_t               axesenabling;

+  /**

+   * @brief L3GD20 block data update.

+   */

+  l3gd20_bdu_t              blockdataupdate;

+  /**

+   * @brief L3GD20 endianness.

+   */

+  l3gd20_end_t              endianness;

+} L3GD20Config;

+

+/**

+ * @brief   Structure representing a L3GD20 driver.

+ */

+typedef struct L3GD20Driver L3GD20Driver;

+

+/**

+ * @brief   @p L3GD20 specific methods.

+ */

+#define _l3gd20_methods                                                     \

+  _base_gyroscope_methods                                                   \

+  /* Retrieve the temperature of L3GD20 chip.*/                             \

+  msg_t (*get_temperature)(void *instance, float* temperature);

+

+

+/**

+ * @extends BaseGyroscopeVMT

+ *

+ * @brief   @p L3GD20 virtual methods table.

+ */

+struct L3GD20VMT {

+  _l3gd20_methods

+};

+

+/**

+ * @brief   @p L3GD20Driver specific data.

+ */

+#define _l3gd20_data                                                        \

+  _base_gyroscope_data                                                      \

+  /* Driver state.*/                                                        \

+  l3gd20_state_t            state;                                          \

+  /* Current configuration data.*/                                          \

+  const L3GD20Config        *config;                                        \

+  /* Current sensitivity.*/                                                 \

+  float                     sensitivity[L3GD20_NUMBER_OF_AXES];             \

+  /* Bias data.*/                                                           \

+  int32_t                   bias[L3GD20_NUMBER_OF_AXES];

+

+/**

+ * @extends BaseGyroscope

+ *

+ * @brief   L3GD20 3-axis gyroscope class.

+ * @details This class extends @p BaseGyroscope by adding physical

+ *          driver implementation.

+ */

+struct L3GD20Driver {

+  /** @brief BaseSensor Virtual Methods Table. */

+  const struct BaseSensorVMT *vmt_basesensor;

+  /** @brief BaseGyroscope Virtual Methods Table. */

+  const struct BaseGyroscopeVMT *vmt_basegyroscope;

+  /** @brief L3GD20 Virtual Methods Table. */

+  const struct L3GD20VMT *vmt_l3gd20;

+  _l3gd20_data

+};

+/** @} */

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+/**

+ * @brief   Get current MEMS temperature.

+ * @detail  This information is very useful especially for high accuracy IMU

+ *

+ * @param[in] ip        pointer to a @p BaseGyroscope class.

+ * @param[out] temp     the MEMS temperature as single precision floating.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ * @api

+ */

+#define gyroscopeGetTemp(ip, tpp)                                           \

+        (ip)->vmt_l3gd20->get_temperature(ip, tpp)

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void l3gd20ObjectInit(L3GD20Driver *devp);

+  void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config);

+  void l3gd20Stop(L3GD20Driver *devp);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* _L3GD20_H_ */

+

+/** @} */

+

diff --git a/os/ex/ST/lis302dl.c b/os/ex/ST/lis302dl.c
index 0a76a39db..32b347929 100644
--- a/os/ex/ST/lis302dl.c
+++ b/os/ex/ST/lis302dl.c
@@ -1,429 +1,429 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lis302dl.c
- * @brief   LIS302DL MEMS interface module code.
- *
- * @addtogroup lis302dl
- * @{
- */
-
-#include "hal.h"
-#include "lis302dl.h"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-#define  LIS302DL_SENS_2G                   ((float)18.0f)
-#define  LIS302DL_SENS_8G                   ((float)72.0f)
-
-#define  LIS302DL_DI                        ((uint8_t)0xFF)
-#define  LIS302DL_DI_0                      ((uint8_t)0x01)
-#define  LIS302DL_DI_1                      ((uint8_t)0x02)
-#define  LIS302DL_DI_2                      ((uint8_t)0x04)
-#define  LIS302DL_DI_3                      ((uint8_t)0x08)
-#define  LIS302DL_DI_4                      ((uint8_t)0x10)
-#define  LIS302DL_DI_5                      ((uint8_t)0x20)
-#define  LIS302DL_DI_6                      ((uint8_t)0x40)
-#define  LIS302DL_DI_7                      ((uint8_t)0x80)
-
-#define  LIS302DL_AD                        ((uint8_t)0x3F)
-#define  LIS302DL_AD_0                      ((uint8_t)0x01)
-#define  LIS302DL_AD_1                      ((uint8_t)0x02)
-#define  LIS302DL_AD_2                      ((uint8_t)0x04)
-#define  LIS302DL_AD_3                      ((uint8_t)0x08)
-#define  LIS302DL_AD_4                      ((uint8_t)0x10)
-#define  LIS302DL_AD_5                      ((uint8_t)0x20)
-
-#define  LIS302DL_MS                        ((uint8_t)0x40)
-#define  LIS302DL_RW                        ((uint8_t)0x80)
-
-#define  LIS302DL_AD_WHO_AM_I               ((uint8_t)0x0F)
-#define  LIS302DL_AD_CTRL_REG1              ((uint8_t)0x20)
-#define  LIS302DL_AD_CTRL_REG2              ((uint8_t)0x21)
-#define  LIS302DL_AD_CTRL_REG3              ((uint8_t)0x22)
-#define  LIS302DL_AD_HP_FILER_RESET         ((uint8_t)0x23)
-#define  LIS302DL_AD_STATUS_REG             ((uint8_t)0x27)
-#define  LIS302DL_AD_OUT_X                  ((uint8_t)0x29)
-#define  LIS302DL_AD_OUT_Y                  ((uint8_t)0x2B)
-#define  LIS302DL_AD_OUT_Z                  ((uint8_t)0x2D)
-#define  LIS302DL_AD_FF_WU_CFG_1            ((uint8_t)0x30)
-#define  LIS302DL_AD_FF_WU_SRC_1            ((uint8_t)0x31)
-#define  LIS302DL_AD_FF_WU_THS_1            ((uint8_t)0x32)
-#define  LIS302DL_AD_FF_WU_DURATION_1       ((uint8_t)0x33)
-#define  LIS302DL_AD_FF_WU_CFG_2            ((uint8_t)0x34)
-#define  LIS302DL_AD_FF_WU_SRC_2            ((uint8_t)0x35)
-#define  LIS302DL_AD_FF_WU_THS_2            ((uint8_t)0x36)
-#define  LIS302DL_AD_FF_WU_DURATION_2       ((uint8_t)0x37)
-#define  LIS302DL_AD_CLICK_CFG              ((uint8_t)0x38)
-#define  LIS302DL_AD_CLICK_SRC              ((uint8_t)0x39)
-#define  LIS302DL_AD_CLICK_THSY_X           ((uint8_t)0x3B)
-#define  LIS302DL_AD_CLICK_THSZ             ((uint8_t)0x3C)
-#define  LIS302DL_AD_CLICK_TIME_LIMIT       ((uint8_t)0x3D)
-#define  LIS302DL_AD_CLICK_LATENCY          ((uint8_t)0x3E)
-#define  LIS302DL_AD_CLICK_WINDOW           ((uint8_t)0x3F)
-
-#define  LIS302DL_CTRL_REG1_FS_MASK         ((uint8_t)0x20)
-#define  TO_G                               ((float)0.001f)
-#define  TO_SI                              ((float)0.00981f)
-/*===========================================================================*/
-/* Driver exported variables.                                                */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local variables and types.                                         */
-/*===========================================================================*/
-
-/**
- * @brief  LIS302DL Power Mode
- */
-typedef enum {
-  LIS302DL_PM_POWER_DOWN   = 0x00,  /**< Power down enabled.                */          
-  LIS302DL_PM_SLEEP_NORMAL = 0x40   /**< Normal operation mode.             */          
-}lis302dl_pm_t;
-
-/*===========================================================================*/
-/* Driver local functions.                                                   */
-/*===========================================================================*/
-
-#if (LIS302DL_USE_SPI) || defined(__DOXYGEN__)
-/**
- * @brief   Reads a generic register value using SPI.
- * @pre     The SPI interface must be initialized and the driver started.
- *
- * @param[in] spip      pointer to the SPI interface
- * @param[in] reg       register number
- * @return              register value.
- */
-static uint8_t lis302dlSPIReadRegister(SPIDriver *spip, uint8_t reg) {
-  uint8_t txbuf[2] = {LIS302DL_RW | reg, 0xFF};
-  uint8_t rxbuf[2] = {0x00, 0x00};
-  spiSelect(spip);
-  spiExchange(spip, 2, txbuf, rxbuf);
-  spiUnselect(spip);
-  return rxbuf[1];
-}
-
-/**
- * @brief   Writes a value into a generic register using SPI.
- * @pre     The SPI interface must be initialized and the driver started.
- *
- * @param[in] spip      pointer to the SPI interface
- * @param[in] reg       register number
- * @param[in] value     register value.
- */
-static void lis302dlSPIWriteRegister(SPIDriver *spip, uint8_t reg,
-                                   uint8_t value) {
-
-  switch (reg) {
-    default:
-      /* Reserved register must not be written, according to the datasheet
-       * this could permanently damage the device.
-       */
-      osalDbgAssert(FALSE, "lis302dlSPIWriteRegister(), reserved register");
-    case LIS302DL_AD_WHO_AM_I:
-
-    case LIS302DL_AD_HP_FILER_RESET:
-    case LIS302DL_AD_STATUS_REG:
-    case LIS302DL_AD_OUT_X:
-    case LIS302DL_AD_OUT_Y:
-    case LIS302DL_AD_OUT_Z:
-    case LIS302DL_AD_FF_WU_SRC_1:
-    case LIS302DL_AD_FF_WU_SRC_2:
-    case LIS302DL_AD_CLICK_SRC:
-    /* Read only registers cannot be written, the command is ignored.*/
-      return;
-    case LIS302DL_AD_CTRL_REG1:
-    case LIS302DL_AD_CTRL_REG2:
-    case LIS302DL_AD_CTRL_REG3:
-    case LIS302DL_AD_FF_WU_CFG_1:
-    case LIS302DL_AD_FF_WU_THS_1:
-    case LIS302DL_AD_FF_WU_DURATION_1:
-    case LIS302DL_AD_FF_WU_CFG_2:
-    case LIS302DL_AD_FF_WU_THS_2:
-    case LIS302DL_AD_FF_WU_DURATION_2:
-    case LIS302DL_AD_CLICK_CFG:
-    case LIS302DL_AD_CLICK_THSY_X:
-    case LIS302DL_AD_CLICK_THSZ:
-    case LIS302DL_AD_CLICK_TIME_LIMIT:
-    case LIS302DL_AD_CLICK_LATENCY:
-    case LIS302DL_AD_CLICK_WINDOW:
-      spiSelect(spip);
-      uint8_t txbuf[2] = {reg, value};
-      spiSend(spip, 2, txbuf);
-      spiUnselect(spip);
-  }
-}
-#endif /* LIS302DL_USE_SPI */
-
-/*
- * Interface implementation.
- */
-static size_t get_axes_number(void *ip) {
-
-  osalDbgCheck(ip != NULL);
-  return LIS302DL_NUMBER_OF_AXES;
-}
-
-static msg_t read_raw(void *ip, int32_t axes[LIS302DL_NUMBER_OF_AXES]) {
-  int8_t tmp;
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-  
-  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),
-              "read_raw(), invalid state");
-
-#if LIS302DL_USE_SPI
-  osalDbgAssert((((LIS302DLDriver *)ip)->config->spip->state == SPI_READY),
-                "read_raw(), channel not ready");
-#if	LIS302DL_SHARED_SPI
-  spiAcquireBus(((LIS302DLDriver *)ip)->config->spip);
-  spiStart(((LIS302DLDriver *)ip)->config->spip,
-           ((LIS302DLDriver *)ip)->config->spicfg);
-#endif /* LIS302DL_SHARED_SPI */   
-  if(((LIS302DLDriver *)ip)->config->axesenabling & LIS302DL_AE_X){
-    tmp = lis302dlSPIReadRegister(((LIS302DLDriver *)ip)->config->spip,
-										LIS302DL_AD_OUT_X);
-    axes[0] = tmp + ((LIS302DLDriver *)ip)->bias[0];
-  }
-  if(((LIS302DLDriver *)ip)->config->axesenabling & LIS302DL_AE_Y){
-    tmp = lis302dlSPIReadRegister(((LIS302DLDriver *)ip)->config->spip,
-										LIS302DL_AD_OUT_Y);
-    axes[1] = tmp  + ((LIS302DLDriver *)ip)->bias[1];
-  }
-  if(((LIS302DLDriver *)ip)->config->axesenabling & LIS302DL_AE_Z){
-    tmp = lis302dlSPIReadRegister(((LIS302DLDriver *)ip)->config->spip,
-										LIS302DL_AD_OUT_Z);
-    axes[2] = tmp + ((LIS302DLDriver *)ip)->bias[2];
-  }
-#if	LIS302DL_SHARED_SPI
-  spiReleaseBus(((LIS302DLDriver *)ip)->config->spip);
-#endif /* LIS302DL_SHARED_SPI */   
-#endif /* LIS302DL_USE_SPI */ 
-  return MSG_OK;
-}
-
-static msg_t read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[LIS302DL_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-
-  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),
-              "read_cooked(), invalid state");
-
-  msg = read_raw(ip, raw);
-  for(i = 0; i < LIS302DL_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] * ((LIS302DLDriver *)ip)->sensitivity[i];
-    if(((LIS302DLDriver *)ip)->config->unit == LIS302DL_ACC_UNIT_G){
-      axes[i] *= TO_G;
-    }
-    else if(((LIS302DLDriver *)ip)->config->unit == LIS302DL_ACC_UNIT_SI){
-      axes[i] *= TO_SI;
-    }
-  }
-  return msg;
-}
-
-static msg_t set_bias(void *ip, int32_t *bp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (bp !=NULL));
-
-  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY) ||
-                (((LIS302DLDriver *)ip)->state == LIS302DL_STOP),
-                "set_bias(), invalid state");
-  
-  for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++) {
-    ((LIS302DLDriver *)ip)->bias[i] = bp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t reset_bias(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY) ||
-                (((LIS302DLDriver *)ip)->state == LIS302DL_STOP),
-              "reset_bias(), invalid state");
-
-  for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
-    ((LIS302DLDriver *)ip)->bias[i] = 0;
-  return MSG_OK;
-}
-
-static msg_t set_sensivity(void *ip, float *sp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (sp !=NULL));
-
-  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),
-                "set_sensivity(), invalid state");
-  
-  for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++) {
-    ((LIS302DLDriver *)ip)->sensitivity[i] = sp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t reset_sensivity(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),
-                "reset_sensivity(), invalid state");
-
-  if(((LIS302DLDriver *)ip)->config->fullscale == LIS302DL_FS_2G)
-    for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
-      ((LIS302DLDriver *)ip)->sensitivity[i] = LIS302DL_SENS_2G;
-  else if(((LIS302DLDriver *)ip)->config->fullscale == LIS302DL_FS_8G)
-	for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
-      ((LIS302DLDriver *)ip)->sensitivity[i] = LIS302DL_SENS_8G;
-  else {
-    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");
-    return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
-static const struct BaseSensorVMT vmt_basesensor = {
-  get_axes_number, read_raw, read_cooked
-};
-
-static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {
-  get_axes_number, read_raw, read_cooked,
-  set_bias, reset_bias, set_sensivity, reset_sensivity 
-};
-
-static const struct LIS302DLVMT vmt_lis302dl = {
-  get_axes_number, read_raw, read_cooked,
-  set_bias, reset_bias, set_sensivity, reset_sensivity 
-};
-
-/*===========================================================================*/
-/* Driver exported functions.                                                */
-/*===========================================================================*/
-
-/**
- * @brief   Initializes an instance.
- *
- * @param[out] devp     pointer to the @p LIS302DLDriver object
- *
- * @init
- */
-void lis302dlObjectInit(LIS302DLDriver *devp) {
-  uint32_t i;
-  devp->vmt_basesensor = &vmt_basesensor;
-  devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;
-  devp->vmt_lis302dl = &vmt_lis302dl;
-  devp->config = NULL;
-  for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
-    devp->bias[i] = 0;
-  devp->state  = LIS302DL_STOP;
-}
-
-/**
- * @brief   Configures and activates LIS302DL Complex Driver peripheral.
- *
- * @param[in] devp      pointer to the @p LIS302DLDriver object
- * @param[in] config    pointer to the @p LIS302DLConfig object
- *
- * @api
- */
-void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config) {
-  uint32_t i;
-  osalDbgCheck((devp != NULL) && (config != NULL));
-
-  osalDbgAssert((devp->state == LIS302DL_STOP) || (devp->state == LIS302DL_READY),
-              "lis302dlStart(), invalid state");			  
-
-  devp->config = config;
-  
-#if LIS302DL_USE_SPI
-#if	LIS302DL_SHARED_SPI
-  spiAcquireBus((devp)->config->spip);
-#endif /* LIS302DL_SHARED_SPI */
-  spiStart((devp)->config->spip,
-           (devp)->config->spicfg);
-  lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG1,
-                         LIS302DL_PM_SLEEP_NORMAL |
-                         devp->config->axesenabling |
-						 devp->config->fullscale |
-						 devp->config->outputdatarate);
-  lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG2,
-                         devp->config->highpass);
-#if	LIS302DL_SHARED_SPI
-  spiReleaseBus((devp)->config->spip);
-#endif /* LIS302DL_SHARED_SPI */  
-#endif /* LIS302DL_USE_SPI */
-  
-  /* Storing sensitivity information according to full scale value */
-  if(devp->config->fullscale == LIS302DL_FS_2G)
-    for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS302DL_SENS_2G;
-  else if(devp->config->fullscale == LIS302DL_FS_8G)
-	for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS302DL_SENS_8G;
-  else {
-    osalDbgAssert(FALSE, "lis302dlStart(), accelerometer full scale issue");
-  }
-  /* This is the Accelerometer transient recovery time */
-  osalThreadSleepMilliseconds(10);
-
-  devp->state = LIS302DL_READY;
-} 
-
-/**
- * @brief   Deactivates the LIS302DL Complex Driver peripheral.
- *
- * @param[in] devp       pointer to the @p LIS302DLDriver object
- *
- * @api
- */
-void lis302dlStop(LIS302DLDriver *devp) {
-
-  osalDbgCheck(devp != NULL);
-
-  osalDbgAssert((devp->state == LIS302DL_STOP) || (devp->state == LIS302DL_READY),
-                "lis302dlStop(), invalid state");
-
-#if (LIS302DL_USE_SPI)
-  if (devp->state == LIS302DL_STOP) {
-#if	LIS302DL_SHARED_SPI
-    spiAcquireBus((devp)->config->spip);
-    spiStart((devp)->config->spip,
-             (devp)->config->spicfg);
-#endif /* LIS302DL_SHARED_SPI */
-    lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG1,
-                           LIS302DL_PM_POWER_DOWN | LIS302DL_AE_DISABLED);
-    spiStop((devp)->config->spip);
-#if	LIS302DL_SHARED_SPI
-    spiReleaseBus((devp)->config->spip);
-#endif /* LIS302DL_SHARED_SPI */    
-  }			  
-#endif /* LIS302DL_USE_SPI */
-  devp->state = LIS302DL_STOP;
-}
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lis302dl.c

+ * @brief   LIS302DL MEMS interface module code.

+ *

+ * @addtogroup lis302dl

+ * @{

+ */

+

+#include "hal.h"

+#include "lis302dl.h"

+

+/*===========================================================================*/

+/* Driver local definitions.                                                 */

+/*===========================================================================*/

+

+#define  LIS302DL_SENS_2G                   ((float)18.0f)

+#define  LIS302DL_SENS_8G                   ((float)72.0f)

+

+#define  LIS302DL_DI                        ((uint8_t)0xFF)

+#define  LIS302DL_DI_0                      ((uint8_t)0x01)

+#define  LIS302DL_DI_1                      ((uint8_t)0x02)

+#define  LIS302DL_DI_2                      ((uint8_t)0x04)

+#define  LIS302DL_DI_3                      ((uint8_t)0x08)

+#define  LIS302DL_DI_4                      ((uint8_t)0x10)

+#define  LIS302DL_DI_5                      ((uint8_t)0x20)

+#define  LIS302DL_DI_6                      ((uint8_t)0x40)

+#define  LIS302DL_DI_7                      ((uint8_t)0x80)

+

+#define  LIS302DL_AD                        ((uint8_t)0x3F)

+#define  LIS302DL_AD_0                      ((uint8_t)0x01)

+#define  LIS302DL_AD_1                      ((uint8_t)0x02)

+#define  LIS302DL_AD_2                      ((uint8_t)0x04)

+#define  LIS302DL_AD_3                      ((uint8_t)0x08)

+#define  LIS302DL_AD_4                      ((uint8_t)0x10)

+#define  LIS302DL_AD_5                      ((uint8_t)0x20)

+

+#define  LIS302DL_MS                        ((uint8_t)0x40)

+#define  LIS302DL_RW                        ((uint8_t)0x80)

+

+#define  LIS302DL_AD_WHO_AM_I               ((uint8_t)0x0F)

+#define  LIS302DL_AD_CTRL_REG1              ((uint8_t)0x20)

+#define  LIS302DL_AD_CTRL_REG2              ((uint8_t)0x21)

+#define  LIS302DL_AD_CTRL_REG3              ((uint8_t)0x22)

+#define  LIS302DL_AD_HP_FILER_RESET         ((uint8_t)0x23)

+#define  LIS302DL_AD_STATUS_REG             ((uint8_t)0x27)

+#define  LIS302DL_AD_OUT_X                  ((uint8_t)0x29)

+#define  LIS302DL_AD_OUT_Y                  ((uint8_t)0x2B)

+#define  LIS302DL_AD_OUT_Z                  ((uint8_t)0x2D)

+#define  LIS302DL_AD_FF_WU_CFG_1            ((uint8_t)0x30)

+#define  LIS302DL_AD_FF_WU_SRC_1            ((uint8_t)0x31)

+#define  LIS302DL_AD_FF_WU_THS_1            ((uint8_t)0x32)

+#define  LIS302DL_AD_FF_WU_DURATION_1       ((uint8_t)0x33)

+#define  LIS302DL_AD_FF_WU_CFG_2            ((uint8_t)0x34)

+#define  LIS302DL_AD_FF_WU_SRC_2            ((uint8_t)0x35)

+#define  LIS302DL_AD_FF_WU_THS_2            ((uint8_t)0x36)

+#define  LIS302DL_AD_FF_WU_DURATION_2       ((uint8_t)0x37)

+#define  LIS302DL_AD_CLICK_CFG              ((uint8_t)0x38)

+#define  LIS302DL_AD_CLICK_SRC              ((uint8_t)0x39)

+#define  LIS302DL_AD_CLICK_THSY_X           ((uint8_t)0x3B)

+#define  LIS302DL_AD_CLICK_THSZ             ((uint8_t)0x3C)

+#define  LIS302DL_AD_CLICK_TIME_LIMIT       ((uint8_t)0x3D)

+#define  LIS302DL_AD_CLICK_LATENCY          ((uint8_t)0x3E)

+#define  LIS302DL_AD_CLICK_WINDOW           ((uint8_t)0x3F)

+

+#define  LIS302DL_CTRL_REG1_FS_MASK         ((uint8_t)0x20)

+#define  TO_G                               ((float)0.001f)

+#define  TO_SI                              ((float)0.00981f)

+/*===========================================================================*/

+/* Driver exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local variables and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @brief  LIS302DL Power Mode

+ */

+typedef enum {

+  LIS302DL_PM_POWER_DOWN   = 0x00,  /**< Power down enabled.                */          

+  LIS302DL_PM_SLEEP_NORMAL = 0x40   /**< Normal operation mode.             */          

+}lis302dl_pm_t;

+

+/*===========================================================================*/

+/* Driver local functions.                                                   */

+/*===========================================================================*/

+

+#if (LIS302DL_USE_SPI) || defined(__DOXYGEN__)

+/**

+ * @brief   Reads a generic register value using SPI.

+ * @pre     The SPI interface must be initialized and the driver started.

+ *

+ * @param[in] spip      pointer to the SPI interface

+ * @param[in] reg       register number

+ * @return              register value.

+ */

+static uint8_t lis302dlSPIReadRegister(SPIDriver *spip, uint8_t reg) {

+  uint8_t txbuf[2] = {LIS302DL_RW | reg, 0xFF};

+  uint8_t rxbuf[2] = {0x00, 0x00};

+  spiSelect(spip);

+  spiExchange(spip, 2, txbuf, rxbuf);

+  spiUnselect(spip);

+  return rxbuf[1];

+}

+

+/**

+ * @brief   Writes a value into a generic register using SPI.

+ * @pre     The SPI interface must be initialized and the driver started.

+ *

+ * @param[in] spip      pointer to the SPI interface

+ * @param[in] reg       register number

+ * @param[in] value     register value.

+ */

+static void lis302dlSPIWriteRegister(SPIDriver *spip, uint8_t reg,

+                                   uint8_t value) {

+

+  switch (reg) {

+    default:

+      /* Reserved register must not be written, according to the datasheet

+       * this could permanently damage the device.

+       */

+      osalDbgAssert(FALSE, "lis302dlSPIWriteRegister(), reserved register");

+    case LIS302DL_AD_WHO_AM_I:

+

+    case LIS302DL_AD_HP_FILER_RESET:

+    case LIS302DL_AD_STATUS_REG:

+    case LIS302DL_AD_OUT_X:

+    case LIS302DL_AD_OUT_Y:

+    case LIS302DL_AD_OUT_Z:

+    case LIS302DL_AD_FF_WU_SRC_1:

+    case LIS302DL_AD_FF_WU_SRC_2:

+    case LIS302DL_AD_CLICK_SRC:

+    /* Read only registers cannot be written, the command is ignored.*/

+      return;

+    case LIS302DL_AD_CTRL_REG1:

+    case LIS302DL_AD_CTRL_REG2:

+    case LIS302DL_AD_CTRL_REG3:

+    case LIS302DL_AD_FF_WU_CFG_1:

+    case LIS302DL_AD_FF_WU_THS_1:

+    case LIS302DL_AD_FF_WU_DURATION_1:

+    case LIS302DL_AD_FF_WU_CFG_2:

+    case LIS302DL_AD_FF_WU_THS_2:

+    case LIS302DL_AD_FF_WU_DURATION_2:

+    case LIS302DL_AD_CLICK_CFG:

+    case LIS302DL_AD_CLICK_THSY_X:

+    case LIS302DL_AD_CLICK_THSZ:

+    case LIS302DL_AD_CLICK_TIME_LIMIT:

+    case LIS302DL_AD_CLICK_LATENCY:

+    case LIS302DL_AD_CLICK_WINDOW:

+      spiSelect(spip);

+      uint8_t txbuf[2] = {reg, value};

+      spiSend(spip, 2, txbuf);

+      spiUnselect(spip);

+  }

+}

+#endif /* LIS302DL_USE_SPI */

+

+/*

+ * Interface implementation.

+ */

+static size_t get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return LIS302DL_NUMBER_OF_AXES;

+}

+

+static msg_t read_raw(void *ip, int32_t axes[LIS302DL_NUMBER_OF_AXES]) {

+  int8_t tmp;

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+  

+  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),

+              "read_raw(), invalid state");

+

+#if LIS302DL_USE_SPI

+  osalDbgAssert((((LIS302DLDriver *)ip)->config->spip->state == SPI_READY),

+                "read_raw(), channel not ready");

+#if	LIS302DL_SHARED_SPI

+  spiAcquireBus(((LIS302DLDriver *)ip)->config->spip);

+  spiStart(((LIS302DLDriver *)ip)->config->spip,

+           ((LIS302DLDriver *)ip)->config->spicfg);

+#endif /* LIS302DL_SHARED_SPI */   

+  if(((LIS302DLDriver *)ip)->config->axesenabling & LIS302DL_AE_X){

+    tmp = lis302dlSPIReadRegister(((LIS302DLDriver *)ip)->config->spip,

+										LIS302DL_AD_OUT_X);

+    axes[0] = tmp + ((LIS302DLDriver *)ip)->bias[0];

+  }

+  if(((LIS302DLDriver *)ip)->config->axesenabling & LIS302DL_AE_Y){

+    tmp = lis302dlSPIReadRegister(((LIS302DLDriver *)ip)->config->spip,

+										LIS302DL_AD_OUT_Y);

+    axes[1] = tmp  + ((LIS302DLDriver *)ip)->bias[1];

+  }

+  if(((LIS302DLDriver *)ip)->config->axesenabling & LIS302DL_AE_Z){

+    tmp = lis302dlSPIReadRegister(((LIS302DLDriver *)ip)->config->spip,

+										LIS302DL_AD_OUT_Z);

+    axes[2] = tmp + ((LIS302DLDriver *)ip)->bias[2];

+  }

+#if	LIS302DL_SHARED_SPI

+  spiReleaseBus(((LIS302DLDriver *)ip)->config->spip);

+#endif /* LIS302DL_SHARED_SPI */   

+#endif /* LIS302DL_USE_SPI */ 

+  return MSG_OK;

+}

+

+static msg_t read_cooked(void *ip, float axes[]) {

+  uint32_t i;

+  int32_t raw[LIS302DL_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),

+              "read_cooked(), invalid state");

+

+  msg = read_raw(ip, raw);

+  for(i = 0; i < LIS302DL_NUMBER_OF_AXES ; i++){

+    axes[i] = raw[i] * ((LIS302DLDriver *)ip)->sensitivity[i];

+    if(((LIS302DLDriver *)ip)->config->unit == LIS302DL_ACC_UNIT_G){

+      axes[i] *= TO_G;

+    }

+    else if(((LIS302DLDriver *)ip)->config->unit == LIS302DL_ACC_UNIT_SI){

+      axes[i] *= TO_SI;

+    }

+  }

+  return msg;

+}

+

+static msg_t set_bias(void *ip, int32_t *bp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (bp !=NULL));

+

+  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY) ||

+                (((LIS302DLDriver *)ip)->state == LIS302DL_STOP),

+                "set_bias(), invalid state");

+  

+  for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++) {

+    ((LIS302DLDriver *)ip)->bias[i] = bp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t reset_bias(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY) ||

+                (((LIS302DLDriver *)ip)->state == LIS302DL_STOP),

+              "reset_bias(), invalid state");

+

+  for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)

+    ((LIS302DLDriver *)ip)->bias[i] = 0;

+  return MSG_OK;

+}

+

+static msg_t set_sensivity(void *ip, float *sp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (sp !=NULL));

+

+  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),

+                "set_sensivity(), invalid state");

+  

+  for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++) {

+    ((LIS302DLDriver *)ip)->sensitivity[i] = sp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t reset_sensivity(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LIS302DLDriver *)ip)->state == LIS302DL_READY),

+                "reset_sensivity(), invalid state");

+

+  if(((LIS302DLDriver *)ip)->config->fullscale == LIS302DL_FS_2G)

+    for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)

+      ((LIS302DLDriver *)ip)->sensitivity[i] = LIS302DL_SENS_2G;

+  else if(((LIS302DLDriver *)ip)->config->fullscale == LIS302DL_FS_8G)

+	for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)

+      ((LIS302DLDriver *)ip)->sensitivity[i] = LIS302DL_SENS_8G;

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");

+    return MSG_RESET;

+  }

+  return MSG_OK;

+}

+

+static const struct BaseSensorVMT vmt_basesensor = {

+  get_axes_number, read_raw, read_cooked

+};

+

+static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {

+  get_axes_number, read_raw, read_cooked,

+  set_bias, reset_bias, set_sensivity, reset_sensivity 

+};

+

+static const struct LIS302DLVMT vmt_lis302dl = {

+  get_axes_number, read_raw, read_cooked,

+  set_bias, reset_bias, set_sensivity, reset_sensivity 

+};

+

+/*===========================================================================*/

+/* Driver exported functions.                                                */

+/*===========================================================================*/

+

+/**

+ * @brief   Initializes an instance.

+ *

+ * @param[out] devp     pointer to the @p LIS302DLDriver object

+ *

+ * @init

+ */

+void lis302dlObjectInit(LIS302DLDriver *devp) {

+  uint32_t i;

+  devp->vmt_basesensor = &vmt_basesensor;

+  devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;

+  devp->vmt_lis302dl = &vmt_lis302dl;

+  devp->config = NULL;

+  for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)

+    devp->bias[i] = 0;

+  devp->state  = LIS302DL_STOP;

+}

+

+/**

+ * @brief   Configures and activates LIS302DL Complex Driver peripheral.

+ *

+ * @param[in] devp      pointer to the @p LIS302DLDriver object

+ * @param[in] config    pointer to the @p LIS302DLConfig object

+ *

+ * @api

+ */

+void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config) {

+  uint32_t i;

+  osalDbgCheck((devp != NULL) && (config != NULL));

+

+  osalDbgAssert((devp->state == LIS302DL_STOP) || (devp->state == LIS302DL_READY),

+              "lis302dlStart(), invalid state");			  

+

+  devp->config = config;

+  

+#if LIS302DL_USE_SPI

+#if	LIS302DL_SHARED_SPI

+  spiAcquireBus((devp)->config->spip);

+#endif /* LIS302DL_SHARED_SPI */

+  spiStart((devp)->config->spip,

+           (devp)->config->spicfg);

+  lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG1,

+                         LIS302DL_PM_SLEEP_NORMAL |

+                         devp->config->axesenabling |

+						 devp->config->fullscale |

+						 devp->config->outputdatarate);

+  lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG2,

+                         devp->config->highpass);

+#if	LIS302DL_SHARED_SPI

+  spiReleaseBus((devp)->config->spip);

+#endif /* LIS302DL_SHARED_SPI */  

+#endif /* LIS302DL_USE_SPI */

+  

+  /* Storing sensitivity information according to full scale value */

+  if(devp->config->fullscale == LIS302DL_FS_2G)

+    for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS302DL_SENS_2G;

+  else if(devp->config->fullscale == LIS302DL_FS_8G)

+	for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS302DL_SENS_8G;

+  else {

+    osalDbgAssert(FALSE, "lis302dlStart(), accelerometer full scale issue");

+  }

+  /* This is the Accelerometer transient recovery time */

+  osalThreadSleepMilliseconds(10);

+

+  devp->state = LIS302DL_READY;

+} 

+

+/**

+ * @brief   Deactivates the LIS302DL Complex Driver peripheral.

+ *

+ * @param[in] devp       pointer to the @p LIS302DLDriver object

+ *

+ * @api

+ */

+void lis302dlStop(LIS302DLDriver *devp) {

+

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == LIS302DL_STOP) || (devp->state == LIS302DL_READY),

+                "lis302dlStop(), invalid state");

+

+#if (LIS302DL_USE_SPI)

+  if (devp->state == LIS302DL_STOP) {

+#if	LIS302DL_SHARED_SPI

+    spiAcquireBus((devp)->config->spip);

+    spiStart((devp)->config->spip,

+             (devp)->config->spicfg);

+#endif /* LIS302DL_SHARED_SPI */

+    lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG1,

+                           LIS302DL_PM_POWER_DOWN | LIS302DL_AE_DISABLED);

+    spiStop((devp)->config->spip);

+#if	LIS302DL_SHARED_SPI

+    spiReleaseBus((devp)->config->spip);

+#endif /* LIS302DL_SHARED_SPI */    

+  }			  

+#endif /* LIS302DL_USE_SPI */

+  devp->state = LIS302DL_STOP;

+}

+/** @} */

diff --git a/os/ex/ST/lis302dl.h b/os/ex/ST/lis302dl.h
index baa513205..e4c175029 100644
--- a/os/ex/ST/lis302dl.h
+++ b/os/ex/ST/lis302dl.h
@@ -1,302 +1,302 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lis302dl.h
- * @brief   LIS302DL MEMS interface module header.
- *
- * @{
- */
-
-#ifndef _LIS302DL_H_
-#define _LIS302DL_H_
-
-#include "hal_accelerometer.h"
-
-/*===========================================================================*/
-/* Driver constants.                                                         */
-/*===========================================================================*/
-
-/**
- * @brief   LIS302DL number of axes.
- */
-#define  LIS302DL_NUMBER_OF_AXES            ((size_t)   3U)
-
-/*===========================================================================*/
-/* Driver pre-compile time settings.                                         */
-/*===========================================================================*/
-
-/**
- * @name    Configuration options
- * @{
- */
-/**
- * @brief   LIS302DL SPI interface switch.
- * @details If set to @p TRUE the support for SPI is included.
- * @note    The default is @p TRUE.
- */
-#if !defined(LIS302DL_USE_SPI) || defined(__DOXYGEN__)
-#define LIS302DL_USE_SPI                    TRUE
-#endif
-
-/**
- * @brief   LIS302DL I2C interface switch.
- * @details If set to @p TRUE the support for I2C is included.
- * @note    The default is @p FALSE.
- */
-#if !defined(LIS302DL_USE_I2C) || defined(__DOXYGEN__)
-#define LIS302DL_USE_I2C                    FALSE
-#endif
-
-/**
- * @brief   LIS302DL shared SPI switch.
- * @details If set to @p TRUE the device acquires SPI bus ownership
- *          on each transaction.
- * @note    The default is @p FALSE. Requires SPI_USE_MUTUAL_EXCLUSION.
- */
-#if !defined(LIS302DL_SHARED_SPI) || defined(__DOXYGEN__)
-#define LIS302DL_SHARED_SPI                 FALSE
-#endif
-/** @} */
-
-/*===========================================================================*/
-/* Derived constants and error checks.                                       */
-/*===========================================================================*/
-
-#if !(LIS302DL_USE_SPI ^ LIS302DL_USE_I2C)
-#error "LIS302DL_USE_SPI and LIS302DL_USE_I2C cannot be both true or both false"
-#endif
-
-#if LIS302DL_USE_SPI && !HAL_USE_SPI
-#error "LIS302DL_USE_SPI requires HAL_USE_SPI"
-#endif
-
-#if LIS302DL_USE_I2C && !HAL_USE_I2C
-#error "LIS302DL_USE_I2C requires HAL_USE_I2C"
-#endif
-
-#if LIS302DL_SHARED_SPI && !SPI_USE_MUTUAL_EXCLUSION
-#error "LIS302DL_SHARED_SPI requires SPI_USE_MUTUAL_EXCLUSION"
-#endif
-
-/*===========================================================================*/
-/* Driver data structures and types.                                         */
-/*===========================================================================*/
-
-/**
- * @name    LIS302DL data structures and types
- * @{
- */
-/**
- * @brief   LIS302DL full scale.
- */
-typedef enum {
-  LIS302DL_FS_2G = 0x00,            /**< Full scale ±2g.                    */
-  LIS302DL_FS_8G = 0x20             /**< Full scale ±8g.                    */
-}lis302dl_fs_t;
-
-/**
- * @brief   LIS302DL output data rate and bandwidth.
- */
-typedef enum {
-  LIS302DL_ODR_100HZ = 0x00,        /**< ODR 100 Hz.                        */
-  LIS302DL_ODR_400HZ = 0x80         /**< ODR 400 Hz.                        */
-}lis302dl_odr_t;
-
-/**
- * @brief   LIS302DL high pass filtering.
- */
-typedef enum {
-  LIS302DL_HP_DISABLED = 0x00,      /**< HP bypassed.                       */
-  LIS302DL_HP_0 = 0x10,             /**< HP cutoff 2Hz (ODR 100Hz) or 8Hz   */
-  LIS302DL_HP_1 = 0x11,             /**< HP cutoff 1Hz or 4Hz               */
-  LIS302DL_HP_2 = 0x12,             /**< HP cutoff 0.5Hz or 2Hz             */
-  LIS302DL_HP_3 = 0x13              /**< HP cutoff 0.25Hz or 1Hz            */
-}lis302dl_hp_t;
-
-/**
- * @brief   LIS302DL axes enabling.
- */
-typedef enum {
-  LIS302DL_AE_DISABLED = 0x00,      /**< All axes disabled.                 */
-  LIS302DL_AE_X = 0x01,             /**< Only X-axis enabled.               */
-  LIS302DL_AE_Y = 0x02,             /**< Only Y-axis enabled.               */
-  LIS302DL_AE_XY = 0x03,            /**< X and Y axes enabled.              */
-  LIS302DL_AE_Z = 0x04,             /**< Only Z-axis enabled.               */
-  LIS302DL_AE_XZ = 0x05,            /**< X and Z axes enabled.              */
-  LIS302DL_AE_YZ = 0x06,            /**< Y and Z axes enabled.              */
-  LIS302DL_AE_XYZ = 0x07            /**< All axes enabled.                  */
-}lis302dl_ae_t;
-
-/**
- * @brief   LIS302DL accelerometer subsystem unit.
- */
-typedef enum {
-  LIS302DL_ACC_UNIT_G = 0x00,       /**< Cooked data in g.                  */
-  LIS302DL_ACC_UNIT_MG = 0x01,      /**< Cooked data in mg.                 */
-  LIS302DL_ACC_UNIT_SI = 0x02,      /**< Cooked data in m/s^2.              */
-} lis302dl_acc_unit_t;
-
-/**
- * @brief   Driver state machine possible states.
- */
-typedef enum {
-  LIS302DL_UNINIT = 0,              /**< Not initialized.                   */
-  LIS302DL_STOP = 1,                /**< Stopped.                           */
-  LIS302DL_READY = 2,               /**< Ready.                             */
-} lis302dl_state_t;
-
-/**
- * @brief   LIS302DL configuration structure.
- */
-typedef struct {
-
-#if (LIS302DL_USE_SPI) || defined(__DOXYGEN__)
-  /**
-   * @brief SPI driver associated to this LIS302DL.
-   */
-  SPIDriver                 *spip;
-  /**
-   * @brief SPI configuration associated to this LIS302DL.
-   */
-  const SPIConfig           *spicfg;
-#endif /* LIS302DL_USE_SPI */
-#if (LIS302DL_USE_I2C) || defined(__DOXYGEN__)
-  /**
-   * @brief I2C driver associated to this LIS302DL.
-   */
-  I2CDriver                 *i2cp;
-  /**
-   * @brief I2C configuration associated to this LIS302DL.
-   */
-  const I2CConfig           *i2ccfg;
-#endif /* LIS302DL_USE_I2C */
-  /**
-   * @brief LIS302DL full scale value.
-   */
-  lis302dl_fs_t               fullscale;
-  /**
-   * @brief LIS302DL output data rate selection.
-   */
-  lis302dl_odr_t              outputdatarate;
-  /**
-   * @brief   LIS302DL high pass filtering
-   */
-  lis302dl_hp_t               highpass;
-  /**
-   * @brief LIS302DL axes enabling.
-   */
-  lis302dl_ae_t               axesenabling;
-  /**
-   * @brief LIS302DL unit for cooked data.
-   */
-  lis302dl_acc_unit_t         unit;
-} LIS302DLConfig;
-
-/**
- * @brief   Structure representing a LIS302DL driver.
- */
-typedef struct LIS302DLDriver LIS302DLDriver;
-
-/**
- * @brief   @p LIS302DL specific methods.
- */
-#define _lis302dl_methods                                                   \
-  _base_accelerometer_methods
-
-
-/**
- * @extends BaseAccelerometerVMT.
- *
- * @brief   @p LIS302DL virtual methods table.
- */
-struct LIS302DLVMT {
-  _lis302dl_methods
-};
-
-/**
- * @brief   @p LIS302DLDriver specific data.
- */
-#define _lis302dl_data                                                      \
-  _base_accelerometer_data                                                  \
-  /* Driver state.*/                                                        \
-  lis302dl_state_t            state;                                        \
-  /* Current configuration data.*/                                          \
-  const LIS302DLConfig        *config;                                      \
-  /* Current sensitivity.*/                                                 \
-  float                     sensitivity[LIS302DL_NUMBER_OF_AXES];           \
-  /* Bias data.*/                                                           \
-  int32_t                   bias[LIS302DL_NUMBER_OF_AXES];
-
-/**
- * @extends BaseAccelerometer.
- *
- * @brief   LIS302DL 3-axis accelerometer class.
- * @details This class extends @p BaseAccelerometer by adding physical
- *          driver implementation.
- */
-struct LIS302DLDriver {
-  /** @brief BaseSensor Virtual Methods Table. */
-  const struct BaseSensorVMT *vmt_basesensor;
-  /** @brief BaseAccelerometer Virtual Methods Table. */
-  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;
-  /** @brief LIS302DL Virtual Methods Table. */
-  const struct LIS302DLVMT *vmt_lis302dl;
-  _lis302dl_data
-};
-/** @} */
-
-/*===========================================================================*/
-/* Driver macros.                                                            */
-/*===========================================================================*/
-
-/**
- * @brief   Get current MEMS temperature.
- * @detail  This information is very useful especially for high accuracy IMU.
- *
- * @param[in] ip        pointer to a @p BaseAccelerometer class.
- * @param[out] temp     the MEMS temperature as single precision floating.
- *
- * @return              The operation status.
- * @retval MSG_OK       if the function succeeded.
- * @retval MSG_RESET    if one or more errors occurred.
- * @api
- */
-#define accelerometerGetTemp(ip, tpp)                                           \
-        (ip)->vmt_lis302dl->get_temperature(ip, tpp)
-
-/*===========================================================================*/
-/* External declarations.                                                    */
-/*===========================================================================*/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-  void lis302dlObjectInit(LIS302DLDriver *devp);
-  void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config);
-  void lis302dlStop(LIS302DLDriver *devp);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _LIS302DL_H_ */
-
-/** @} */
-
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lis302dl.h

+ * @brief   LIS302DL MEMS interface module header.

+ *

+ * @{

+ */

+

+#ifndef _LIS302DL_H_

+#define _LIS302DL_H_

+

+#include "hal_accelerometer.h"

+

+/*===========================================================================*/

+/* Driver constants.                                                         */

+/*===========================================================================*/

+

+/**

+ * @brief   LIS302DL number of axes.

+ */

+#define  LIS302DL_NUMBER_OF_AXES            ((size_t)   3U)

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    Configuration options

+ * @{

+ */

+/**

+ * @brief   LIS302DL SPI interface switch.

+ * @details If set to @p TRUE the support for SPI is included.

+ * @note    The default is @p TRUE.

+ */

+#if !defined(LIS302DL_USE_SPI) || defined(__DOXYGEN__)

+#define LIS302DL_USE_SPI                    TRUE

+#endif

+

+/**

+ * @brief   LIS302DL I2C interface switch.

+ * @details If set to @p TRUE the support for I2C is included.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(LIS302DL_USE_I2C) || defined(__DOXYGEN__)

+#define LIS302DL_USE_I2C                    FALSE

+#endif

+

+/**

+ * @brief   LIS302DL shared SPI switch.

+ * @details If set to @p TRUE the device acquires SPI bus ownership

+ *          on each transaction.

+ * @note    The default is @p FALSE. Requires SPI_USE_MUTUAL_EXCLUSION.

+ */

+#if !defined(LIS302DL_SHARED_SPI) || defined(__DOXYGEN__)

+#define LIS302DL_SHARED_SPI                 FALSE

+#endif

+/** @} */

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+#if !(LIS302DL_USE_SPI ^ LIS302DL_USE_I2C)

+#error "LIS302DL_USE_SPI and LIS302DL_USE_I2C cannot be both true or both false"

+#endif

+

+#if LIS302DL_USE_SPI && !HAL_USE_SPI

+#error "LIS302DL_USE_SPI requires HAL_USE_SPI"

+#endif

+

+#if LIS302DL_USE_I2C && !HAL_USE_I2C

+#error "LIS302DL_USE_I2C requires HAL_USE_I2C"

+#endif

+

+#if LIS302DL_SHARED_SPI && !SPI_USE_MUTUAL_EXCLUSION

+#error "LIS302DL_SHARED_SPI requires SPI_USE_MUTUAL_EXCLUSION"

+#endif

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    LIS302DL data structures and types

+ * @{

+ */

+/**

+ * @brief   LIS302DL full scale.

+ */

+typedef enum {

+  LIS302DL_FS_2G = 0x00,            /**< Full scale ±2g.                    */

+  LIS302DL_FS_8G = 0x20             /**< Full scale ±8g.                    */

+}lis302dl_fs_t;

+

+/**

+ * @brief   LIS302DL output data rate and bandwidth.

+ */

+typedef enum {

+  LIS302DL_ODR_100HZ = 0x00,        /**< ODR 100 Hz.                        */

+  LIS302DL_ODR_400HZ = 0x80         /**< ODR 400 Hz.                        */

+}lis302dl_odr_t;

+

+/**

+ * @brief   LIS302DL high pass filtering.

+ */

+typedef enum {

+  LIS302DL_HP_DISABLED = 0x00,      /**< HP bypassed.                       */

+  LIS302DL_HP_0 = 0x10,             /**< HP cutoff 2Hz (ODR 100Hz) or 8Hz   */

+  LIS302DL_HP_1 = 0x11,             /**< HP cutoff 1Hz or 4Hz               */

+  LIS302DL_HP_2 = 0x12,             /**< HP cutoff 0.5Hz or 2Hz             */

+  LIS302DL_HP_3 = 0x13              /**< HP cutoff 0.25Hz or 1Hz            */

+}lis302dl_hp_t;

+

+/**

+ * @brief   LIS302DL axes enabling.

+ */

+typedef enum {

+  LIS302DL_AE_DISABLED = 0x00,      /**< All axes disabled.                 */

+  LIS302DL_AE_X = 0x01,             /**< Only X-axis enabled.               */

+  LIS302DL_AE_Y = 0x02,             /**< Only Y-axis enabled.               */

+  LIS302DL_AE_XY = 0x03,            /**< X and Y axes enabled.              */

+  LIS302DL_AE_Z = 0x04,             /**< Only Z-axis enabled.               */

+  LIS302DL_AE_XZ = 0x05,            /**< X and Z axes enabled.              */

+  LIS302DL_AE_YZ = 0x06,            /**< Y and Z axes enabled.              */

+  LIS302DL_AE_XYZ = 0x07            /**< All axes enabled.                  */

+}lis302dl_ae_t;

+

+/**

+ * @brief   LIS302DL accelerometer subsystem unit.

+ */

+typedef enum {

+  LIS302DL_ACC_UNIT_G = 0x00,       /**< Cooked data in g.                  */

+  LIS302DL_ACC_UNIT_MG = 0x01,      /**< Cooked data in mg.                 */

+  LIS302DL_ACC_UNIT_SI = 0x02,      /**< Cooked data in m/s^2.              */

+} lis302dl_acc_unit_t;

+

+/**

+ * @brief   Driver state machine possible states.

+ */

+typedef enum {

+  LIS302DL_UNINIT = 0,              /**< Not initialized.                   */

+  LIS302DL_STOP = 1,                /**< Stopped.                           */

+  LIS302DL_READY = 2,               /**< Ready.                             */

+} lis302dl_state_t;

+

+/**

+ * @brief   LIS302DL configuration structure.

+ */

+typedef struct {

+

+#if (LIS302DL_USE_SPI) || defined(__DOXYGEN__)

+  /**

+   * @brief SPI driver associated to this LIS302DL.

+   */

+  SPIDriver                 *spip;

+  /**

+   * @brief SPI configuration associated to this LIS302DL.

+   */

+  const SPIConfig           *spicfg;

+#endif /* LIS302DL_USE_SPI */

+#if (LIS302DL_USE_I2C) || defined(__DOXYGEN__)

+  /**

+   * @brief I2C driver associated to this LIS302DL.

+   */

+  I2CDriver                 *i2cp;

+  /**

+   * @brief I2C configuration associated to this LIS302DL.

+   */

+  const I2CConfig           *i2ccfg;

+#endif /* LIS302DL_USE_I2C */

+  /**

+   * @brief LIS302DL full scale value.

+   */

+  lis302dl_fs_t               fullscale;

+  /**

+   * @brief LIS302DL output data rate selection.

+   */

+  lis302dl_odr_t              outputdatarate;

+  /**

+   * @brief   LIS302DL high pass filtering

+   */

+  lis302dl_hp_t               highpass;

+  /**

+   * @brief LIS302DL axes enabling.

+   */

+  lis302dl_ae_t               axesenabling;

+  /**

+   * @brief LIS302DL unit for cooked data.

+   */

+  lis302dl_acc_unit_t         unit;

+} LIS302DLConfig;

+

+/**

+ * @brief   Structure representing a LIS302DL driver.

+ */

+typedef struct LIS302DLDriver LIS302DLDriver;

+

+/**

+ * @brief   @p LIS302DL specific methods.

+ */

+#define _lis302dl_methods                                                   \

+  _base_accelerometer_methods

+

+

+/**

+ * @extends BaseAccelerometerVMT.

+ *

+ * @brief   @p LIS302DL virtual methods table.

+ */

+struct LIS302DLVMT {

+  _lis302dl_methods

+};

+

+/**

+ * @brief   @p LIS302DLDriver specific data.

+ */

+#define _lis302dl_data                                                      \

+  _base_accelerometer_data                                                  \

+  /* Driver state.*/                                                        \

+  lis302dl_state_t            state;                                        \

+  /* Current configuration data.*/                                          \

+  const LIS302DLConfig        *config;                                      \

+  /* Current sensitivity.*/                                                 \

+  float                     sensitivity[LIS302DL_NUMBER_OF_AXES];           \

+  /* Bias data.*/                                                           \

+  int32_t                   bias[LIS302DL_NUMBER_OF_AXES];

+

+/**

+ * @extends BaseAccelerometer.

+ *

+ * @brief   LIS302DL 3-axis accelerometer class.

+ * @details This class extends @p BaseAccelerometer by adding physical

+ *          driver implementation.

+ */

+struct LIS302DLDriver {

+  /** @brief BaseSensor Virtual Methods Table. */

+  const struct BaseSensorVMT *vmt_basesensor;

+  /** @brief BaseAccelerometer Virtual Methods Table. */

+  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;

+  /** @brief LIS302DL Virtual Methods Table. */

+  const struct LIS302DLVMT *vmt_lis302dl;

+  _lis302dl_data

+};

+/** @} */

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+/**

+ * @brief   Get current MEMS temperature.

+ * @detail  This information is very useful especially for high accuracy IMU.

+ *

+ * @param[in] ip        pointer to a @p BaseAccelerometer class.

+ * @param[out] temp     the MEMS temperature as single precision floating.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ * @api

+ */

+#define accelerometerGetTemp(ip, tpp)                                           \

+        (ip)->vmt_lis302dl->get_temperature(ip, tpp)

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void lis302dlObjectInit(LIS302DLDriver *devp);

+  void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config);

+  void lis302dlStop(LIS302DLDriver *devp);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* _LIS302DL_H_ */

+

+/** @} */

+

diff --git a/os/ex/ST/lis3dsh.c b/os/ex/ST/lis3dsh.c
index c28f60304..cdf4d9754 100644
--- a/os/ex/ST/lis3dsh.c
+++ b/os/ex/ST/lis3dsh.c
@@ -1,614 +1,614 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lis3dsh.c
- * @brief   LIS3DSH MEMS interface module code.
- *
- * @addtogroup lis3dsh
- * @{
- */
-
-#include "hal.h"
-#include "lis3dsh.h"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-#define  LIS3DSH_SENS_2G                    ((float)0.06f)
-#define  LIS3DSH_SENS_4G                    ((float)0.12f)
-#define  LIS3DSH_SENS_6G                    ((float)0.18f)
-#define  LIS3DSH_SENS_8G                    ((float)0.24f)
-#define  LIS3DSH_SENS_16G                   ((float)0.73f)
-
-#define  LIS3DSH_DI                         ((uint8_t)0xFF)
-#define  LIS3DSH_DI_0                       ((uint8_t)0x01)
-#define  LIS3DSH_DI_1                       ((uint8_t)0x02)
-#define  LIS3DSH_DI_2                       ((uint8_t)0x04)
-#define  LIS3DSH_DI_3                       ((uint8_t)0x08)
-#define  LIS3DSH_DI_4                       ((uint8_t)0x10)
-#define  LIS3DSH_DI_5                       ((uint8_t)0x20)
-#define  LIS3DSH_DI_6                       ((uint8_t)0x40)
-#define  LIS3DSH_DI_7                       ((uint8_t)0x80)
-
-#define  LIS3DSH_AD                         ((uint8_t)0x3F)
-#define  LIS3DSH_AD_0                       ((uint8_t)0x01)
-#define  LIS3DSH_AD_1                       ((uint8_t)0x02)
-#define  LIS3DSH_AD_2                       ((uint8_t)0x04)
-#define  LIS3DSH_AD_3                       ((uint8_t)0x08)
-#define  LIS3DSH_AD_4                       ((uint8_t)0x10)
-#define  LIS3DSH_AD_5                       ((uint8_t)0x20)
-
-#define  LIS3DSH_MS                         ((uint8_t)0x40)
-#define  LIS3DSH_RW                         ((uint8_t)0x80)
-
-#define  LIS3DSH_AD_INFO1                   ((uint8_t)0x0D)
-#define  LIS3DSH_AD_INFO2                   ((uint8_t)0x0E)
-#define  LIS3DSH_AD_WHO_AM_I                ((uint8_t)0x0F)
-#define  LIS3DSH_AD_CTRL_REG3               ((uint8_t)0x23)
-#define  LIS3DSH_AD_CTRL_REG4               ((uint8_t)0x20)
-#define  LIS3DSH_AD_CTRL_REG5               ((uint8_t)0x24)
-#define  LIS3DSH_AD_CTRL_REG6               ((uint8_t)0x25)
-#define  LIS3DSH_AD_STATUS                  ((uint8_t)0x27)
-#define  LIS3DSH_AD_OUT_T                   ((uint8_t)0x0C)
-#define  LIS3DSH_AD_OFF_X                   ((uint8_t)0x10)
-#define  LIS3DSH_AD_OFF_Y                   ((uint8_t)0x11)
-#define  LIS3DSH_AD_OFF_Z                   ((uint8_t)0x12)
-#define  LIS3DSH_AD_CS_X                    ((uint8_t)0x13)
-#define  LIS3DSH_AD_CS_Y                    ((uint8_t)0x14)
-#define  LIS3DSH_AD_CS_Z                    ((uint8_t)0x15)
-#define  LIS3DSH_AD_LC_L                    ((uint8_t)0x16)
-#define  LIS3DSH_AD_LC_H                    ((uint8_t)0x17)
-#define  LIS3DSH_AD_STAT                    ((uint8_t)0x18)
-#define  LIS3DSH_AD_VFC_1                   ((uint8_t)0x1B)
-#define  LIS3DSH_AD_VFC_2                   ((uint8_t)0x1C)
-#define  LIS3DSH_AD_VFC_3                   ((uint8_t)0x1D)
-#define  LIS3DSH_AD_VFC_4                   ((uint8_t)0x1E)
-#define  LIS3DSH_AD_THRS3                   ((uint8_t)0x1F)
-#define  LIS3DSH_AD_OUT_X_L                 ((uint8_t)0x28)
-#define  LIS3DSH_AD_OUT_X_H                 ((uint8_t)0x29)
-#define  LIS3DSH_AD_OUT_Y_L                 ((uint8_t)0x2A)
-#define  LIS3DSH_AD_OUT_Y_H                 ((uint8_t)0x2B)
-#define  LIS3DSH_AD_OUT_Z_L                 ((uint8_t)0x2C)
-#define  LIS3DSH_AD_OUT_Z_H                 ((uint8_t)0x2D)
-#define  LIS3DSH_AD_FIFO_CTRL               ((uint8_t)0x2E)
-#define  LIS3DSH_AD_FIFO_SRC                ((uint8_t)0x2F)
-#define  LIS3DSH_AD_CTRL_REG1               ((uint8_t)0x21)
-#define  LIS3DSH_AD_ST1_0                   ((uint8_t)0x40)
-#define  LIS3DSH_AD_ST1_1                   ((uint8_t)0x41)
-#define  LIS3DSH_AD_ST1_2                   ((uint8_t)0x42)
-#define  LIS3DSH_AD_ST1_3                   ((uint8_t)0x43)
-#define  LIS3DSH_AD_ST1_4                   ((uint8_t)0x44)
-#define  LIS3DSH_AD_ST1_5                   ((uint8_t)0x45)
-#define  LIS3DSH_AD_ST1_6                   ((uint8_t)0x46)
-#define  LIS3DSH_AD_ST1_7                   ((uint8_t)0x47)
-#define  LIS3DSH_AD_ST1_8                   ((uint8_t)0x48)
-#define  LIS3DSH_AD_ST1_9                   ((uint8_t)0x49)
-#define  LIS3DSH_AD_ST1_A                   ((uint8_t)0x4A)
-#define  LIS3DSH_AD_ST1_B                   ((uint8_t)0x4B)
-#define  LIS3DSH_AD_ST1_C                   ((uint8_t)0x4C)
-#define  LIS3DSH_AD_ST1_D                   ((uint8_t)0x4D)
-#define  LIS3DSH_AD_ST1_E                   ((uint8_t)0x4E)
-#define  LIS3DSH_AD_ST1_F                   ((uint8_t)0x4F)
-#define  LIS3DSH_AD_TIM4_1                  ((uint8_t)0x50)
-#define  LIS3DSH_AD_TIM3_1                  ((uint8_t)0x51)
-#define  LIS3DSH_AD_TIM2_1_L                ((uint8_t)0x52)
-#define  LIS3DSH_AD_TIM2_1_H                ((uint8_t)0x53)
-#define  LIS3DSH_AD_TIM1_1_L                ((uint8_t)0x54)
-#define  LIS3DSH_AD_TIM1_1_H                ((uint8_t)0x55)
-#define  LIS3DSH_AD_THRS2_1                 ((uint8_t)0x56)
-#define  LIS3DSH_AD_THRS1_1                 ((uint8_t)0x57)
-#define  LIS3DSH_AD_MASK1_B                 ((uint8_t)0x59)
-#define  LIS3DSH_AD_MASK1_A                 ((uint8_t)0x5A)
-#define  LIS3DSH_AD_SETT1                   ((uint8_t)0x5B)
-#define  LIS3DSH_AD_PR1                     ((uint8_t)0x5C)
-#define  LIS3DSH_AD_TC1_L                   ((uint8_t)0x5D)
-#define  LIS3DSH_AD_TC1_H                   ((uint8_t)0x5E)
-#define  LIS3DSH_AD_OUTS1                   ((uint8_t)0x5F)
-#define  LIS3DSH_AD_PEAK1                   ((uint8_t)0x19)
-#define  LIS3DSH_AD_CTRL_REG2               ((uint8_t)0x22)
-#define  LIS3DSH_AD_ST2_0                   ((uint8_t)0x60)
-#define  LIS3DSH_AD_ST2_1                   ((uint8_t)0x61)
-#define  LIS3DSH_AD_ST2_2                   ((uint8_t)0x62)
-#define  LIS3DSH_AD_ST2_3                   ((uint8_t)0x63)
-#define  LIS3DSH_AD_ST2_4                   ((uint8_t)0x64)
-#define  LIS3DSH_AD_ST2_5                   ((uint8_t)0x65)
-#define  LIS3DSH_AD_ST2_6                   ((uint8_t)0x66)
-#define  LIS3DSH_AD_ST2_7                   ((uint8_t)0x67)
-#define  LIS3DSH_AD_ST2_8                   ((uint8_t)0x68)
-#define  LIS3DSH_AD_ST2_9                   ((uint8_t)0x69)
-#define  LIS3DSH_AD_ST2_A                   ((uint8_t)0x6A)
-#define  LIS3DSH_AD_ST2_B                   ((uint8_t)0x6B)
-#define  LIS3DSH_AD_ST2_C                   ((uint8_t)0x6C)
-#define  LIS3DSH_AD_ST2_D                   ((uint8_t)0x6D)
-#define  LIS3DSH_AD_ST2_E                   ((uint8_t)0x6E)
-#define  LIS3DSH_AD_ST2_F                   ((uint8_t)0x6F)
-#define  LIS3DSH_AD_TIM4_2                  ((uint8_t)0x70)
-#define  LIS3DSH_AD_TIM3_2                  ((uint8_t)0x71)
-#define  LIS3DSH_AD_TIM2_2_L                ((uint8_t)0x72)
-#define  LIS3DSH_AD_TIM2_2_H                ((uint8_t)0x73)
-#define  LIS3DSH_AD_TIM1_2_L                ((uint8_t)0x74)
-#define  LIS3DSH_AD_TIM1_2_H                ((uint8_t)0x75)
-#define  LIS3DSH_AD_THRS2_2                 ((uint8_t)0x76)
-#define  LIS3DSH_AD_THRS1_2                 ((uint8_t)0x77)
-#define  LIS3DSH_AD_MASK2_B                 ((uint8_t)0x79)
-#define  LIS3DSH_AD_MASK2_A                 ((uint8_t)0x7A)
-#define  LIS3DSH_AD_SETT2                   ((uint8_t)0x7B)
-#define  LIS3DSH_AD_PR2                     ((uint8_t)0x7C)
-#define  LIS3DSH_AD_TC2_L                   ((uint8_t)0x7D)
-#define  LIS3DSH_AD_TC2_H                   ((uint8_t)0x7E)
-#define  LIS3DSH_AD_OUTS2                   ((uint8_t)0x7F)
-#define  LIS3DSH_AD_PEAK2                   ((uint8_t)0x1A)
-#define  LIS3DSH_AD_DES2                    ((uint8_t)0x78)
-
-#define  LIS3DSH_CTRL_REG5_FS_MASK          ((uint8_t)0x38)
-#define  TO_G                               ((float)0.001f)
-#define  TO_SI                              ((float)0.00981f)
-/*===========================================================================*/
-/* Driver exported variables.                                                */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local variables and types.                                         */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local functions.                                                   */
-/*===========================================================================*/
-
-#if (LIS3DSH_USE_SPI) || defined(__DOXYGEN__)
-/**
- * @brief   Reads a generic register value using SPI.
- * @pre     The SPI interface must be initialized and the driver started.
- *
- * @param[in] spip      pointer to the SPI interface
- * @param[in] reg       register number
- * @return              register value.
- */
-static uint8_t lis3dshSPIReadRegister(SPIDriver *spip, uint8_t reg) {
-  uint8_t txbuf[2] = {LIS3DSH_RW | reg, 0xFF};
-  uint8_t rxbuf[2] = {0x00, 0x00};
-  spiSelect(spip);
-  spiExchange(spip, 2, txbuf, rxbuf);
-  spiUnselect(spip);
-  return rxbuf[1];
-}
-
-/**
- * @brief   Writes a value into a generic register using SPI.
- * @pre     The SPI interface must be initialized and the driver started.
- *
- * @param[in] spip      pointer to the SPI interface
- * @param[in] reg       register number
- * @param[in] value     register value.
- */
-static void lis3dshSPIWriteRegister(SPIDriver *spip, uint8_t reg,
-                                   uint8_t value) {
-
-  switch (reg) {
-    default:
-      /* Reserved register must not be written, according to the datasheet
-       * this could permanently damage the device.
-       */
-      osalDbgAssert(FALSE, "lis3dshSPIWriteRegister(), reserved register");
-    case LIS3DSH_AD_INFO1:
-    case LIS3DSH_AD_INFO2:
-    case LIS3DSH_AD_WHO_AM_I:
-    case LIS3DSH_AD_STATUS:
-    case LIS3DSH_AD_OUT_T:
-    case LIS3DSH_AD_STAT:
-    case LIS3DSH_AD_OUT_X_L:
-    case LIS3DSH_AD_OUT_X_H:
-    case LIS3DSH_AD_OUT_Y_L:
-    case LIS3DSH_AD_OUT_Y_H:
-    case LIS3DSH_AD_OUT_Z_L:
-    case LIS3DSH_AD_OUT_Z_H:
-    case LIS3DSH_AD_FIFO_SRC:
-	case LIS3DSH_AD_FIFO_CTRL:
-    case LIS3DSH_AD_PR1:
-    case LIS3DSH_AD_TC1_L:
-    case LIS3DSH_AD_TC1_H:
-    case LIS3DSH_AD_OUTS1:
-    case LIS3DSH_AD_PEAK1:
-    case LIS3DSH_AD_PR2:
-    case LIS3DSH_AD_TC2_L:
-    case LIS3DSH_AD_TC2_H:
-    case LIS3DSH_AD_OUTS2:
-    case LIS3DSH_AD_PEAK2:
-    /* Read only registers cannot be written, the command is ignored.*/
-      return;
-    case LIS3DSH_AD_CTRL_REG3:
-    case LIS3DSH_AD_CTRL_REG4:
-    case LIS3DSH_AD_CTRL_REG5:
-    case LIS3DSH_AD_CTRL_REG6:
-    case LIS3DSH_AD_OFF_X:
-    case LIS3DSH_AD_OFF_Y:
-    case LIS3DSH_AD_OFF_Z:
-    case LIS3DSH_AD_CS_X:
-    case LIS3DSH_AD_CS_Y:
-    case LIS3DSH_AD_CS_Z:
-    case LIS3DSH_AD_LC_L:
-    case LIS3DSH_AD_LC_H:
-    case LIS3DSH_AD_VFC_1:
-    case LIS3DSH_AD_VFC_2:
-    case LIS3DSH_AD_VFC_3:
-    case LIS3DSH_AD_VFC_4:
-    case LIS3DSH_AD_THRS3:
-    case LIS3DSH_AD_CTRL_REG1:
-	case LIS3DSH_AD_ST1_0:
-    case LIS3DSH_AD_ST1_1:
-    case LIS3DSH_AD_ST1_2:
-    case LIS3DSH_AD_ST1_3:
-    case LIS3DSH_AD_ST1_4:
-    case LIS3DSH_AD_ST1_5:
-    case LIS3DSH_AD_ST1_6:
-    case LIS3DSH_AD_ST1_7:
-    case LIS3DSH_AD_ST1_8:
-    case LIS3DSH_AD_ST1_9:
-    case LIS3DSH_AD_ST1_A:
-    case LIS3DSH_AD_ST1_B:
-    case LIS3DSH_AD_ST1_C:
-    case LIS3DSH_AD_ST1_D:
-    case LIS3DSH_AD_ST1_E:
-    case LIS3DSH_AD_ST1_F:
-    case LIS3DSH_AD_TIM4_1:
-    case LIS3DSH_AD_TIM3_1:
-    case LIS3DSH_AD_TIM2_1_L:
-    case LIS3DSH_AD_TIM2_1_H:
-    case LIS3DSH_AD_TIM1_1_L:
-    case LIS3DSH_AD_TIM1_1_H:
-    case LIS3DSH_AD_THRS2_1:
-    case LIS3DSH_AD_THRS1_1:
-    case LIS3DSH_AD_MASK1_B:
-    case LIS3DSH_AD_MASK1_A:
-    case LIS3DSH_AD_SETT1:
-	case LIS3DSH_AD_CTRL_REG2:
-	case LIS3DSH_AD_ST2_0:
-    case LIS3DSH_AD_ST2_1:
-    case LIS3DSH_AD_ST2_2:
-    case LIS3DSH_AD_ST2_3:
-    case LIS3DSH_AD_ST2_4:
-    case LIS3DSH_AD_ST2_5:
-    case LIS3DSH_AD_ST2_6:
-    case LIS3DSH_AD_ST2_7:
-    case LIS3DSH_AD_ST2_8:
-    case LIS3DSH_AD_ST2_9:
-    case LIS3DSH_AD_ST2_A:
-    case LIS3DSH_AD_ST2_B:
-    case LIS3DSH_AD_ST2_C:
-    case LIS3DSH_AD_ST2_D:
-    case LIS3DSH_AD_ST2_E:
-    case LIS3DSH_AD_ST2_F:
-    case LIS3DSH_AD_TIM4_2:
-    case LIS3DSH_AD_TIM3_2:
-    case LIS3DSH_AD_TIM2_2_L:
-    case LIS3DSH_AD_TIM2_2_H:
-    case LIS3DSH_AD_TIM1_2_L:
-    case LIS3DSH_AD_TIM1_2_H:
-    case LIS3DSH_AD_THRS2_2:
-    case LIS3DSH_AD_THRS1_2:
-    case LIS3DSH_AD_MASK2_B:
-    case LIS3DSH_AD_MASK2_A:
-    case LIS3DSH_AD_SETT2:
-	case LIS3DSH_AD_DES2:
-      spiSelect(spip);
-      uint8_t txbuf[2] = {reg, value};
-      spiSend(spip, 2, txbuf);
-      spiUnselect(spip);
-  }
-}
-#endif /* LIS3DSH_USE_SPI */
-
-/*
- * Interface implementation.
- */
-static size_t get_axes_number(void *ip) {
-
-  osalDbgCheck(ip != NULL);
-  return LIS3DSH_NUMBER_OF_AXES;
-}
-
-static msg_t read_raw(void *ip, int32_t axes[LIS3DSH_NUMBER_OF_AXES]) {
-  int16_t tmp;
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-
-  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY),
-              "read_raw(), invalid state");
-
-#if LIS3DSH_USE_SPI
-  osalDbgAssert((((LIS3DSHDriver *)ip)->config->spip->state == SPI_READY),
-                "read_raw(), channel not ready");
-#if	LIS3DSH_SHARED_SPI
-  spiAcquireBus(((LIS3DSHDriver *)ip)->config->spip);
-  spiStart(((LIS3DSHDriver *)ip)->config->spip,
-           ((LIS3DSHDriver *)ip)->config->spicfg);
-#endif /* LIS3DSH_SHARED_SPI */   
-  if(((LIS3DSHDriver *)ip)->config->axesenabling & LIS3DSH_AE_X){
-    tmp = lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,
-                                  LIS3DSH_AD_OUT_X_L);
-    tmp += lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,
-                                   LIS3DSH_AD_OUT_X_H) << 8;
-    axes[0] = (int32_t)tmp + ((LIS3DSHDriver *)ip)->bias[0];
-  }
-  if(((LIS3DSHDriver *)ip)->config->axesenabling & LIS3DSH_AE_Y){
-    tmp = lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,
-                                  LIS3DSH_AD_OUT_Y_L);
-    tmp += lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,
-                                   LIS3DSH_AD_OUT_Y_H) << 8;
-    axes[1] = (int32_t)tmp + ((LIS3DSHDriver *)ip)->bias[1];
-  }
-  if(((LIS3DSHDriver *)ip)->config->axesenabling & LIS3DSH_AE_Z){
-    tmp = lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,
-                                  LIS3DSH_AD_OUT_Z_L);
-    tmp += lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,
-                                   LIS3DSH_AD_OUT_Z_H) << 8;
-    axes[2] = (int32_t)tmp + ((LIS3DSHDriver *)ip)->bias[2];
-  }
-#if	LIS3DSH_SHARED_SPI
-  spiReleaseBus(((LIS3DSHDriver *)ip)->config->spip);
-#endif /* LIS3DSH_SHARED_SPI */   
-#endif /* LIS3DSH_USE_SPI */ 
-  return MSG_OK;
-}
-
-static msg_t read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[LIS3DSH_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-
-  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY),
-              "read_cooked(), invalid state");
-
-  msg = read_raw(ip, raw);
-  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] * ((LIS3DSHDriver *)ip)->sensitivity[i];
-    if(((LIS3DSHDriver *)ip)->config->unit == LIS3DSH_ACC_UNIT_G){
-      axes[i] *= TO_G;
-    }
-    else if(((LIS3DSHDriver *)ip)->config->unit == LIS3DSH_ACC_UNIT_SI){
-      axes[i] *= TO_SI;
-    }
-  }
-  return msg;
-}
-
-static msg_t set_bias(void *ip, int32_t *bp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (bp !=NULL));
-
-  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY) ||
-                (((LIS3DSHDriver *)ip)->state == LIS3DSH_STOP),
-                "set_bias(), invalid state");
-  
-  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) {
-    ((LIS3DSHDriver *)ip)->bias[i] = bp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t reset_bias(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY) ||
-                (((LIS3DSHDriver *)ip)->state == LIS3DSH_STOP),
-              "reset_bias(), invalid state");
-
-  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-    ((LIS3DSHDriver *)ip)->bias[i] = 0;
-  return MSG_OK;
-}
-
-static msg_t set_sensivity(void *ip, float *sp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (sp !=NULL));
-
-  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY),
-                "set_sensivity(), invalid state");
-  
-  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) {
-    ((LIS3DSHDriver *)ip)->sensitivity[i] = sp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t reset_sensivity(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY),
-                "reset_sensivity(), invalid state");
-
-  if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_2G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_2G;
-  else if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_4G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_4G;
-  else if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_6G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_6G;
-  else if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_8G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_8G;
-  else if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_16G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_16G;
-  else {
-    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");
-    return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
-static msg_t get_temperature(void *ip, int8_t* tempp) {
-#if LIS3DSH_USE_SPI
-  osalDbgAssert((((LIS3DSHDriver *)ip)->config->spip->state == SPI_READY),
-                "read_raw(), channel not ready");
-#if	LIS3DSH_SHARED_SPI
-  spiAcquireBus(((LIS3DSHDriver *)ip)->config->spip);
-  spiStart(((LIS3DSHDriver *)ip)->config->spip,
-           ((LIS3DSHDriver *)ip)->config->spicfg);
-#endif /* LIS3DSH_SHARED_SPI */   
-  *tempp = lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,
-                                    LIS3DSH_AD_OUT_T);
-#if	LIS3DSH_SHARED_SPI
-  spiReleaseBus(((LIS3DSHDriver *)ip)->config->spip);
-#endif /* LIS3DSH_SHARED_SPI */   
-#endif /* LIS3DSH_USE_SPI */ 
-  return MSG_OK;
-}
-
-static const struct BaseSensorVMT vmt_basesensor = {
-  get_axes_number, read_raw, read_cooked
-};
-
-static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {
-  get_axes_number, read_raw, read_cooked,
-  set_bias, reset_bias, set_sensivity, reset_sensivity 
-};
-
-static const struct LIS3DSHVMT vmt_lis3dsh = {
-  get_axes_number, read_raw, read_cooked,
-  set_bias, reset_bias, set_sensivity, reset_sensivity,
-  get_temperature
-};
-
-/*===========================================================================*/
-/* Driver exported functions.                                                */
-/*===========================================================================*/
-
-/**
- * @brief   Initializes an instance.
- *
- * @param[out] devp     pointer to the @p LIS3DSHDriver object
- *
- * @init
- */
-void lis3dshObjectInit(LIS3DSHDriver *devp) {
-  uint32_t i;
-  devp->vmt_basesensor = &vmt_basesensor;
-  devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;
-  devp->vmt_lis3dsh = &vmt_lis3dsh;
-  devp->config = NULL;
-  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-    devp->bias[i] = 0;
-  devp->state  = LIS3DSH_STOP;
-}
-
-/**
- * @brief   Configures and activates LIS3DSH Complex Driver peripheral.
- *
- * @param[in] devp      pointer to the @p LIS3DSHDriver object
- * @param[in] config    pointer to the @p LIS3DSHConfig object
- *
- * @api
- */
-void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) {
-  uint32_t i;
-  osalDbgCheck((devp != NULL) && (config != NULL));
-
-  osalDbgAssert((devp->state == LIS3DSH_STOP) || (devp->state == LIS3DSH_READY),
-              "lis3dshStart(), invalid state");			  
-
-  devp->config = config;
-  
-#if LIS3DSH_USE_SPI
-#if	LIS3DSH_SHARED_SPI
-  spiAcquireBus((devp)->config->spip);
-#endif /* LIS3DSH_SHARED_SPI */
-  spiStart((devp)->config->spip,
-           (devp)->config->spicfg);
-  lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG4,
-                         devp->config->axesenabling |
-                         devp->config->blockdataupdate |
-						 devp->config->outputdatarate);
-  lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG5,
-                         devp->config->fullscale |
-                         devp->config->antialiasing );
-#if	LIS3DSH_SHARED_SPI
-  spiReleaseBus((devp)->config->spip);
-#endif /* LIS3DSH_SHARED_SPI */  
-#endif /* LIS3DSH_USE_SPI */
-  
-  /* Storing sensitivity information according to full scale value */
-  if(devp->config->fullscale == LIS3DSH_FS_2G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3DSH_SENS_2G;
-  else if(devp->config->fullscale == LIS3DSH_FS_4G)
-	for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3DSH_SENS_4G;
-  else if(devp->config->fullscale == LIS3DSH_FS_6G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3DSH_SENS_6G;
-  else if(devp->config->fullscale == LIS3DSH_FS_8G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3DSH_SENS_8G;
-  else if(devp->config->fullscale == LIS3DSH_FS_16G)
-    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3DSH_SENS_16G;
-  else {
-    osalDbgAssert(FALSE, "lis3dshStart(), accelerometer full scale issue");
-  }
-  /* This is the Accelerometer transient recovery time */
-  osalThreadSleepMilliseconds(10);
-
-  devp->state = LIS3DSH_READY;
-} 
-
-/**
- * @brief   Deactivates the LIS3DSH Complex Driver peripheral.
- *
- * @param[in] devp       pointer to the @p LIS3DSHDriver object
- *
- * @api
- */
-void lis3dshStop(LIS3DSHDriver *devp) {
-
-  osalDbgCheck(devp != NULL);
-
-  osalDbgAssert((devp->state == LIS3DSH_STOP) || (devp->state == LIS3DSH_READY),
-                "lis3dshStop(), invalid state");
-
-#if (LIS3DSH_USE_SPI)
-  if (devp->state == LIS3DSH_STOP) {
-#if	LIS3DSH_SHARED_SPI
-    spiAcquireBus((devp)->config->spip);
-    spiStart((devp)->config->spip,
-             (devp)->config->spicfg);
-#endif /* LIS3DSH_SHARED_SPI */
-    lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG4,
-                           LIS3DSH_ODR_PD | LIS3DSH_AE_DISABLED);
-    spiStop((devp)->config->spip);
-#if	LIS3DSH_SHARED_SPI
-    spiReleaseBus((devp)->config->spip);
-#endif /* LIS3DSH_SHARED_SPI */    
-  }			  
-#endif /* LIS3DSH_USE_SPI */
-  devp->state = LIS3DSH_STOP;
-}
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lis3dsh.c

+ * @brief   LIS3DSH MEMS interface module code.

+ *

+ * @addtogroup lis3dsh

+ * @{

+ */

+

+#include "hal.h"

+#include "lis3dsh.h"

+

+/*===========================================================================*/

+/* Driver local definitions.                                                 */

+/*===========================================================================*/

+

+#define  LIS3DSH_SENS_2G                    ((float)0.06f)

+#define  LIS3DSH_SENS_4G                    ((float)0.12f)

+#define  LIS3DSH_SENS_6G                    ((float)0.18f)

+#define  LIS3DSH_SENS_8G                    ((float)0.24f)

+#define  LIS3DSH_SENS_16G                   ((float)0.73f)

+

+#define  LIS3DSH_DI                         ((uint8_t)0xFF)

+#define  LIS3DSH_DI_0                       ((uint8_t)0x01)

+#define  LIS3DSH_DI_1                       ((uint8_t)0x02)

+#define  LIS3DSH_DI_2                       ((uint8_t)0x04)

+#define  LIS3DSH_DI_3                       ((uint8_t)0x08)

+#define  LIS3DSH_DI_4                       ((uint8_t)0x10)

+#define  LIS3DSH_DI_5                       ((uint8_t)0x20)

+#define  LIS3DSH_DI_6                       ((uint8_t)0x40)

+#define  LIS3DSH_DI_7                       ((uint8_t)0x80)

+

+#define  LIS3DSH_AD                         ((uint8_t)0x3F)

+#define  LIS3DSH_AD_0                       ((uint8_t)0x01)

+#define  LIS3DSH_AD_1                       ((uint8_t)0x02)

+#define  LIS3DSH_AD_2                       ((uint8_t)0x04)

+#define  LIS3DSH_AD_3                       ((uint8_t)0x08)

+#define  LIS3DSH_AD_4                       ((uint8_t)0x10)

+#define  LIS3DSH_AD_5                       ((uint8_t)0x20)

+

+#define  LIS3DSH_MS                         ((uint8_t)0x40)

+#define  LIS3DSH_RW                         ((uint8_t)0x80)

+

+#define  LIS3DSH_AD_INFO1                   ((uint8_t)0x0D)

+#define  LIS3DSH_AD_INFO2                   ((uint8_t)0x0E)

+#define  LIS3DSH_AD_WHO_AM_I                ((uint8_t)0x0F)

+#define  LIS3DSH_AD_CTRL_REG3               ((uint8_t)0x23)

+#define  LIS3DSH_AD_CTRL_REG4               ((uint8_t)0x20)

+#define  LIS3DSH_AD_CTRL_REG5               ((uint8_t)0x24)

+#define  LIS3DSH_AD_CTRL_REG6               ((uint8_t)0x25)

+#define  LIS3DSH_AD_STATUS                  ((uint8_t)0x27)

+#define  LIS3DSH_AD_OUT_T                   ((uint8_t)0x0C)

+#define  LIS3DSH_AD_OFF_X                   ((uint8_t)0x10)

+#define  LIS3DSH_AD_OFF_Y                   ((uint8_t)0x11)

+#define  LIS3DSH_AD_OFF_Z                   ((uint8_t)0x12)

+#define  LIS3DSH_AD_CS_X                    ((uint8_t)0x13)

+#define  LIS3DSH_AD_CS_Y                    ((uint8_t)0x14)

+#define  LIS3DSH_AD_CS_Z                    ((uint8_t)0x15)

+#define  LIS3DSH_AD_LC_L                    ((uint8_t)0x16)

+#define  LIS3DSH_AD_LC_H                    ((uint8_t)0x17)

+#define  LIS3DSH_AD_STAT                    ((uint8_t)0x18)

+#define  LIS3DSH_AD_VFC_1                   ((uint8_t)0x1B)

+#define  LIS3DSH_AD_VFC_2                   ((uint8_t)0x1C)

+#define  LIS3DSH_AD_VFC_3                   ((uint8_t)0x1D)

+#define  LIS3DSH_AD_VFC_4                   ((uint8_t)0x1E)

+#define  LIS3DSH_AD_THRS3                   ((uint8_t)0x1F)

+#define  LIS3DSH_AD_OUT_X_L                 ((uint8_t)0x28)

+#define  LIS3DSH_AD_OUT_X_H                 ((uint8_t)0x29)

+#define  LIS3DSH_AD_OUT_Y_L                 ((uint8_t)0x2A)

+#define  LIS3DSH_AD_OUT_Y_H                 ((uint8_t)0x2B)

+#define  LIS3DSH_AD_OUT_Z_L                 ((uint8_t)0x2C)

+#define  LIS3DSH_AD_OUT_Z_H                 ((uint8_t)0x2D)

+#define  LIS3DSH_AD_FIFO_CTRL               ((uint8_t)0x2E)

+#define  LIS3DSH_AD_FIFO_SRC                ((uint8_t)0x2F)

+#define  LIS3DSH_AD_CTRL_REG1               ((uint8_t)0x21)

+#define  LIS3DSH_AD_ST1_0                   ((uint8_t)0x40)

+#define  LIS3DSH_AD_ST1_1                   ((uint8_t)0x41)

+#define  LIS3DSH_AD_ST1_2                   ((uint8_t)0x42)

+#define  LIS3DSH_AD_ST1_3                   ((uint8_t)0x43)

+#define  LIS3DSH_AD_ST1_4                   ((uint8_t)0x44)

+#define  LIS3DSH_AD_ST1_5                   ((uint8_t)0x45)

+#define  LIS3DSH_AD_ST1_6                   ((uint8_t)0x46)

+#define  LIS3DSH_AD_ST1_7                   ((uint8_t)0x47)

+#define  LIS3DSH_AD_ST1_8                   ((uint8_t)0x48)

+#define  LIS3DSH_AD_ST1_9                   ((uint8_t)0x49)

+#define  LIS3DSH_AD_ST1_A                   ((uint8_t)0x4A)

+#define  LIS3DSH_AD_ST1_B                   ((uint8_t)0x4B)

+#define  LIS3DSH_AD_ST1_C                   ((uint8_t)0x4C)

+#define  LIS3DSH_AD_ST1_D                   ((uint8_t)0x4D)

+#define  LIS3DSH_AD_ST1_E                   ((uint8_t)0x4E)

+#define  LIS3DSH_AD_ST1_F                   ((uint8_t)0x4F)

+#define  LIS3DSH_AD_TIM4_1                  ((uint8_t)0x50)

+#define  LIS3DSH_AD_TIM3_1                  ((uint8_t)0x51)

+#define  LIS3DSH_AD_TIM2_1_L                ((uint8_t)0x52)

+#define  LIS3DSH_AD_TIM2_1_H                ((uint8_t)0x53)

+#define  LIS3DSH_AD_TIM1_1_L                ((uint8_t)0x54)

+#define  LIS3DSH_AD_TIM1_1_H                ((uint8_t)0x55)

+#define  LIS3DSH_AD_THRS2_1                 ((uint8_t)0x56)

+#define  LIS3DSH_AD_THRS1_1                 ((uint8_t)0x57)

+#define  LIS3DSH_AD_MASK1_B                 ((uint8_t)0x59)

+#define  LIS3DSH_AD_MASK1_A                 ((uint8_t)0x5A)

+#define  LIS3DSH_AD_SETT1                   ((uint8_t)0x5B)

+#define  LIS3DSH_AD_PR1                     ((uint8_t)0x5C)

+#define  LIS3DSH_AD_TC1_L                   ((uint8_t)0x5D)

+#define  LIS3DSH_AD_TC1_H                   ((uint8_t)0x5E)

+#define  LIS3DSH_AD_OUTS1                   ((uint8_t)0x5F)

+#define  LIS3DSH_AD_PEAK1                   ((uint8_t)0x19)

+#define  LIS3DSH_AD_CTRL_REG2               ((uint8_t)0x22)

+#define  LIS3DSH_AD_ST2_0                   ((uint8_t)0x60)

+#define  LIS3DSH_AD_ST2_1                   ((uint8_t)0x61)

+#define  LIS3DSH_AD_ST2_2                   ((uint8_t)0x62)

+#define  LIS3DSH_AD_ST2_3                   ((uint8_t)0x63)

+#define  LIS3DSH_AD_ST2_4                   ((uint8_t)0x64)

+#define  LIS3DSH_AD_ST2_5                   ((uint8_t)0x65)

+#define  LIS3DSH_AD_ST2_6                   ((uint8_t)0x66)

+#define  LIS3DSH_AD_ST2_7                   ((uint8_t)0x67)

+#define  LIS3DSH_AD_ST2_8                   ((uint8_t)0x68)

+#define  LIS3DSH_AD_ST2_9                   ((uint8_t)0x69)

+#define  LIS3DSH_AD_ST2_A                   ((uint8_t)0x6A)

+#define  LIS3DSH_AD_ST2_B                   ((uint8_t)0x6B)

+#define  LIS3DSH_AD_ST2_C                   ((uint8_t)0x6C)

+#define  LIS3DSH_AD_ST2_D                   ((uint8_t)0x6D)

+#define  LIS3DSH_AD_ST2_E                   ((uint8_t)0x6E)

+#define  LIS3DSH_AD_ST2_F                   ((uint8_t)0x6F)

+#define  LIS3DSH_AD_TIM4_2                  ((uint8_t)0x70)

+#define  LIS3DSH_AD_TIM3_2                  ((uint8_t)0x71)

+#define  LIS3DSH_AD_TIM2_2_L                ((uint8_t)0x72)

+#define  LIS3DSH_AD_TIM2_2_H                ((uint8_t)0x73)

+#define  LIS3DSH_AD_TIM1_2_L                ((uint8_t)0x74)

+#define  LIS3DSH_AD_TIM1_2_H                ((uint8_t)0x75)

+#define  LIS3DSH_AD_THRS2_2                 ((uint8_t)0x76)

+#define  LIS3DSH_AD_THRS1_2                 ((uint8_t)0x77)

+#define  LIS3DSH_AD_MASK2_B                 ((uint8_t)0x79)

+#define  LIS3DSH_AD_MASK2_A                 ((uint8_t)0x7A)

+#define  LIS3DSH_AD_SETT2                   ((uint8_t)0x7B)

+#define  LIS3DSH_AD_PR2                     ((uint8_t)0x7C)

+#define  LIS3DSH_AD_TC2_L                   ((uint8_t)0x7D)

+#define  LIS3DSH_AD_TC2_H                   ((uint8_t)0x7E)

+#define  LIS3DSH_AD_OUTS2                   ((uint8_t)0x7F)

+#define  LIS3DSH_AD_PEAK2                   ((uint8_t)0x1A)

+#define  LIS3DSH_AD_DES2                    ((uint8_t)0x78)

+

+#define  LIS3DSH_CTRL_REG5_FS_MASK          ((uint8_t)0x38)

+#define  TO_G                               ((float)0.001f)

+#define  TO_SI                              ((float)0.00981f)

+/*===========================================================================*/

+/* Driver exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local variables and types.                                         */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local functions.                                                   */

+/*===========================================================================*/

+

+#if (LIS3DSH_USE_SPI) || defined(__DOXYGEN__)

+/**

+ * @brief   Reads a generic register value using SPI.

+ * @pre     The SPI interface must be initialized and the driver started.

+ *

+ * @param[in] spip      pointer to the SPI interface

+ * @param[in] reg       register number

+ * @return              register value.

+ */

+static uint8_t lis3dshSPIReadRegister(SPIDriver *spip, uint8_t reg) {

+  uint8_t txbuf[2] = {LIS3DSH_RW | reg, 0xFF};

+  uint8_t rxbuf[2] = {0x00, 0x00};

+  spiSelect(spip);

+  spiExchange(spip, 2, txbuf, rxbuf);

+  spiUnselect(spip);

+  return rxbuf[1];

+}

+

+/**

+ * @brief   Writes a value into a generic register using SPI.

+ * @pre     The SPI interface must be initialized and the driver started.

+ *

+ * @param[in] spip      pointer to the SPI interface

+ * @param[in] reg       register number

+ * @param[in] value     register value.

+ */

+static void lis3dshSPIWriteRegister(SPIDriver *spip, uint8_t reg,

+                                   uint8_t value) {

+

+  switch (reg) {

+    default:

+      /* Reserved register must not be written, according to the datasheet

+       * this could permanently damage the device.

+       */

+      osalDbgAssert(FALSE, "lis3dshSPIWriteRegister(), reserved register");

+    case LIS3DSH_AD_INFO1:

+    case LIS3DSH_AD_INFO2:

+    case LIS3DSH_AD_WHO_AM_I:

+    case LIS3DSH_AD_STATUS:

+    case LIS3DSH_AD_OUT_T:

+    case LIS3DSH_AD_STAT:

+    case LIS3DSH_AD_OUT_X_L:

+    case LIS3DSH_AD_OUT_X_H:

+    case LIS3DSH_AD_OUT_Y_L:

+    case LIS3DSH_AD_OUT_Y_H:

+    case LIS3DSH_AD_OUT_Z_L:

+    case LIS3DSH_AD_OUT_Z_H:

+    case LIS3DSH_AD_FIFO_SRC:

+	case LIS3DSH_AD_FIFO_CTRL:

+    case LIS3DSH_AD_PR1:

+    case LIS3DSH_AD_TC1_L:

+    case LIS3DSH_AD_TC1_H:

+    case LIS3DSH_AD_OUTS1:

+    case LIS3DSH_AD_PEAK1:

+    case LIS3DSH_AD_PR2:

+    case LIS3DSH_AD_TC2_L:

+    case LIS3DSH_AD_TC2_H:

+    case LIS3DSH_AD_OUTS2:

+    case LIS3DSH_AD_PEAK2:

+    /* Read only registers cannot be written, the command is ignored.*/

+      return;

+    case LIS3DSH_AD_CTRL_REG3:

+    case LIS3DSH_AD_CTRL_REG4:

+    case LIS3DSH_AD_CTRL_REG5:

+    case LIS3DSH_AD_CTRL_REG6:

+    case LIS3DSH_AD_OFF_X:

+    case LIS3DSH_AD_OFF_Y:

+    case LIS3DSH_AD_OFF_Z:

+    case LIS3DSH_AD_CS_X:

+    case LIS3DSH_AD_CS_Y:

+    case LIS3DSH_AD_CS_Z:

+    case LIS3DSH_AD_LC_L:

+    case LIS3DSH_AD_LC_H:

+    case LIS3DSH_AD_VFC_1:

+    case LIS3DSH_AD_VFC_2:

+    case LIS3DSH_AD_VFC_3:

+    case LIS3DSH_AD_VFC_4:

+    case LIS3DSH_AD_THRS3:

+    case LIS3DSH_AD_CTRL_REG1:

+	case LIS3DSH_AD_ST1_0:

+    case LIS3DSH_AD_ST1_1:

+    case LIS3DSH_AD_ST1_2:

+    case LIS3DSH_AD_ST1_3:

+    case LIS3DSH_AD_ST1_4:

+    case LIS3DSH_AD_ST1_5:

+    case LIS3DSH_AD_ST1_6:

+    case LIS3DSH_AD_ST1_7:

+    case LIS3DSH_AD_ST1_8:

+    case LIS3DSH_AD_ST1_9:

+    case LIS3DSH_AD_ST1_A:

+    case LIS3DSH_AD_ST1_B:

+    case LIS3DSH_AD_ST1_C:

+    case LIS3DSH_AD_ST1_D:

+    case LIS3DSH_AD_ST1_E:

+    case LIS3DSH_AD_ST1_F:

+    case LIS3DSH_AD_TIM4_1:

+    case LIS3DSH_AD_TIM3_1:

+    case LIS3DSH_AD_TIM2_1_L:

+    case LIS3DSH_AD_TIM2_1_H:

+    case LIS3DSH_AD_TIM1_1_L:

+    case LIS3DSH_AD_TIM1_1_H:

+    case LIS3DSH_AD_THRS2_1:

+    case LIS3DSH_AD_THRS1_1:

+    case LIS3DSH_AD_MASK1_B:

+    case LIS3DSH_AD_MASK1_A:

+    case LIS3DSH_AD_SETT1:

+	case LIS3DSH_AD_CTRL_REG2:

+	case LIS3DSH_AD_ST2_0:

+    case LIS3DSH_AD_ST2_1:

+    case LIS3DSH_AD_ST2_2:

+    case LIS3DSH_AD_ST2_3:

+    case LIS3DSH_AD_ST2_4:

+    case LIS3DSH_AD_ST2_5:

+    case LIS3DSH_AD_ST2_6:

+    case LIS3DSH_AD_ST2_7:

+    case LIS3DSH_AD_ST2_8:

+    case LIS3DSH_AD_ST2_9:

+    case LIS3DSH_AD_ST2_A:

+    case LIS3DSH_AD_ST2_B:

+    case LIS3DSH_AD_ST2_C:

+    case LIS3DSH_AD_ST2_D:

+    case LIS3DSH_AD_ST2_E:

+    case LIS3DSH_AD_ST2_F:

+    case LIS3DSH_AD_TIM4_2:

+    case LIS3DSH_AD_TIM3_2:

+    case LIS3DSH_AD_TIM2_2_L:

+    case LIS3DSH_AD_TIM2_2_H:

+    case LIS3DSH_AD_TIM1_2_L:

+    case LIS3DSH_AD_TIM1_2_H:

+    case LIS3DSH_AD_THRS2_2:

+    case LIS3DSH_AD_THRS1_2:

+    case LIS3DSH_AD_MASK2_B:

+    case LIS3DSH_AD_MASK2_A:

+    case LIS3DSH_AD_SETT2:

+	case LIS3DSH_AD_DES2:

+      spiSelect(spip);

+      uint8_t txbuf[2] = {reg, value};

+      spiSend(spip, 2, txbuf);

+      spiUnselect(spip);

+  }

+}

+#endif /* LIS3DSH_USE_SPI */

+

+/*

+ * Interface implementation.

+ */

+static size_t get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return LIS3DSH_NUMBER_OF_AXES;

+}

+

+static msg_t read_raw(void *ip, int32_t axes[LIS3DSH_NUMBER_OF_AXES]) {

+  int16_t tmp;

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY),

+              "read_raw(), invalid state");

+

+#if LIS3DSH_USE_SPI

+  osalDbgAssert((((LIS3DSHDriver *)ip)->config->spip->state == SPI_READY),

+                "read_raw(), channel not ready");

+#if	LIS3DSH_SHARED_SPI

+  spiAcquireBus(((LIS3DSHDriver *)ip)->config->spip);

+  spiStart(((LIS3DSHDriver *)ip)->config->spip,

+           ((LIS3DSHDriver *)ip)->config->spicfg);

+#endif /* LIS3DSH_SHARED_SPI */   

+  if(((LIS3DSHDriver *)ip)->config->axesenabling & LIS3DSH_AE_X){

+    tmp = lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,

+                                  LIS3DSH_AD_OUT_X_L);

+    tmp += lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,

+                                   LIS3DSH_AD_OUT_X_H) << 8;

+    axes[0] = (int32_t)tmp + ((LIS3DSHDriver *)ip)->bias[0];

+  }

+  if(((LIS3DSHDriver *)ip)->config->axesenabling & LIS3DSH_AE_Y){

+    tmp = lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,

+                                  LIS3DSH_AD_OUT_Y_L);

+    tmp += lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,

+                                   LIS3DSH_AD_OUT_Y_H) << 8;

+    axes[1] = (int32_t)tmp + ((LIS3DSHDriver *)ip)->bias[1];

+  }

+  if(((LIS3DSHDriver *)ip)->config->axesenabling & LIS3DSH_AE_Z){

+    tmp = lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,

+                                  LIS3DSH_AD_OUT_Z_L);

+    tmp += lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,

+                                   LIS3DSH_AD_OUT_Z_H) << 8;

+    axes[2] = (int32_t)tmp + ((LIS3DSHDriver *)ip)->bias[2];

+  }

+#if	LIS3DSH_SHARED_SPI

+  spiReleaseBus(((LIS3DSHDriver *)ip)->config->spip);

+#endif /* LIS3DSH_SHARED_SPI */   

+#endif /* LIS3DSH_USE_SPI */ 

+  return MSG_OK;

+}

+

+static msg_t read_cooked(void *ip, float axes[]) {

+  uint32_t i;

+  int32_t raw[LIS3DSH_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY),

+              "read_cooked(), invalid state");

+

+  msg = read_raw(ip, raw);

+  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES ; i++){

+    axes[i] = raw[i] * ((LIS3DSHDriver *)ip)->sensitivity[i];

+    if(((LIS3DSHDriver *)ip)->config->unit == LIS3DSH_ACC_UNIT_G){

+      axes[i] *= TO_G;

+    }

+    else if(((LIS3DSHDriver *)ip)->config->unit == LIS3DSH_ACC_UNIT_SI){

+      axes[i] *= TO_SI;

+    }

+  }

+  return msg;

+}

+

+static msg_t set_bias(void *ip, int32_t *bp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (bp !=NULL));

+

+  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY) ||

+                (((LIS3DSHDriver *)ip)->state == LIS3DSH_STOP),

+                "set_bias(), invalid state");

+  

+  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) {

+    ((LIS3DSHDriver *)ip)->bias[i] = bp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t reset_bias(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY) ||

+                (((LIS3DSHDriver *)ip)->state == LIS3DSH_STOP),

+              "reset_bias(), invalid state");

+

+  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+    ((LIS3DSHDriver *)ip)->bias[i] = 0;

+  return MSG_OK;

+}

+

+static msg_t set_sensivity(void *ip, float *sp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (sp !=NULL));

+

+  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY),

+                "set_sensivity(), invalid state");

+  

+  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) {

+    ((LIS3DSHDriver *)ip)->sensitivity[i] = sp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t reset_sensivity(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LIS3DSHDriver *)ip)->state == LIS3DSH_READY),

+                "reset_sensivity(), invalid state");

+

+  if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_2G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_2G;

+  else if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_4G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_4G;

+  else if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_6G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_6G;

+  else if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_8G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_8G;

+  else if(((LIS3DSHDriver *)ip)->config->fullscale == LIS3DSH_FS_16G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      ((LIS3DSHDriver *)ip)->sensitivity[i] = LIS3DSH_SENS_16G;

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");

+    return MSG_RESET;

+  }

+  return MSG_OK;

+}

+

+static msg_t get_temperature(void *ip, int8_t* tempp) {

+#if LIS3DSH_USE_SPI

+  osalDbgAssert((((LIS3DSHDriver *)ip)->config->spip->state == SPI_READY),

+                "read_raw(), channel not ready");

+#if	LIS3DSH_SHARED_SPI

+  spiAcquireBus(((LIS3DSHDriver *)ip)->config->spip);

+  spiStart(((LIS3DSHDriver *)ip)->config->spip,

+           ((LIS3DSHDriver *)ip)->config->spicfg);

+#endif /* LIS3DSH_SHARED_SPI */   

+  *tempp = lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip,

+                                    LIS3DSH_AD_OUT_T);

+#if	LIS3DSH_SHARED_SPI

+  spiReleaseBus(((LIS3DSHDriver *)ip)->config->spip);

+#endif /* LIS3DSH_SHARED_SPI */   

+#endif /* LIS3DSH_USE_SPI */ 

+  return MSG_OK;

+}

+

+static const struct BaseSensorVMT vmt_basesensor = {

+  get_axes_number, read_raw, read_cooked

+};

+

+static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {

+  get_axes_number, read_raw, read_cooked,

+  set_bias, reset_bias, set_sensivity, reset_sensivity 

+};

+

+static const struct LIS3DSHVMT vmt_lis3dsh = {

+  get_axes_number, read_raw, read_cooked,

+  set_bias, reset_bias, set_sensivity, reset_sensivity,

+  get_temperature

+};

+

+/*===========================================================================*/

+/* Driver exported functions.                                                */

+/*===========================================================================*/

+

+/**

+ * @brief   Initializes an instance.

+ *

+ * @param[out] devp     pointer to the @p LIS3DSHDriver object

+ *

+ * @init

+ */

+void lis3dshObjectInit(LIS3DSHDriver *devp) {

+  uint32_t i;

+  devp->vmt_basesensor = &vmt_basesensor;

+  devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;

+  devp->vmt_lis3dsh = &vmt_lis3dsh;

+  devp->config = NULL;

+  for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+    devp->bias[i] = 0;

+  devp->state  = LIS3DSH_STOP;

+}

+

+/**

+ * @brief   Configures and activates LIS3DSH Complex Driver peripheral.

+ *

+ * @param[in] devp      pointer to the @p LIS3DSHDriver object

+ * @param[in] config    pointer to the @p LIS3DSHConfig object

+ *

+ * @api

+ */

+void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) {

+  uint32_t i;

+  osalDbgCheck((devp != NULL) && (config != NULL));

+

+  osalDbgAssert((devp->state == LIS3DSH_STOP) || (devp->state == LIS3DSH_READY),

+              "lis3dshStart(), invalid state");			  

+

+  devp->config = config;

+  

+#if LIS3DSH_USE_SPI

+#if	LIS3DSH_SHARED_SPI

+  spiAcquireBus((devp)->config->spip);

+#endif /* LIS3DSH_SHARED_SPI */

+  spiStart((devp)->config->spip,

+           (devp)->config->spicfg);

+  lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG4,

+                         devp->config->axesenabling |

+                         devp->config->blockdataupdate |

+						 devp->config->outputdatarate);

+  lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG5,

+                         devp->config->fullscale |

+                         devp->config->antialiasing );

+#if	LIS3DSH_SHARED_SPI

+  spiReleaseBus((devp)->config->spip);

+#endif /* LIS3DSH_SHARED_SPI */  

+#endif /* LIS3DSH_USE_SPI */

+  

+  /* Storing sensitivity information according to full scale value */

+  if(devp->config->fullscale == LIS3DSH_FS_2G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3DSH_SENS_2G;

+  else if(devp->config->fullscale == LIS3DSH_FS_4G)

+	for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3DSH_SENS_4G;

+  else if(devp->config->fullscale == LIS3DSH_FS_6G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3DSH_SENS_6G;

+  else if(devp->config->fullscale == LIS3DSH_FS_8G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3DSH_SENS_8G;

+  else if(devp->config->fullscale == LIS3DSH_FS_16G)

+    for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3DSH_SENS_16G;

+  else {

+    osalDbgAssert(FALSE, "lis3dshStart(), accelerometer full scale issue");

+  }

+  /* This is the Accelerometer transient recovery time */

+  osalThreadSleepMilliseconds(10);

+

+  devp->state = LIS3DSH_READY;

+} 

+

+/**

+ * @brief   Deactivates the LIS3DSH Complex Driver peripheral.

+ *

+ * @param[in] devp       pointer to the @p LIS3DSHDriver object

+ *

+ * @api

+ */

+void lis3dshStop(LIS3DSHDriver *devp) {

+

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == LIS3DSH_STOP) || (devp->state == LIS3DSH_READY),

+                "lis3dshStop(), invalid state");

+

+#if (LIS3DSH_USE_SPI)

+  if (devp->state == LIS3DSH_STOP) {

+#if	LIS3DSH_SHARED_SPI

+    spiAcquireBus((devp)->config->spip);

+    spiStart((devp)->config->spip,

+             (devp)->config->spicfg);

+#endif /* LIS3DSH_SHARED_SPI */

+    lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG4,

+                           LIS3DSH_ODR_PD | LIS3DSH_AE_DISABLED);

+    spiStop((devp)->config->spip);

+#if	LIS3DSH_SHARED_SPI

+    spiReleaseBus((devp)->config->spip);

+#endif /* LIS3DSH_SHARED_SPI */    

+  }			  

+#endif /* LIS3DSH_USE_SPI */

+  devp->state = LIS3DSH_STOP;

+}

+/** @} */

diff --git a/os/ex/ST/lis3dsh.h b/os/ex/ST/lis3dsh.h
index 1f0ebcd17..9cc202039 100644
--- a/os/ex/ST/lis3dsh.h
+++ b/os/ex/ST/lis3dsh.h
@@ -1,326 +1,326 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lis3dsh.h
- * @brief   LIS3DSH MEMS interface module header.
- *
- * @{
- */
-
-#ifndef _LIS3DSH_H_
-#define _LIS3DSH_H_
-
-#include "hal_accelerometer.h"
-
-/*===========================================================================*/
-/* Driver constants.                                                         */
-/*===========================================================================*/
-
-/**
- * @brief   LIS3DSH number of axes.
- */
-#define  LIS3DSH_NUMBER_OF_AXES             ((size_t)   3U)
-
-/*===========================================================================*/
-/* Driver pre-compile time settings.                                         */
-/*===========================================================================*/
-
-/**
- * @name    Configuration options.
- * @{
- */
-/**
- * @brief   LIS3DSH SPI interface switch.
- * @details If set to @p TRUE the support for SPI is included.
- * @note    The default is @p TRUE.
- */
-#if !defined(LIS3DSH_USE_SPI) || defined(__DOXYGEN__)
-#define LIS3DSH_USE_SPI                     TRUE
-#endif
-
-/**
- * @brief   LIS3DSH I2C interface switch.
- * @details If set to @p TRUE the support for I2C is included.
- * @note    The default is @p FALSE.
- */
-#if !defined(LIS3DSH_USE_I2C) || defined(__DOXYGEN__)
-#define LIS3DSH_USE_I2C                     FALSE
-#endif
-
-/**
- * @brief   LIS3DSH shared SPI switch.
- * @details If set to @p TRUE the device acquires SPI bus ownership
- *          on each transaction.
- * @note    The default is @p FALSE. Requires SPI_USE_MUTUAL_EXCLUSION.
- */
-#if !defined(LIS3DSH_SHARED_SPI) || defined(__DOXYGEN__)
-#define LIS3DSH_SHARED_SPI                  FALSE
-#endif
-/** @} */
-
-/*===========================================================================*/
-/* Derived constants and error checks.                                       */
-/*===========================================================================*/
-
-#if !(LIS3DSH_USE_SPI ^ LIS3DSH_USE_I2C)
-#error "LIS3DSH_USE_SPI and LIS3DSH_USE_I2C cannot be both true or both false"
-#endif
-
-#if LIS3DSH_USE_SPI && !HAL_USE_SPI
-#error "LIS3DSH_USE_SPI requires HAL_USE_SPI"
-#endif
-
-#if LIS3DSH_USE_I2C && !HAL_USE_I2C
-#error "LIS3DSH_USE_I2C requires HAL_USE_I2C"
-#endif
-
-#if LIS3DSH_SHARED_SPI && !SPI_USE_MUTUAL_EXCLUSION
-#error "LIS3DSH_SHARED_SPI requires SPI_USE_MUTUAL_EXCLUSION"
-#endif
-
-/*===========================================================================*/
-/* Driver data structures and types.                                         */
-/*===========================================================================*/
-
-/**
- * @name    LIS3DSH data structures and types
- * @{
- */
-/**
- * @brief   LIS3DSH full scale.
- */
-typedef enum {
-  LIS3DSH_FS_2G = 0x00,             /**< Full scale ±2g.                    */
-  LIS3DSH_FS_4G = 0x08,             /**< Full scale ±4g.                    */
-  LIS3DSH_FS_6G = 0x10,             /**< Full scale ±6g.                    */
-  LIS3DSH_FS_8G = 0x18,             /**< Full scale ±8g.                    */
-  LIS3DSH_FS_16G = 0x20             /**< Full scale ±16g.                   */
-}lis3dsh_fs_t;
-
-/**
- * @brief   LIS3DSH output data rate.
- */
-typedef enum {
-  LIS3DSH_ODR_PD = 0x00,            /**< ODR 100 Hz.                        */
-  LIS3DSH_ODR_3_125HZ = 0x10,       /**< ODR 3.125 Hz.                      */
-  LIS3DSH_ODR_6_25HZ = 0x20,        /**< ODR 6.25 Hz.                       */
-  LIS3DSH_ODR_12_5HZ = 0x30,        /**< ODR 12.5 Hz.                       */
-  LIS3DSH_ODR_25HZ = 0x40,          /**< ODR 25 Hz.                         */
-  LIS3DSH_ODR_50HZ = 0x50,          /**< ODR 50 Hz.                         */
-  LIS3DSH_ODR_100HZ = 0x60,         /**< ODR 100 Hz.                        */
-  LIS3DSH_ODR_400HZ = 0x70,         /**< ODR 400 Hz.                        */
-  LIS3DSH_ODR_800HZ = 0x80,         /**< ODR 800 Hz.                        */
-  LIS3DSH_ODR_1600HZ = 0x90         /**< ODR 1600 Hz.                       */
-}lis3dsh_odr_t;
-
-/**
- * @brief   LIS3DSH anti-aliasing bandwidth.
- */
-typedef enum {
-  LIS3DSH_BW_800HZ = 0x00,          /**< AA filter BW 800Hz.                */
-  LIS3DSH_BW_200HZ = 0x40,          /**< AA filter BW 200Hz.                */
-  LIS3DSH_BW_400HZ = 0x80,          /**< AA filter BW 400Hz.                */
-  LIS3DSH_BW_50HZ = 0xC0            /**< AA filter BW 50Hz.                 */
-}lis3dsh_bw_t;
-
-/**
- * @brief   LIS3DSH axes enabling.
- */
-typedef enum {
-  LIS3DSH_AE_DISABLED = 0x00,       /**< All axes disabled.                 */
-  LIS3DSH_AE_X = 0x01,              /**< Only X-axis enabled.               */
-  LIS3DSH_AE_Y = 0x02,              /**< Only Y-axis enabled.               */
-  LIS3DSH_AE_XY = 0x03,             /**< X and Y axes enabled.              */
-  LIS3DSH_AE_Z = 0x04,              /**< Only Z-axis enabled.               */
-  LIS3DSH_AE_XZ = 0x05,             /**< X and Z axes enabled.              */
-  LIS3DSH_AE_YZ = 0x06,             /**< Y and Z axes enabled.              */
-  LIS3DSH_AE_XYZ = 0x07             /**< All axes enabled.                  */
-}lis3dsh_ae_t;
-
-/**
- * @brief   LIS3DSH block data update.
- */
-typedef enum {
-  LIS3DSH_BDU_CONTINUOUS = 0x00,    /**< Block data continuously updated.   */
-  LIS3DSH_BDU_BLOCKED = 0x80        /**< Block data updated after reading.  */
-} lis3dsh_bdu_t;
-
-/**
- * @brief   LIS3DSH accelerometer subsystem unit.
- */
-typedef enum {
-  LIS3DSH_ACC_UNIT_G = 0x00,        /**< Cooked data in g.                  */
-  LIS3DSH_ACC_UNIT_MG = 0x01,       /**< Cooked data in mg.                 */
-  LIS3DSH_ACC_UNIT_SI = 0x02,       /**< Cooked data in m/s^2.              */
-} lis3dsh_acc_unit_t;
-
-/**
- * @brief   Driver state machine possible states.
- */
-typedef enum {
-  LIS3DSH_UNINIT = 0,               /**< Not initialized.                   */
-  LIS3DSH_STOP = 1,                 /**< Stopped.                           */
-  LIS3DSH_READY = 2,                /**< Ready.                             */
-} lis3dsh_state_t;
-
-/**
- * @brief   LIS3DSH configuration structure.
- */
-typedef struct {
-
-#if (LIS3DSH_USE_SPI) || defined(__DOXYGEN__)
-  /**
-   * @brief SPI driver associated to this LIS3DSH.
-   */
-  SPIDriver                 *spip;
-  /**
-   * @brief SPI configuration associated to this LIS3DSH.
-   */
-  const SPIConfig           *spicfg;
-#endif /* LIS3DSH_USE_SPI */
-#if (LIS3DSH_USE_I2C) || defined(__DOXYGEN__)
-  /**
-   * @brief I2C driver associated to this LIS3DSH.
-   */
-  I2CDriver                 *i2cp;
-  /**
-   * @brief I2C configuration associated to this LIS3DSH.
-   */
-  const I2CConfig           *i2ccfg;
-#endif /* LIS3DSH_USE_I2C */
-  /**
-   * @brief LIS3DSH full scale value.
-   */
-  lis3dsh_fs_t               fullscale;
-  /**
-   * @brief LIS3DSH output data rate selection.
-   */
-  lis3dsh_odr_t              outputdatarate;
-  /**
-   * @brief   LIS3DSH anti-aliasing bandwidth.
-   */
-  lis3dsh_bw_t               antialiasing;
-  /**
-   * @brief LIS3DSH axes enabling.
-   */
-  lis3dsh_ae_t               axesenabling;
-  /**
-   * @brief   LIS3DSH block data update.
-   */  
-  lis3dsh_bdu_t              blockdataupdate;
-  /**
-   * @brief LIS3DSH unit for cooked data.
-   */
-  lis3dsh_acc_unit_t         unit;
-} LIS3DSHConfig;
-
-/**
- * @brief   Structure representing a LIS3DSH driver.
- */
-typedef struct LIS3DSHDriver LIS3DSHDriver;
-
-/**
- * @brief   @p LIS3DSH specific methods.
- */
-#define _lis3dsh_methods                                                    \
-  _base_accelerometer_methods                                               \
-  /* Retrieve the temperature of L3GD20 chip.*/                             \
-  msg_t (*get_temperature)(void *instance, int8_t* temperature);
-
-/**
- * @extends BaseAccelerometerVMT
- *
- * @brief   @p LIS3DSH virtual methods table.
- */
-struct LIS3DSHVMT {
-  _lis3dsh_methods
-};
-
-/**
- * @brief   @p LIS3DSHDriver specific data.
- */
-#define _lis3dsh_data                                                       \
-  _base_accelerometer_data                                                  \
-  /* Driver state.*/                                                        \
-  lis3dsh_state_t            state;                                         \
-  /* Current configuration data.*/                                          \
-  const LIS3DSHConfig        *config;                                       \
-  /* Current sensitivity.*/                                                 \
-  float                     sensitivity[LIS3DSH_NUMBER_OF_AXES];            \
-  /* Bias data.*/                                                           \
-  int32_t                   bias[LIS3DSH_NUMBER_OF_AXES];
-
-/**
- * @extends BaseAccelerometer
- *
- * @brief   LIS3DSH 3-axis accelerometer class.
- * @details This class extends @p BaseAccelerometer by adding physical
- *          driver implementation.
- */
-struct LIS3DSHDriver {
-  /** @brief BaseSensor Virtual Methods Table. */
-  const struct BaseSensorVMT *vmt_basesensor;
-  /** @brief BaseAccelerometer Virtual Methods Table. */
-  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;
-  /** @brief LIS3DSH Virtual Methods Table. */
-  const struct LIS3DSHVMT *vmt_lis3dsh;
-  _lis3dsh_data
-};
-/** @} */
-
-/*===========================================================================*/
-/* Driver macros.                                                            */
-/*===========================================================================*/
-
-/**
- * @brief   Get current MEMS temperature.
- * @detail  This information is very useful especially for high accuracy IMU
- * @note    Value is raw since there is a lack of information in datasheet.
- *
- * @param[in] ip        pointer to a @p BaseAccelerometer class.
- * @param[out] temp     the MEMS temperature as raw data.
- *
- * @return              The operation status.
- * @retval MSG_OK       if the function succeeded.
- * @retval MSG_RESET    if one or more errors occurred.
- * @api
- */
-#define accelerometerGetTemp(ip, tpp)                                       \
-        (ip)->vmt_lis3dsh->get_temperature(ip, tpp)
-
-/*===========================================================================*/
-/* External declarations.                                                    */
-/*===========================================================================*/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-  void lis3dshObjectInit(LIS3DSHDriver *devp);
-  void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config);
-  void lis3dshStop(LIS3DSHDriver *devp);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _LIS3DSH_H_ */
-
-/** @} */
-
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lis3dsh.h

+ * @brief   LIS3DSH MEMS interface module header.

+ *

+ * @{

+ */

+

+#ifndef _LIS3DSH_H_

+#define _LIS3DSH_H_

+

+#include "hal_accelerometer.h"

+

+/*===========================================================================*/

+/* Driver constants.                                                         */

+/*===========================================================================*/

+

+/**

+ * @brief   LIS3DSH number of axes.

+ */

+#define  LIS3DSH_NUMBER_OF_AXES             ((size_t)   3U)

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    Configuration options.

+ * @{

+ */

+/**

+ * @brief   LIS3DSH SPI interface switch.

+ * @details If set to @p TRUE the support for SPI is included.

+ * @note    The default is @p TRUE.

+ */

+#if !defined(LIS3DSH_USE_SPI) || defined(__DOXYGEN__)

+#define LIS3DSH_USE_SPI                     TRUE

+#endif

+

+/**

+ * @brief   LIS3DSH I2C interface switch.

+ * @details If set to @p TRUE the support for I2C is included.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(LIS3DSH_USE_I2C) || defined(__DOXYGEN__)

+#define LIS3DSH_USE_I2C                     FALSE

+#endif

+

+/**

+ * @brief   LIS3DSH shared SPI switch.

+ * @details If set to @p TRUE the device acquires SPI bus ownership

+ *          on each transaction.

+ * @note    The default is @p FALSE. Requires SPI_USE_MUTUAL_EXCLUSION.

+ */

+#if !defined(LIS3DSH_SHARED_SPI) || defined(__DOXYGEN__)

+#define LIS3DSH_SHARED_SPI                  FALSE

+#endif

+/** @} */

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+#if !(LIS3DSH_USE_SPI ^ LIS3DSH_USE_I2C)

+#error "LIS3DSH_USE_SPI and LIS3DSH_USE_I2C cannot be both true or both false"

+#endif

+

+#if LIS3DSH_USE_SPI && !HAL_USE_SPI

+#error "LIS3DSH_USE_SPI requires HAL_USE_SPI"

+#endif

+

+#if LIS3DSH_USE_I2C && !HAL_USE_I2C

+#error "LIS3DSH_USE_I2C requires HAL_USE_I2C"

+#endif

+

+#if LIS3DSH_SHARED_SPI && !SPI_USE_MUTUAL_EXCLUSION

+#error "LIS3DSH_SHARED_SPI requires SPI_USE_MUTUAL_EXCLUSION"

+#endif

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    LIS3DSH data structures and types

+ * @{

+ */

+/**

+ * @brief   LIS3DSH full scale.

+ */

+typedef enum {

+  LIS3DSH_FS_2G = 0x00,             /**< Full scale ±2g.                    */

+  LIS3DSH_FS_4G = 0x08,             /**< Full scale ±4g.                    */

+  LIS3DSH_FS_6G = 0x10,             /**< Full scale ±6g.                    */

+  LIS3DSH_FS_8G = 0x18,             /**< Full scale ±8g.                    */

+  LIS3DSH_FS_16G = 0x20             /**< Full scale ±16g.                   */

+}lis3dsh_fs_t;

+

+/**

+ * @brief   LIS3DSH output data rate.

+ */

+typedef enum {

+  LIS3DSH_ODR_PD = 0x00,            /**< ODR 100 Hz.                        */

+  LIS3DSH_ODR_3_125HZ = 0x10,       /**< ODR 3.125 Hz.                      */

+  LIS3DSH_ODR_6_25HZ = 0x20,        /**< ODR 6.25 Hz.                       */

+  LIS3DSH_ODR_12_5HZ = 0x30,        /**< ODR 12.5 Hz.                       */

+  LIS3DSH_ODR_25HZ = 0x40,          /**< ODR 25 Hz.                         */

+  LIS3DSH_ODR_50HZ = 0x50,          /**< ODR 50 Hz.                         */

+  LIS3DSH_ODR_100HZ = 0x60,         /**< ODR 100 Hz.                        */

+  LIS3DSH_ODR_400HZ = 0x70,         /**< ODR 400 Hz.                        */

+  LIS3DSH_ODR_800HZ = 0x80,         /**< ODR 800 Hz.                        */

+  LIS3DSH_ODR_1600HZ = 0x90         /**< ODR 1600 Hz.                       */

+}lis3dsh_odr_t;

+

+/**

+ * @brief   LIS3DSH anti-aliasing bandwidth.

+ */

+typedef enum {

+  LIS3DSH_BW_800HZ = 0x00,          /**< AA filter BW 800Hz.                */

+  LIS3DSH_BW_200HZ = 0x40,          /**< AA filter BW 200Hz.                */

+  LIS3DSH_BW_400HZ = 0x80,          /**< AA filter BW 400Hz.                */

+  LIS3DSH_BW_50HZ = 0xC0            /**< AA filter BW 50Hz.                 */

+}lis3dsh_bw_t;

+

+/**

+ * @brief   LIS3DSH axes enabling.

+ */

+typedef enum {

+  LIS3DSH_AE_DISABLED = 0x00,       /**< All axes disabled.                 */

+  LIS3DSH_AE_X = 0x01,              /**< Only X-axis enabled.               */

+  LIS3DSH_AE_Y = 0x02,              /**< Only Y-axis enabled.               */

+  LIS3DSH_AE_XY = 0x03,             /**< X and Y axes enabled.              */

+  LIS3DSH_AE_Z = 0x04,              /**< Only Z-axis enabled.               */

+  LIS3DSH_AE_XZ = 0x05,             /**< X and Z axes enabled.              */

+  LIS3DSH_AE_YZ = 0x06,             /**< Y and Z axes enabled.              */

+  LIS3DSH_AE_XYZ = 0x07             /**< All axes enabled.                  */

+}lis3dsh_ae_t;

+

+/**

+ * @brief   LIS3DSH block data update.

+ */

+typedef enum {

+  LIS3DSH_BDU_CONTINUOUS = 0x00,    /**< Block data continuously updated.   */

+  LIS3DSH_BDU_BLOCKED = 0x80        /**< Block data updated after reading.  */

+} lis3dsh_bdu_t;

+

+/**

+ * @brief   LIS3DSH accelerometer subsystem unit.

+ */

+typedef enum {

+  LIS3DSH_ACC_UNIT_G = 0x00,        /**< Cooked data in g.                  */

+  LIS3DSH_ACC_UNIT_MG = 0x01,       /**< Cooked data in mg.                 */

+  LIS3DSH_ACC_UNIT_SI = 0x02,       /**< Cooked data in m/s^2.              */

+} lis3dsh_acc_unit_t;

+

+/**

+ * @brief   Driver state machine possible states.

+ */

+typedef enum {

+  LIS3DSH_UNINIT = 0,               /**< Not initialized.                   */

+  LIS3DSH_STOP = 1,                 /**< Stopped.                           */

+  LIS3DSH_READY = 2,                /**< Ready.                             */

+} lis3dsh_state_t;

+

+/**

+ * @brief   LIS3DSH configuration structure.

+ */

+typedef struct {

+

+#if (LIS3DSH_USE_SPI) || defined(__DOXYGEN__)

+  /**

+   * @brief SPI driver associated to this LIS3DSH.

+   */

+  SPIDriver                 *spip;

+  /**

+   * @brief SPI configuration associated to this LIS3DSH.

+   */

+  const SPIConfig           *spicfg;

+#endif /* LIS3DSH_USE_SPI */

+#if (LIS3DSH_USE_I2C) || defined(__DOXYGEN__)

+  /**

+   * @brief I2C driver associated to this LIS3DSH.

+   */

+  I2CDriver                 *i2cp;

+  /**

+   * @brief I2C configuration associated to this LIS3DSH.

+   */

+  const I2CConfig           *i2ccfg;

+#endif /* LIS3DSH_USE_I2C */

+  /**

+   * @brief LIS3DSH full scale value.

+   */

+  lis3dsh_fs_t               fullscale;

+  /**

+   * @brief LIS3DSH output data rate selection.

+   */

+  lis3dsh_odr_t              outputdatarate;

+  /**

+   * @brief   LIS3DSH anti-aliasing bandwidth.

+   */

+  lis3dsh_bw_t               antialiasing;

+  /**

+   * @brief LIS3DSH axes enabling.

+   */

+  lis3dsh_ae_t               axesenabling;

+  /**

+   * @brief   LIS3DSH block data update.

+   */  

+  lis3dsh_bdu_t              blockdataupdate;

+  /**

+   * @brief LIS3DSH unit for cooked data.

+   */

+  lis3dsh_acc_unit_t         unit;

+} LIS3DSHConfig;

+

+/**

+ * @brief   Structure representing a LIS3DSH driver.

+ */

+typedef struct LIS3DSHDriver LIS3DSHDriver;

+

+/**

+ * @brief   @p LIS3DSH specific methods.

+ */

+#define _lis3dsh_methods                                                    \

+  _base_accelerometer_methods                                               \

+  /* Retrieve the temperature of L3GD20 chip.*/                             \

+  msg_t (*get_temperature)(void *instance, int8_t* temperature);

+

+/**

+ * @extends BaseAccelerometerVMT

+ *

+ * @brief   @p LIS3DSH virtual methods table.

+ */

+struct LIS3DSHVMT {

+  _lis3dsh_methods

+};

+

+/**

+ * @brief   @p LIS3DSHDriver specific data.

+ */

+#define _lis3dsh_data                                                       \

+  _base_accelerometer_data                                                  \

+  /* Driver state.*/                                                        \

+  lis3dsh_state_t            state;                                         \

+  /* Current configuration data.*/                                          \

+  const LIS3DSHConfig        *config;                                       \

+  /* Current sensitivity.*/                                                 \

+  float                     sensitivity[LIS3DSH_NUMBER_OF_AXES];            \

+  /* Bias data.*/                                                           \

+  int32_t                   bias[LIS3DSH_NUMBER_OF_AXES];

+

+/**

+ * @extends BaseAccelerometer

+ *

+ * @brief   LIS3DSH 3-axis accelerometer class.

+ * @details This class extends @p BaseAccelerometer by adding physical

+ *          driver implementation.

+ */

+struct LIS3DSHDriver {

+  /** @brief BaseSensor Virtual Methods Table. */

+  const struct BaseSensorVMT *vmt_basesensor;

+  /** @brief BaseAccelerometer Virtual Methods Table. */

+  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;

+  /** @brief LIS3DSH Virtual Methods Table. */

+  const struct LIS3DSHVMT *vmt_lis3dsh;

+  _lis3dsh_data

+};

+/** @} */

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+/**

+ * @brief   Get current MEMS temperature.

+ * @detail  This information is very useful especially for high accuracy IMU

+ * @note    Value is raw since there is a lack of information in datasheet.

+ *

+ * @param[in] ip        pointer to a @p BaseAccelerometer class.

+ * @param[out] temp     the MEMS temperature as raw data.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ * @api

+ */

+#define accelerometerGetTemp(ip, tpp)                                       \

+        (ip)->vmt_lis3dsh->get_temperature(ip, tpp)

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void lis3dshObjectInit(LIS3DSHDriver *devp);

+  void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config);

+  void lis3dshStop(LIS3DSHDriver *devp);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* _LIS3DSH_H_ */

+

+/** @} */

+

diff --git a/os/ex/ST/lis3mdl.c b/os/ex/ST/lis3mdl.c
index 7abcd41e2..659cc29e0 100644
--- a/os/ex/ST/lis3mdl.c
+++ b/os/ex/ST/lis3mdl.c
@@ -1,483 +1,483 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lis3mdl.c
- * @brief   LIS3MDL MEMS interface module code.
- *
- * @addtogroup lis3mdl
- * @{
- */
-
-#include "hal.h"
-#include "lis3mdl.h"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-#define  LIS3MDL_SENS_4GA                    ((float)6842.0f)
-#define  LIS3MDL_SENS_8GA                    ((float)3421.0f)
-#define  LIS3MDL_SENS_12GA                   ((float)2281.0f)
-#define  LIS3MDL_SENS_16GA                   ((float)1711.0f)
-
-#define  LIS3MDL_TEMP_SENS                   ((float)8.0f)
-
-#define  LIS3MDL_DI                          ((uint8_t)0xFF)
-#define  LIS3MDL_DI_0                        ((uint8_t)0x01)
-#define  LIS3MDL_DI_1                        ((uint8_t)0x02)
-#define  LIS3MDL_DI_2                        ((uint8_t)0x04)
-#define  LIS3MDL_DI_3                        ((uint8_t)0x08)
-#define  LIS3MDL_DI_4                        ((uint8_t)0x10)
-#define  LIS3MDL_DI_5                        ((uint8_t)0x20)
-#define  LIS3MDL_DI_6                        ((uint8_t)0x40)
-#define  LIS3MDL_DI_7                        ((uint8_t)0x80)
-
-#define  LIS3MDL_AD                          ((uint8_t)0x3F)
-#define  LIS3MDL_AD_0                        ((uint8_t)0x01)
-#define  LIS3MDL_AD_1                        ((uint8_t)0x02)
-#define  LIS3MDL_AD_2                        ((uint8_t)0x04)
-#define  LIS3MDL_AD_3                        ((uint8_t)0x08)
-#define  LIS3MDL_AD_4                        ((uint8_t)0x10)
-#define  LIS3MDL_AD_5                        ((uint8_t)0x20)
-#define  LIS3MDL_AD_6                        ((uint8_t)0x40)
-#define  LIS3MDL_RW                          ((uint8_t)0x80)
-
-#define  LIS3MDL_AD_WHO_AM_I                 ((uint8_t)0x0F)
-#define  LIS3MDL_AD_CTRL_REG1                ((uint8_t)0x20)
-#define  LIS3MDL_AD_CTRL_REG2                ((uint8_t)0x21)
-#define  LIS3MDL_AD_CTRL_REG3                ((uint8_t)0x22)
-#define  LIS3MDL_AD_CTRL_REG4                ((uint8_t)0x23)
-#define  LIS3MDL_AD_CTRL_REG5                ((uint8_t)0x24)
-#define  LIS3MDL_AD_STATUS_REG               ((uint8_t)0x27)
-#define  LIS3MDL_AD_OUT_X_L                  ((uint8_t)0x28)
-#define  LIS3MDL_AD_OUT_X_H                  ((uint8_t)0x29)
-#define  LIS3MDL_AD_OUT_Y_L                  ((uint8_t)0x2A)
-#define  LIS3MDL_AD_OUT_Y_H                  ((uint8_t)0x2B)
-#define  LIS3MDL_AD_OUT_Z_L                  ((uint8_t)0x2C)
-#define  LIS3MDL_AD_OUT_Z_H                  ((uint8_t)0x2D)
-#define  LIS3MDL_AD_TEMP_OUT_L               ((uint8_t)0x2E)
-#define  LIS3MDL_AD_TEMP_OUT_H               ((uint8_t)0x2F)
-#define  LIS3MDL_AD_INT_CFG                  ((uint8_t)0x30)
-#define  LIS3MDL_AD_INT_SOURCE               ((uint8_t)0x31)
-#define  LIS3MDL_AD_INT_THS_L                ((uint8_t)0x32)
-#define  LIS3MDL_AD_INT_THS_H                ((uint8_t)0x33)
-
-#define  LIS3MDL_CTRL_REG2_FS_MASK           ((uint8_t)0x60)
-
-/*===========================================================================*/
-/* Driver exported variables.                                                */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local variables and types.                                         */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local functions.                                                   */
-/*===========================================================================*/
-
-#if (LIS3MDL_USE_I2C) || defined(__DOXYGEN__)
-/**
- * @brief   Reads registers value using I2C.
- * @pre     The I2C interface must be initialized and the driver started.
- *
- * @param[in] i2cp       pointer to the I2C interface
- * @param[in] sad        slave address without R bit
- * @param[in] reg        first sub-register address
- * @return               the read value.
- */
-uint8_t lis3mdlI2CReadRegister(I2CDriver *i2cp, lis3mdl_sad_t sad, uint8_t reg,
-                               msg_t* msgp) {
-  msg_t msg;
-#if defined(STM32F103_MCUCONF)
-  uint8_t rxbuf[2];
-  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,
-                                   TIME_INFINITE);
-  if(msgp != NULL){
-    *msgp = msg;
-  }
-  return rxbuf[0];
-#else
-  uint8_t txbuf, rxbuf;
-  txbuf = reg;
-  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, &rxbuf, 1,
-                                        TIME_INFINITE);
-  if(msgp != NULL){
-    *msgp = msg;
-  }
-  return rxbuf;
-#endif
-}
-
-/**
- * @brief   Writes a value into a register using I2C.
- * @pre     The I2C interface must be initialized and the driver started.
- *
- * @param[in] i2cp       pointer to the I2C interface
- * @param[in] sad        slave address without R bit
- * @param[in] sub        sub-register address
- * @param[in] value      the value to be written
- * @return               the operation status.
- */
-msg_t lis3mdlI2CWriteRegister(I2CDriver *i2cp, lis3mdl_sad_t sad, uint8_t reg,
-                              uint8_t value) {
-  uint8_t rxbuf;
-  uint8_t txbuf[2];
-  switch (reg) {
-  default:
-    /* Reserved register must not be written, according to the datasheet
-     * this could permanently damage the device.
-     */
-    chDbgAssert(FALSE, "lis3mdlI2CWriteRegister(), reserved register");
-  case LIS3MDL_AD_WHO_AM_I:
-  case LIS3MDL_AD_STATUS_REG:
-  case LIS3MDL_AD_OUT_X_L:
-  case LIS3MDL_AD_OUT_X_H:
-  case LIS3MDL_AD_OUT_Y_L:
-  case LIS3MDL_AD_OUT_Y_H:
-  case LIS3MDL_AD_OUT_Z_L:
-  case LIS3MDL_AD_OUT_Z_H:
-  case LIS3MDL_AD_TEMP_OUT_L:
-  case LIS3MDL_AD_TEMP_OUT_H:
-  case LIS3MDL_AD_INT_SOURCE:
-  case LIS3MDL_AD_INT_THS_L:
-  case LIS3MDL_AD_INT_THS_H:
-  /* Read only registers cannot be written, the command is ignored.*/
-    return MSG_RESET;
-  case LIS3MDL_AD_CTRL_REG1:
-  case LIS3MDL_AD_CTRL_REG2:
-  case LIS3MDL_AD_CTRL_REG3:
-  case LIS3MDL_AD_CTRL_REG4:
-  case LIS3MDL_AD_CTRL_REG5:
-  case LIS3MDL_AD_INT_CFG:
-    txbuf[0] = reg;
-    txbuf[1] = value;
-    return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
-    break;
-  }
-}
-#endif /* LIS3MDL_USE_I2C */
-
-/*
- * Interface implementation.
- */
-static size_t get_axes_number(void *ip) {
-
-  osalDbgCheck(ip != NULL);
-  return LIS3MDL_NUMBER_OF_AXES;
-}
-
-static msg_t read_raw(void *ip, int32_t axes[LIS3MDL_NUMBER_OF_AXES]) {
-  uint16_t tmp;
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),
-              "read_raw(), invalid state");
-
-#if LIS3MDL_USE_I2C
-  osalDbgAssert((((LIS3MDLDriver *)ip)->config->i2cp->state == I2C_READY),
-                "read_raw(), channel not ready");
-#if LIS3MDL_SHARED_I2C
-  i2cAcquireBus(((LIS3MDLDriver *)ip)->config->i2cp);
-  i2cStart(((LIS3MDLDriver *)ip)->config->i2cp,
-           ((LIS3MDLDriver *)ip)->config->i2ccfg);
-#endif /* LIS3MDL_SHARED_I2C */
-
-  tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
-                               ((LIS3MDLDriver *)ip)->config->slaveaddress,
-                                 LIS3MDL_AD_OUT_X_L, NULL);
-  tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
-                                ((LIS3MDLDriver *)ip)->config->slaveaddress,
-                                  LIS3MDL_AD_OUT_X_H, NULL) << 8;
-  axes[0] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[0];
-
-  tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
-                               ((LIS3MDLDriver *)ip)->config->slaveaddress,
-                                 LIS3MDL_AD_OUT_Y_L, NULL);
-  tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
-                                ((LIS3MDLDriver *)ip)->config->slaveaddress,
-                                  LIS3MDL_AD_OUT_Y_H, NULL) << 8;
-  axes[1] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[1];
-
-  tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
-                               ((LIS3MDLDriver *)ip)->config->slaveaddress,
-                                 LIS3MDL_AD_OUT_Z_L, NULL);
-  tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
-                                ((LIS3MDLDriver *)ip)->config->slaveaddress,
-                                  LIS3MDL_AD_OUT_Z_H, NULL) << 8;
-  axes[2] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[2];
-
-#if LIS3MDL_SHARED_I2C
-  i2cReleaseBus(((LIS3MDLDriver *)ip)->config->i2cp);
-#endif /* LIS3MDL_SHARED_I2C */
-#endif /* LIS3MDL_USE_I2C */
-  return MSG_OK;
-}
-
-static msg_t read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[LIS3MDL_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-
-  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),
-              "read_cooked(), invalid state");
-
-  msg = read_raw(ip, raw);
-  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] / ((LIS3MDLDriver *)ip)->sensitivity[i];
-  }
-  return msg;
-}
-
-static msg_t set_bias(void *ip, int32_t *bp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (bp !=NULL));
-
-  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY) ||
-                (((LIS3MDLDriver *)ip)->state == LIS3MDL_STOP),
-                "set_bias(), invalid state");
-  
-  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++) {
-    ((LIS3MDLDriver *)ip)->bias[i] = bp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t reset_bias(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY) ||
-                (((LIS3MDLDriver *)ip)->state == LIS3MDL_STOP),
-              "reset_bias(), invalid state");
-
-  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-    ((LIS3MDLDriver *)ip)->bias[i] = 0;
-  return MSG_OK;
-}
-
-static msg_t set_sensivity(void *ip, float *sp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (sp !=NULL));
-
-  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),
-                "set_sensivity(), invalid state");
-  
-  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++) {
-    ((LIS3MDLDriver *)ip)->sensitivity[i] = sp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t reset_sensivity(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),
-                "reset_sensivity(), invalid state");
-
-  if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_4GA)
-    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-      ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_4GA;
-  else if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_8GA)
-    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-      ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_8GA;
-  else if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_12GA)
-    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-      ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_12GA;
-  else if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_16GA)
-    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-      ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_16GA;
-  else {
-    osalDbgAssert(FALSE, "reset_sensivity(), compass full scale issue");
-    return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
-static msg_t get_temperature(void *ip, float* tempp) {
-  int16_t temp;
-#if LIS3MDL_USE_I2C
-  osalDbgAssert((((LIS3MDLDriver *)ip)->config->i2cp->state == I2C_READY),
-                "gyro_read_raw(), channel not ready");
-#if LIS3MDL_SHARED_I2C
-  i2cAcquireBus(((LIS3MDLDriver *)ip)->config->i2cp);
-  i2cStart(((LIS3MDLDriver *)ip)->config->i2cp,
-           ((LIS3MDLDriver *)ip)->config->i2ccfg);
-#endif /* LIS3MDL_SHARED_I2C */
-  temp = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
-                                          ((LIS3MDLDriver *)ip)->config->slaveaddress,
-                                            LIS3MDL_AD_TEMP_OUT_L, NULL));
-  temp += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,
-                                           ((LIS3MDLDriver *)ip)->config->slaveaddress,
-                                             LIS3MDL_AD_TEMP_OUT_H, NULL) << 8);
-#if LIS3MDL_SHARED_I2C
-  i2cReleaseBus(((LIS3MDLDriver *)ip)->config->i2cp);
-#endif /* LIS3MDL_SHARED_I2C */
-#endif /* LIS3MDL_USE_I2C */
-  *tempp = (float)temp / LIS3MDL_TEMP_SENS;
-  return MSG_OK;
-}
-
-static const struct BaseSensorVMT vmt_basesensor = {
-  get_axes_number, read_raw, read_cooked
-};
-
-static const struct BaseCompassVMT vmt_basecompass = {
-  get_axes_number, read_raw, read_cooked,
-  set_bias, reset_bias, set_sensivity, reset_sensivity
-};
-
-static const struct LIS3MDLVMT vmt_lis3mdl = {
-  get_axes_number, read_raw, read_cooked,
-  set_bias, reset_bias, set_sensivity, reset_sensivity,
-  get_temperature
-};
-
-/*===========================================================================*/
-/* Driver exported functions.                                                */
-/*===========================================================================*/
-
-/**
- * @brief   Initializes an instance.
- *
- * @param[out] devp     pointer to the @p LIS3MDLDriver object
- *
- * @init
- */
-void lis3mdlObjectInit(LIS3MDLDriver *devp) {
-  uint32_t i;
-  devp->vmt_basesensor = &vmt_basesensor;
-  devp->vmt_basecompass = &vmt_basecompass;
-  devp->vmt_lis3mdl = &vmt_lis3mdl;
-  devp->config = NULL;
-  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-    devp->bias[i] = 0;
-  devp->state  = LIS3MDL_STOP;
-}
-
-/**
- * @brief   Configures and activates LIS3MDL Complex Driver peripheral.
- *
- * @param[in] devp      pointer to the @p LIS3MDLDriver object
- * @param[in] config    pointer to the @p LIS3MDLConfig object
- *
- * @api
- */
-void lis3mdlStart(LIS3MDLDriver *devp, const LIS3MDLConfig *config) {
-  uint32_t i;
-  osalDbgCheck((devp != NULL) && (config != NULL));
-
-  osalDbgAssert((devp->state == LIS3MDL_STOP) || (devp->state == LIS3MDL_READY),
-              "lis3mdlStart(), invalid state");
-
-  devp->config = config;
-
-#if LIS3MDL_USE_I2C
-#if LIS3MDL_SHARED_I2C
-  i2cAcquireBus((devp)->config->i2cp);
-#endif /* LIS3MDL_SHARED_I2C */
-  i2cStart((devp)->config->i2cp,
-           (devp)->config->i2ccfg);
-  lis3mdlI2CWriteRegister(devp->config->i2cp,
-                          devp->config->slaveaddress,
-                          LIS3MDL_AD_CTRL_REG1,
-                          devp->config->temperature |
-                          devp->config->outputdatarate |
-                          devp->config->operationmodexy);
-  lis3mdlI2CWriteRegister(devp->config->i2cp,
-                          devp->config->slaveaddress,
-                          LIS3MDL_AD_CTRL_REG2,
-                          devp->config->fullscale);
-  lis3mdlI2CWriteRegister(devp->config->i2cp,
-                          devp->config->slaveaddress,
-                          LIS3MDL_AD_CTRL_REG3,
-                          devp->config->conversionmode);
-  lis3mdlI2CWriteRegister(devp->config->i2cp,
-                          devp->config->slaveaddress,
-                          LIS3MDL_AD_CTRL_REG4,
-                          devp->config->operationmodez |
-                          devp->config->endianness);
-  lis3mdlI2CWriteRegister(devp->config->i2cp,
-                          devp->config->slaveaddress,
-                          LIS3MDL_AD_CTRL_REG5,
-                          devp->config->blockdataupdate);
-#if LIS3MDL_SHARED_I2C
-  i2cReleaseBus((devp)->config->i2cp);
-#endif /* LIS3MDL_SHARED_I2C */
-#endif /* LIS3MDL_USE_I2C */
-  /* Storing sensitivity information according to full scale value */
-  if(devp->config->fullscale == LIS3MDL_FS_4GA)
-    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3MDL_SENS_4GA;
-  else if(devp->config->fullscale == LIS3MDL_FS_8GA)
-    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3MDL_SENS_8GA;
-  else if(devp->config->fullscale == LIS3MDL_FS_12GA)
-    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3MDL_SENS_12GA;
-  else if(devp->config->fullscale == LIS3MDL_FS_16GA)
-    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = LIS3MDL_SENS_16GA;
-  else
-    osalDbgAssert(FALSE, "lis3mdlStart(), compass full scale issue");
-  devp->state = LIS3MDL_READY;
-} 
-
-/**
- * @brief   Deactivates the LIS3MDL Complex Driver peripheral.
- *
- * @param[in] devp       pointer to the @p LIS3MDLDriver object
- *
- * @api
- */
-void lis3mdlStop(LIS3MDLDriver *devp) {
-
-  osalDbgCheck(devp != NULL);
-
-  osalDbgAssert((devp->state == LIS3MDL_STOP) || (devp->state == LIS3MDL_READY),
-                "lis3mdlStop(), invalid state");
-
-#if (LIS3MDL_USE_I2C)
-  if (devp->state == LIS3MDL_STOP) {
-#if LIS3MDL_SHARED_I2C
-    i2cAcquireBus((devp)->config->i2cp);
-    i2cStart((devp)->config->i2cp,
-             (devp)->config->i2ccfg);
-#endif /* LIS3MDL_SHARED_I2C */
-    lis3mdlI2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LIS3MDL_AD_CTRL_REG3,
-                            LIS3MDL_MD_POWER_DOWN);
-    i2cStop((devp)->config->i2cp);
-#if LIS3MDL_SHARED_I2C
-    i2cReleaseBus((devp)->config->i2cp);
-#endif /* LIS3MDL_SHARED_I2C */
-  }
-#endif /* LIS3MDL_USE_I2C */
-  devp->state = LIS3MDL_STOP;
-}
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lis3mdl.c

+ * @brief   LIS3MDL MEMS interface module code.

+ *

+ * @addtogroup lis3mdl

+ * @{

+ */

+

+#include "hal.h"

+#include "lis3mdl.h"

+

+/*===========================================================================*/

+/* Driver local definitions.                                                 */

+/*===========================================================================*/

+

+#define  LIS3MDL_SENS_4GA                    ((float)6842.0f)

+#define  LIS3MDL_SENS_8GA                    ((float)3421.0f)

+#define  LIS3MDL_SENS_12GA                   ((float)2281.0f)

+#define  LIS3MDL_SENS_16GA                   ((float)1711.0f)

+

+#define  LIS3MDL_TEMP_SENS                   ((float)8.0f)

+

+#define  LIS3MDL_DI                          ((uint8_t)0xFF)

+#define  LIS3MDL_DI_0                        ((uint8_t)0x01)

+#define  LIS3MDL_DI_1                        ((uint8_t)0x02)

+#define  LIS3MDL_DI_2                        ((uint8_t)0x04)

+#define  LIS3MDL_DI_3                        ((uint8_t)0x08)

+#define  LIS3MDL_DI_4                        ((uint8_t)0x10)

+#define  LIS3MDL_DI_5                        ((uint8_t)0x20)

+#define  LIS3MDL_DI_6                        ((uint8_t)0x40)

+#define  LIS3MDL_DI_7                        ((uint8_t)0x80)

+

+#define  LIS3MDL_AD                          ((uint8_t)0x3F)

+#define  LIS3MDL_AD_0                        ((uint8_t)0x01)

+#define  LIS3MDL_AD_1                        ((uint8_t)0x02)

+#define  LIS3MDL_AD_2                        ((uint8_t)0x04)

+#define  LIS3MDL_AD_3                        ((uint8_t)0x08)

+#define  LIS3MDL_AD_4                        ((uint8_t)0x10)

+#define  LIS3MDL_AD_5                        ((uint8_t)0x20)

+#define  LIS3MDL_AD_6                        ((uint8_t)0x40)

+#define  LIS3MDL_RW                          ((uint8_t)0x80)

+

+#define  LIS3MDL_AD_WHO_AM_I                 ((uint8_t)0x0F)

+#define  LIS3MDL_AD_CTRL_REG1                ((uint8_t)0x20)

+#define  LIS3MDL_AD_CTRL_REG2                ((uint8_t)0x21)

+#define  LIS3MDL_AD_CTRL_REG3                ((uint8_t)0x22)

+#define  LIS3MDL_AD_CTRL_REG4                ((uint8_t)0x23)

+#define  LIS3MDL_AD_CTRL_REG5                ((uint8_t)0x24)

+#define  LIS3MDL_AD_STATUS_REG               ((uint8_t)0x27)

+#define  LIS3MDL_AD_OUT_X_L                  ((uint8_t)0x28)

+#define  LIS3MDL_AD_OUT_X_H                  ((uint8_t)0x29)

+#define  LIS3MDL_AD_OUT_Y_L                  ((uint8_t)0x2A)

+#define  LIS3MDL_AD_OUT_Y_H                  ((uint8_t)0x2B)

+#define  LIS3MDL_AD_OUT_Z_L                  ((uint8_t)0x2C)

+#define  LIS3MDL_AD_OUT_Z_H                  ((uint8_t)0x2D)

+#define  LIS3MDL_AD_TEMP_OUT_L               ((uint8_t)0x2E)

+#define  LIS3MDL_AD_TEMP_OUT_H               ((uint8_t)0x2F)

+#define  LIS3MDL_AD_INT_CFG                  ((uint8_t)0x30)

+#define  LIS3MDL_AD_INT_SOURCE               ((uint8_t)0x31)

+#define  LIS3MDL_AD_INT_THS_L                ((uint8_t)0x32)

+#define  LIS3MDL_AD_INT_THS_H                ((uint8_t)0x33)

+

+#define  LIS3MDL_CTRL_REG2_FS_MASK           ((uint8_t)0x60)

+

+/*===========================================================================*/

+/* Driver exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local variables and types.                                         */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local functions.                                                   */

+/*===========================================================================*/

+

+#if (LIS3MDL_USE_I2C) || defined(__DOXYGEN__)

+/**

+ * @brief   Reads registers value using I2C.

+ * @pre     The I2C interface must be initialized and the driver started.

+ *

+ * @param[in] i2cp       pointer to the I2C interface

+ * @param[in] sad        slave address without R bit

+ * @param[in] reg        first sub-register address

+ * @return               the read value.

+ */

+uint8_t lis3mdlI2CReadRegister(I2CDriver *i2cp, lis3mdl_sad_t sad, uint8_t reg,

+                               msg_t* msgp) {

+  msg_t msg;

+#if defined(STM32F103_MCUCONF)

+  uint8_t rxbuf[2];

+  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,

+                                   TIME_INFINITE);

+  if(msgp != NULL){

+    *msgp = msg;

+  }

+  return rxbuf[0];

+#else

+  uint8_t txbuf, rxbuf;

+  txbuf = reg;

+  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, &rxbuf, 1,

+                                        TIME_INFINITE);

+  if(msgp != NULL){

+    *msgp = msg;

+  }

+  return rxbuf;

+#endif

+}

+

+/**

+ * @brief   Writes a value into a register using I2C.

+ * @pre     The I2C interface must be initialized and the driver started.

+ *

+ * @param[in] i2cp       pointer to the I2C interface

+ * @param[in] sad        slave address without R bit

+ * @param[in] sub        sub-register address

+ * @param[in] value      the value to be written

+ * @return               the operation status.

+ */

+msg_t lis3mdlI2CWriteRegister(I2CDriver *i2cp, lis3mdl_sad_t sad, uint8_t reg,

+                              uint8_t value) {

+  uint8_t rxbuf;

+  uint8_t txbuf[2];

+  switch (reg) {

+  default:

+    /* Reserved register must not be written, according to the datasheet

+     * this could permanently damage the device.

+     */

+    chDbgAssert(FALSE, "lis3mdlI2CWriteRegister(), reserved register");

+  case LIS3MDL_AD_WHO_AM_I:

+  case LIS3MDL_AD_STATUS_REG:

+  case LIS3MDL_AD_OUT_X_L:

+  case LIS3MDL_AD_OUT_X_H:

+  case LIS3MDL_AD_OUT_Y_L:

+  case LIS3MDL_AD_OUT_Y_H:

+  case LIS3MDL_AD_OUT_Z_L:

+  case LIS3MDL_AD_OUT_Z_H:

+  case LIS3MDL_AD_TEMP_OUT_L:

+  case LIS3MDL_AD_TEMP_OUT_H:

+  case LIS3MDL_AD_INT_SOURCE:

+  case LIS3MDL_AD_INT_THS_L:

+  case LIS3MDL_AD_INT_THS_H:

+  /* Read only registers cannot be written, the command is ignored.*/

+    return MSG_RESET;

+  case LIS3MDL_AD_CTRL_REG1:

+  case LIS3MDL_AD_CTRL_REG2:

+  case LIS3MDL_AD_CTRL_REG3:

+  case LIS3MDL_AD_CTRL_REG4:

+  case LIS3MDL_AD_CTRL_REG5:

+  case LIS3MDL_AD_INT_CFG:

+    txbuf[0] = reg;

+    txbuf[1] = value;

+    return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);

+    break;

+  }

+}

+#endif /* LIS3MDL_USE_I2C */

+

+/*

+ * Interface implementation.

+ */

+static size_t get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return LIS3MDL_NUMBER_OF_AXES;

+}

+

+static msg_t read_raw(void *ip, int32_t axes[LIS3MDL_NUMBER_OF_AXES]) {

+  uint16_t tmp;

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),

+              "read_raw(), invalid state");

+

+#if LIS3MDL_USE_I2C

+  osalDbgAssert((((LIS3MDLDriver *)ip)->config->i2cp->state == I2C_READY),

+                "read_raw(), channel not ready");

+#if LIS3MDL_SHARED_I2C

+  i2cAcquireBus(((LIS3MDLDriver *)ip)->config->i2cp);

+  i2cStart(((LIS3MDLDriver *)ip)->config->i2cp,

+           ((LIS3MDLDriver *)ip)->config->i2ccfg);

+#endif /* LIS3MDL_SHARED_I2C */

+

+  tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,

+                               ((LIS3MDLDriver *)ip)->config->slaveaddress,

+                                 LIS3MDL_AD_OUT_X_L, NULL);

+  tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,

+                                ((LIS3MDLDriver *)ip)->config->slaveaddress,

+                                  LIS3MDL_AD_OUT_X_H, NULL) << 8;

+  axes[0] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[0];

+

+  tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,

+                               ((LIS3MDLDriver *)ip)->config->slaveaddress,

+                                 LIS3MDL_AD_OUT_Y_L, NULL);

+  tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,

+                                ((LIS3MDLDriver *)ip)->config->slaveaddress,

+                                  LIS3MDL_AD_OUT_Y_H, NULL) << 8;

+  axes[1] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[1];

+

+  tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,

+                               ((LIS3MDLDriver *)ip)->config->slaveaddress,

+                                 LIS3MDL_AD_OUT_Z_L, NULL);

+  tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,

+                                ((LIS3MDLDriver *)ip)->config->slaveaddress,

+                                  LIS3MDL_AD_OUT_Z_H, NULL) << 8;

+  axes[2] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[2];

+

+#if LIS3MDL_SHARED_I2C

+  i2cReleaseBus(((LIS3MDLDriver *)ip)->config->i2cp);

+#endif /* LIS3MDL_SHARED_I2C */

+#endif /* LIS3MDL_USE_I2C */

+  return MSG_OK;

+}

+

+static msg_t read_cooked(void *ip, float axes[]) {

+  uint32_t i;

+  int32_t raw[LIS3MDL_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck((ip != NULL) && (axes != NULL));

+

+  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),

+              "read_cooked(), invalid state");

+

+  msg = read_raw(ip, raw);

+  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES ; i++){

+    axes[i] = raw[i] / ((LIS3MDLDriver *)ip)->sensitivity[i];

+  }

+  return msg;

+}

+

+static msg_t set_bias(void *ip, int32_t *bp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (bp !=NULL));

+

+  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY) ||

+                (((LIS3MDLDriver *)ip)->state == LIS3MDL_STOP),

+                "set_bias(), invalid state");

+  

+  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++) {

+    ((LIS3MDLDriver *)ip)->bias[i] = bp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t reset_bias(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY) ||

+                (((LIS3MDLDriver *)ip)->state == LIS3MDL_STOP),

+              "reset_bias(), invalid state");

+

+  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+    ((LIS3MDLDriver *)ip)->bias[i] = 0;

+  return MSG_OK;

+}

+

+static msg_t set_sensivity(void *ip, float *sp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (sp !=NULL));

+

+  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),

+                "set_sensivity(), invalid state");

+  

+  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++) {

+    ((LIS3MDLDriver *)ip)->sensitivity[i] = sp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t reset_sensivity(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY),

+                "reset_sensivity(), invalid state");

+

+  if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_4GA)

+    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+      ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_4GA;

+  else if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_8GA)

+    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+      ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_8GA;

+  else if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_12GA)

+    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+      ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_12GA;

+  else if(((LIS3MDLDriver *)ip)->config->fullscale == LIS3MDL_FS_16GA)

+    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+      ((LIS3MDLDriver *)ip)->sensitivity[i] = LIS3MDL_SENS_16GA;

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), compass full scale issue");

+    return MSG_RESET;

+  }

+  return MSG_OK;

+}

+

+static msg_t get_temperature(void *ip, float* tempp) {

+  int16_t temp;

+#if LIS3MDL_USE_I2C

+  osalDbgAssert((((LIS3MDLDriver *)ip)->config->i2cp->state == I2C_READY),

+                "gyro_read_raw(), channel not ready");

+#if LIS3MDL_SHARED_I2C

+  i2cAcquireBus(((LIS3MDLDriver *)ip)->config->i2cp);

+  i2cStart(((LIS3MDLDriver *)ip)->config->i2cp,

+           ((LIS3MDLDriver *)ip)->config->i2ccfg);

+#endif /* LIS3MDL_SHARED_I2C */

+  temp = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,

+                                          ((LIS3MDLDriver *)ip)->config->slaveaddress,

+                                            LIS3MDL_AD_TEMP_OUT_L, NULL));

+  temp += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp,

+                                           ((LIS3MDLDriver *)ip)->config->slaveaddress,

+                                             LIS3MDL_AD_TEMP_OUT_H, NULL) << 8);

+#if LIS3MDL_SHARED_I2C

+  i2cReleaseBus(((LIS3MDLDriver *)ip)->config->i2cp);

+#endif /* LIS3MDL_SHARED_I2C */

+#endif /* LIS3MDL_USE_I2C */

+  *tempp = (float)temp / LIS3MDL_TEMP_SENS;

+  return MSG_OK;

+}

+

+static const struct BaseSensorVMT vmt_basesensor = {

+  get_axes_number, read_raw, read_cooked

+};

+

+static const struct BaseCompassVMT vmt_basecompass = {

+  get_axes_number, read_raw, read_cooked,

+  set_bias, reset_bias, set_sensivity, reset_sensivity

+};

+

+static const struct LIS3MDLVMT vmt_lis3mdl = {

+  get_axes_number, read_raw, read_cooked,

+  set_bias, reset_bias, set_sensivity, reset_sensivity,

+  get_temperature

+};

+

+/*===========================================================================*/

+/* Driver exported functions.                                                */

+/*===========================================================================*/

+

+/**

+ * @brief   Initializes an instance.

+ *

+ * @param[out] devp     pointer to the @p LIS3MDLDriver object

+ *

+ * @init

+ */

+void lis3mdlObjectInit(LIS3MDLDriver *devp) {

+  uint32_t i;

+  devp->vmt_basesensor = &vmt_basesensor;

+  devp->vmt_basecompass = &vmt_basecompass;

+  devp->vmt_lis3mdl = &vmt_lis3mdl;

+  devp->config = NULL;

+  for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+    devp->bias[i] = 0;

+  devp->state  = LIS3MDL_STOP;

+}

+

+/**

+ * @brief   Configures and activates LIS3MDL Complex Driver peripheral.

+ *

+ * @param[in] devp      pointer to the @p LIS3MDLDriver object

+ * @param[in] config    pointer to the @p LIS3MDLConfig object

+ *

+ * @api

+ */

+void lis3mdlStart(LIS3MDLDriver *devp, const LIS3MDLConfig *config) {

+  uint32_t i;

+  osalDbgCheck((devp != NULL) && (config != NULL));

+

+  osalDbgAssert((devp->state == LIS3MDL_STOP) || (devp->state == LIS3MDL_READY),

+              "lis3mdlStart(), invalid state");

+

+  devp->config = config;

+

+#if LIS3MDL_USE_I2C

+#if LIS3MDL_SHARED_I2C

+  i2cAcquireBus((devp)->config->i2cp);

+#endif /* LIS3MDL_SHARED_I2C */

+  i2cStart((devp)->config->i2cp,

+           (devp)->config->i2ccfg);

+  lis3mdlI2CWriteRegister(devp->config->i2cp,

+                          devp->config->slaveaddress,

+                          LIS3MDL_AD_CTRL_REG1,

+                          devp->config->temperature |

+                          devp->config->outputdatarate |

+                          devp->config->operationmodexy);

+  lis3mdlI2CWriteRegister(devp->config->i2cp,

+                          devp->config->slaveaddress,

+                          LIS3MDL_AD_CTRL_REG2,

+                          devp->config->fullscale);

+  lis3mdlI2CWriteRegister(devp->config->i2cp,

+                          devp->config->slaveaddress,

+                          LIS3MDL_AD_CTRL_REG3,

+                          devp->config->conversionmode);

+  lis3mdlI2CWriteRegister(devp->config->i2cp,

+                          devp->config->slaveaddress,

+                          LIS3MDL_AD_CTRL_REG4,

+                          devp->config->operationmodez |

+                          devp->config->endianness);

+  lis3mdlI2CWriteRegister(devp->config->i2cp,

+                          devp->config->slaveaddress,

+                          LIS3MDL_AD_CTRL_REG5,

+                          devp->config->blockdataupdate);

+#if LIS3MDL_SHARED_I2C

+  i2cReleaseBus((devp)->config->i2cp);

+#endif /* LIS3MDL_SHARED_I2C */

+#endif /* LIS3MDL_USE_I2C */

+  /* Storing sensitivity information according to full scale value */

+  if(devp->config->fullscale == LIS3MDL_FS_4GA)

+    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3MDL_SENS_4GA;

+  else if(devp->config->fullscale == LIS3MDL_FS_8GA)

+    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3MDL_SENS_8GA;

+  else if(devp->config->fullscale == LIS3MDL_FS_12GA)

+    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3MDL_SENS_12GA;

+  else if(devp->config->fullscale == LIS3MDL_FS_16GA)

+    for(i = 0; i < LIS3MDL_NUMBER_OF_AXES; i++)

+      devp->sensitivity[i] = LIS3MDL_SENS_16GA;

+  else

+    osalDbgAssert(FALSE, "lis3mdlStart(), compass full scale issue");

+  devp->state = LIS3MDL_READY;

+} 

+

+/**

+ * @brief   Deactivates the LIS3MDL Complex Driver peripheral.

+ *

+ * @param[in] devp       pointer to the @p LIS3MDLDriver object

+ *

+ * @api

+ */

+void lis3mdlStop(LIS3MDLDriver *devp) {

+

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == LIS3MDL_STOP) || (devp->state == LIS3MDL_READY),

+                "lis3mdlStop(), invalid state");

+

+#if (LIS3MDL_USE_I2C)

+  if (devp->state == LIS3MDL_STOP) {

+#if LIS3MDL_SHARED_I2C

+    i2cAcquireBus((devp)->config->i2cp);

+    i2cStart((devp)->config->i2cp,

+             (devp)->config->i2ccfg);

+#endif /* LIS3MDL_SHARED_I2C */

+    lis3mdlI2CWriteRegister(devp->config->i2cp,

+                            devp->config->slaveaddress,

+                            LIS3MDL_AD_CTRL_REG3,

+                            LIS3MDL_MD_POWER_DOWN);

+    i2cStop((devp)->config->i2cp);

+#if LIS3MDL_SHARED_I2C

+    i2cReleaseBus((devp)->config->i2cp);

+#endif /* LIS3MDL_SHARED_I2C */

+  }

+#endif /* LIS3MDL_USE_I2C */

+  devp->state = LIS3MDL_STOP;

+}

+/** @} */

diff --git a/os/ex/ST/lis3mdl.h b/os/ex/ST/lis3mdl.h
index e80579984..3a641b575 100644
--- a/os/ex/ST/lis3mdl.h
+++ b/os/ex/ST/lis3mdl.h
@@ -1,368 +1,368 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lis3mdl.h
- * @brief   LIS3MDL MEMS interface module header.
- *
- * @{
- */
-
-#ifndef _LIS3MDL_H_
-#define _LIS3MDL_H_
-
-#include "hal_compass.h"
-
-/*===========================================================================*/
-/* Driver constants.                                                         */
-/*===========================================================================*/
-
-/**
- * @brief   LIS3MDL number of axes.
- */
-#define  LIS3MDL_NUMBER_OF_AXES             ((size_t)   3U)
-
-/*===========================================================================*/
-/* Driver pre-compile time settings.                                         */
-/*===========================================================================*/
-
-/**
- * @name    Configuration options
- * @{
- */
-/**
- * @brief   LIS3MDL SPI interface switch.
- * @details If set to @p TRUE the support for SPI is included.
- * @note    The default is @p FALSE.
- */
-#if !defined(LIS3MDL_USE_SPI) || defined(__DOXYGEN__)
-#define LIS3MDL_USE_SPI                     FALSE
-#endif
-
-/**
- * @brief   LIS3MDL I2C interface switch.
- * @details If set to @p TRUE the support for I2C is included.
- * @note    The default is @p TRUE.
- */
-#if !defined(LIS3MDL_USE_I2C) || defined(__DOXYGEN__)
-#define LIS3MDL_USE_I2C                     TRUE
-#endif
-
-/**
- * @brief   LIS3MDL shared I2C switch.
- * @details If set to @p TRUE the device acquires I2C bus ownership
- *          on each transaction.
- * @note    The default is @p FALSE. Requires I2C_USE_MUTUAL_EXCLUSION
- */
-#if !defined(LIS3MDL_SHARED_I2C) || defined(__DOXYGEN__)
-#define LIS3MDL_SHARED_I2C                  FALSE
-#endif
-/** @} */
-
-/*===========================================================================*/
-/* Derived constants and error checks.                                       */
-/*===========================================================================*/
-
-#if !(LIS3MDL_USE_SPI ^ LIS3MDL_USE_I2C)
-#error "LIS3MDL_USE_SPI and LIS3MDL_USE_I2C cannot be both true or both false"
-#endif
-
-#if LIS3MDL_USE_SPI && !HAL_USE_SPI
-#error "LIS3MDL_USE_SPI requires HAL_USE_SPI"
-#endif
-
-#if LIS3MDL_USE_I2C && !HAL_USE_I2C
-#error "LIS3MDL_USE_I2C requires HAL_USE_I2C"
-#endif
-
-#if LIS3MDL_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION
-#error "LIS3MDL_SHARED_I2C requires I2C_USE_MUTUAL_EXCLUSION"
-#endif
-
-/*===========================================================================*/
-/* Driver data structures and types.                                         */
-/*===========================================================================*/
-
-/**
- * @name    LIS3MDL data structures and types
- * @{
- */
-/**
- * @brief  LIS3MDL slave address
- */
-typedef enum {
-  LIS3MDL_SAD_GND = 0x1C,           /**< Slave Address when SA1 is to GND   */
-  LIS3MDL_SAD_VCC = 0x1E            /**< Slave Address when SA1 is to VCC   */
-}lis3mdl_sad_t;
-
-/**
- * @brief   LIS3MDL full scale
- */
-typedef enum {
-  LIS3MDL_FS_4GA = 0x00,            /**< ±4 Gauss                           */
-  LIS3MDL_FS_8GA = 0x02,            /**< ±8 Gauss                           */
-  LIS3MDL_FS_12GA = 0x04,           /**< ±12 Gauss                          */
-  LIS3MDL_FS_16GA = 0x0C            /**< ±16 Gauss                          */
-}lis3mdl_fs_t;
-
-/**
- * @brief   LIS3MDL output data rate
- */
-typedef enum {
-  LIS3MDL_ODR_0_625HZ = 0x00,       /**< Output Data Rate = 0.625 Hz        */
-  LIS3MDL_ODR_1_25HZ = 0x04,        /**< Output Data Rate = 1.25 Hz         */
-  LIS3MDL_ODR_2_5HZ = 0x08,         /**< Output Data Rate = 2.5 Hz          */
-  LIS3MDL_ODR_5HZ = 0x0C,           /**< Output Data Rate = 5 Hz            */
-  LIS3MDL_ODR_10HZ = 0x10,          /**< Output Data Rate = 10 Hz           */
-  LIS3MDL_ODR_20HZ = 0x14,          /**< Output Data Rate = 20 Hz           */
-  LIS3MDL_ODR_40HZ = 0x18,          /**< Output Data Rate = 40 Hz           */
-  LIS3MDL_ODR_80HZ = 0x1C           /**< Output Data Rate = 80 Hz           */
-}lis3mdl_odr_t;
-
-/**
- * @brief  LIS3MDL low power mode configuration
- */
-typedef enum {
-  LIS3MDL_LP_DISABLED = 0x00,       /**< Low Power mode disabled            */
-  LIS3MDL_LP_ENABLED = 0x20         /**< Low Power mode enabled             */
-}lis3mdl_lp_t;
-
-/**
- * @brief  LIS3MDL conversion mode
- */
-typedef enum {
-  LIS3MDL_MD_CONTINUOUS = 0x00,     /**< Continuous conversion mode         */
-  LIS3MDL_MD_SINGLE = 0x01,         /**< Single conversion mode             */
-  LIS3MDL_MD_POWER_DOWN = 0x02      /**< Power down mode                    */
-}lis3mdl_md_t;
-
-/**
- * @brief  LIS3MDL operation mode for X and Y axes
- */
-typedef enum {
-  LIS3MDL_OMXY_LOW_POWER  = 0x00,   /**< X-Y axes low power mode            */
-  LIS3MDL_OMXY_MEDIUM = 0x20,       /**< X-Y axes medium performance mode   */
-  LIS3MDL_OMXY_HIGH = 0x40,         /**< X-Y axes high performance mode     */
-  LIS3MDL_OMXY_ULTRA = 0x60         /**< X-Y axes ultra performance mode    */
-}lis3mdl_omxy_t;
-
-/**
- * @brief  LIS3MDL operation mode for Z axis
- */
-typedef enum {
-  LIS3MDL_OMZ_LOW_POWER  = 0x00,    /**< Z axis low power mode              */
-  LIS3MDL_OMZ_MEDIUM = 0x04,        /**< Z axis medium performance mode     */
-  LIS3MDL_OMZ_HIGH = 0x08,          /**< Z axis high performance mode       */
-  LIS3MDL_OMZ_ULTRA = 0x0C          /**< Z axis ultra performance mode      */
-}lis3mdl_omz_t;
-
-/**
- * @brief   LIS3MDL temperature sensor enabling
- */
-typedef enum {
-  LIS3MDL_TEMP_DISABLED = 0x00,     /**< Temperature sensor disabled.       */
-  LIS3MDL_TEMP_ENABLED = 0x80       /**< Temperature sensor enabled.        */
-}lis3mdl_temp_t;
-
-/**
- * @brief  LIS3MDL block data update
- */
-typedef enum {
-  LIS3MDL_BDU_CONTINUOUS = 0x00,    /**< Continuous Update                  */
-  LIS3MDL_BDU_BLOCKED = 0x40        /**< Block data updated after reading.  */
-}lis3mdl_bdu_t;
-
-/**
- * @brief   LIS3MDL endianness
- */
-typedef enum {
-  LIS3MDL_END_LITTLE = 0x00,        /**< Little endian.                     */ 
-  LIS3MDL_END_BIG = 0x02            /**< Big endian.                        */
-}lis3mdl_end_t;
-
-/**
- * @brief   Driver state machine possible states.
- */
-typedef enum {
-  LIS3MDL_UNINIT = 0,               /**< Not initialized.                   */
-  LIS3MDL_STOP = 1,                 /**< Stopped.                           */
-  LIS3MDL_READY = 2,                /**< Ready.                             */
-} lis3mdl_state_t;
-
-/**
- * @brief   LIS3MDL configuration structure.
- */
-typedef struct {
-
-#if (LIS3MDL_USE_SPI) || defined(__DOXYGEN__)
-  /**
-   * @brief SPI driver associated to this LIS3MDL.
-   */
-  SPIDriver                 *spip;
-  /**
-   * @brief SPI configuration associated to this LIS3MDL.
-   */
-  const SPIConfig           *spicfg;
-#endif /* LIS3MDL_USE_SPI */
-#if (LIS3MDL_USE_I2C) || defined(__DOXYGEN__)
-  /**
-   * @brief I2C driver associated to this LIS3MDL.
-   */
-  I2CDriver                 *i2cp;
-  /**
-   * @brief I2C configuration associated to this LIS3MDL.
-   */
-  const I2CConfig           *i2ccfg;
-#endif /* LIS3MDL_USE_I2C */
-  /**
-   * @brief  LIS3MDL slave address
-   */
-  lis3mdl_sad_t              slaveaddress;
-  /**
-   * @brief   LIS3MDL full scale
-   */
-  lis3mdl_fs_t               fullscale;
-  /**
-   * @brief   LIS3MDL output data rate
-   */
-  lis3mdl_odr_t              outputdatarate;
-  /**
-   * @brief  LIS3MDL low power mode configuration
-   */
-  lis3mdl_lp_t               lowpowermode;
-  /**
-   * @brief  LIS3MDL conversion mode
-   */
-  lis3mdl_md_t               conversionmode;
-  /**
-   * @brief  LIS3MDL operation mode for X and Y axes
-   */
-  lis3mdl_omxy_t             operationmodexy;
-  /**
-   * @brief  LIS3MDL operation mode for Z axis
-   */
-  lis3mdl_omz_t              operationmodez;
-  /**
-   * @brief   LIS3MDL temperature sensor enabling
-   */
-  lis3mdl_temp_t             temperature;
-  /**
-   * @brief  LIS3MDL block data update
-   */
-  lis3mdl_bdu_t              blockdataupdate;
-  /**
-   * @brief   LIS3MDL endianness
-   */
-  lis3mdl_end_t              endianness;
-} LIS3MDLConfig;
-
-/**
- * @brief   Structure representing a LIS3MDL driver.
- */
-typedef struct LIS3MDLDriver LIS3MDLDriver;
-
-/**
- * @brief   @p LIS3MDL specific methods.
- */
-#define _lis3mdl_methods                                                    \
-  _base_compass_methods                                                     \
-  /* Retrieve the temperature of LIS3MDL chip.*/                            \
-  msg_t (*get_temperature)(void *instance, float* temperature);
-
-
-/**
- * @extends BaseCompassVMT
- *
- * @brief   @p LIS3MDL virtual methods table.
- */
-struct LIS3MDLVMT {
-  _lis3mdl_methods
-};
-
-/**
- * @brief   @p LIS3MDLDriver specific data.
- */
-#define _lis3mdl_data                                                       \
-  _base_compass_data                                                        \
-  /* Driver state.*/                                                        \
-  lis3mdl_state_t           state;                                          \
-  /* Current configuration data.*/                                          \
-  const LIS3MDLConfig       *config;                                        \
-  /* Current sensitivity.*/                                                 \
-  float                     sensitivity[LIS3MDL_NUMBER_OF_AXES];            \
-  /* Bias data.*/                                                           \
-  int32_t                   bias[LIS3MDL_NUMBER_OF_AXES];
-
-/**
- * @extends BaseCompass
- *
- * @brief   LIS3MDL 3-axis compass class.
- * @details This class extends @p BaseCompass by adding physical
- *          driver implementation.
- */
-struct LIS3MDLDriver {
-  /** @brief BaseSensor Virtual Methods Table. */
-  const struct BaseSensorVMT *vmt_basesensor;
-  /** @brief BaseCompass Virtual Methods Table. */
-  const struct BaseCompassVMT *vmt_basecompass;
-  /** @brief LIS3MDL Virtual Methods Table. */
-  const struct LIS3MDLVMT *vmt_lis3mdl;
-  _lis3mdl_data
-};
-/** @} */
-
-/*===========================================================================*/
-/* Driver macros.                                                            */
-/*===========================================================================*/
-
-/**
- * @brief   Get current MEMS temperature.
- * @detail  This information is very useful especially for high accuracy IMU.
- * @note    Temperature sensor must be enabled using a proper configuration.
- *
- * @param[in] ip        pointer to a @p BaseCompass class.
- * @param[out] temp     the MEMS temperature as single precision floating.
- *
- * @return              The operation status.
- * @retval MSG_OK       if the function succeeded.
- * @retval MSG_RESET    if one or more errors occurred.
- * @api
- */
-#define compassGetTemp(ip, tpp)                                             \
-        (ip)->vmt_lis3mdl->get_temperature(ip, tpp)
-
-/*===========================================================================*/
-/* External declarations.                                                    */
-/*===========================================================================*/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-  void lis3mdlObjectInit(LIS3MDLDriver *devp);
-  void lis3mdlStart(LIS3MDLDriver *devp, const LIS3MDLConfig *config);
-  void lis3mdlStop(LIS3MDLDriver *devp);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _LIS3MDL_H_ */
-
-/** @} */
-
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lis3mdl.h

+ * @brief   LIS3MDL MEMS interface module header.

+ *

+ * @{

+ */

+

+#ifndef _LIS3MDL_H_

+#define _LIS3MDL_H_

+

+#include "hal_compass.h"

+

+/*===========================================================================*/

+/* Driver constants.                                                         */

+/*===========================================================================*/

+

+/**

+ * @brief   LIS3MDL number of axes.

+ */

+#define  LIS3MDL_NUMBER_OF_AXES             ((size_t)   3U)

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    Configuration options

+ * @{

+ */

+/**

+ * @brief   LIS3MDL SPI interface switch.

+ * @details If set to @p TRUE the support for SPI is included.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(LIS3MDL_USE_SPI) || defined(__DOXYGEN__)

+#define LIS3MDL_USE_SPI                     FALSE

+#endif

+

+/**

+ * @brief   LIS3MDL I2C interface switch.

+ * @details If set to @p TRUE the support for I2C is included.

+ * @note    The default is @p TRUE.

+ */

+#if !defined(LIS3MDL_USE_I2C) || defined(__DOXYGEN__)

+#define LIS3MDL_USE_I2C                     TRUE

+#endif

+

+/**

+ * @brief   LIS3MDL shared I2C switch.

+ * @details If set to @p TRUE the device acquires I2C bus ownership

+ *          on each transaction.

+ * @note    The default is @p FALSE. Requires I2C_USE_MUTUAL_EXCLUSION

+ */

+#if !defined(LIS3MDL_SHARED_I2C) || defined(__DOXYGEN__)

+#define LIS3MDL_SHARED_I2C                  FALSE

+#endif

+/** @} */

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+#if !(LIS3MDL_USE_SPI ^ LIS3MDL_USE_I2C)

+#error "LIS3MDL_USE_SPI and LIS3MDL_USE_I2C cannot be both true or both false"

+#endif

+

+#if LIS3MDL_USE_SPI && !HAL_USE_SPI

+#error "LIS3MDL_USE_SPI requires HAL_USE_SPI"

+#endif

+

+#if LIS3MDL_USE_I2C && !HAL_USE_I2C

+#error "LIS3MDL_USE_I2C requires HAL_USE_I2C"

+#endif

+

+#if LIS3MDL_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION

+#error "LIS3MDL_SHARED_I2C requires I2C_USE_MUTUAL_EXCLUSION"

+#endif

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    LIS3MDL data structures and types

+ * @{

+ */

+/**

+ * @brief  LIS3MDL slave address

+ */

+typedef enum {

+  LIS3MDL_SAD_GND = 0x1C,           /**< Slave Address when SA1 is to GND   */

+  LIS3MDL_SAD_VCC = 0x1E            /**< Slave Address when SA1 is to VCC   */

+}lis3mdl_sad_t;

+

+/**

+ * @brief   LIS3MDL full scale

+ */

+typedef enum {

+  LIS3MDL_FS_4GA = 0x00,            /**< ±4 Gauss                           */

+  LIS3MDL_FS_8GA = 0x02,            /**< ±8 Gauss                           */

+  LIS3MDL_FS_12GA = 0x04,           /**< ±12 Gauss                          */

+  LIS3MDL_FS_16GA = 0x0C            /**< ±16 Gauss                          */

+}lis3mdl_fs_t;

+

+/**

+ * @brief   LIS3MDL output data rate

+ */

+typedef enum {

+  LIS3MDL_ODR_0_625HZ = 0x00,       /**< Output Data Rate = 0.625 Hz        */

+  LIS3MDL_ODR_1_25HZ = 0x04,        /**< Output Data Rate = 1.25 Hz         */

+  LIS3MDL_ODR_2_5HZ = 0x08,         /**< Output Data Rate = 2.5 Hz          */

+  LIS3MDL_ODR_5HZ = 0x0C,           /**< Output Data Rate = 5 Hz            */

+  LIS3MDL_ODR_10HZ = 0x10,          /**< Output Data Rate = 10 Hz           */

+  LIS3MDL_ODR_20HZ = 0x14,          /**< Output Data Rate = 20 Hz           */

+  LIS3MDL_ODR_40HZ = 0x18,          /**< Output Data Rate = 40 Hz           */

+  LIS3MDL_ODR_80HZ = 0x1C           /**< Output Data Rate = 80 Hz           */

+}lis3mdl_odr_t;

+

+/**

+ * @brief  LIS3MDL low power mode configuration

+ */

+typedef enum {

+  LIS3MDL_LP_DISABLED = 0x00,       /**< Low Power mode disabled            */

+  LIS3MDL_LP_ENABLED = 0x20         /**< Low Power mode enabled             */

+}lis3mdl_lp_t;

+

+/**

+ * @brief  LIS3MDL conversion mode

+ */

+typedef enum {

+  LIS3MDL_MD_CONTINUOUS = 0x00,     /**< Continuous conversion mode         */

+  LIS3MDL_MD_SINGLE = 0x01,         /**< Single conversion mode             */

+  LIS3MDL_MD_POWER_DOWN = 0x02      /**< Power down mode                    */

+}lis3mdl_md_t;

+

+/**

+ * @brief  LIS3MDL operation mode for X and Y axes

+ */

+typedef enum {

+  LIS3MDL_OMXY_LOW_POWER  = 0x00,   /**< X-Y axes low power mode            */

+  LIS3MDL_OMXY_MEDIUM = 0x20,       /**< X-Y axes medium performance mode   */

+  LIS3MDL_OMXY_HIGH = 0x40,         /**< X-Y axes high performance mode     */

+  LIS3MDL_OMXY_ULTRA = 0x60         /**< X-Y axes ultra performance mode    */

+}lis3mdl_omxy_t;

+

+/**

+ * @brief  LIS3MDL operation mode for Z axis

+ */

+typedef enum {

+  LIS3MDL_OMZ_LOW_POWER  = 0x00,    /**< Z axis low power mode              */

+  LIS3MDL_OMZ_MEDIUM = 0x04,        /**< Z axis medium performance mode     */

+  LIS3MDL_OMZ_HIGH = 0x08,          /**< Z axis high performance mode       */

+  LIS3MDL_OMZ_ULTRA = 0x0C          /**< Z axis ultra performance mode      */

+}lis3mdl_omz_t;

+

+/**

+ * @brief   LIS3MDL temperature sensor enabling

+ */

+typedef enum {

+  LIS3MDL_TEMP_DISABLED = 0x00,     /**< Temperature sensor disabled.       */

+  LIS3MDL_TEMP_ENABLED = 0x80       /**< Temperature sensor enabled.        */

+}lis3mdl_temp_t;

+

+/**

+ * @brief  LIS3MDL block data update

+ */

+typedef enum {

+  LIS3MDL_BDU_CONTINUOUS = 0x00,    /**< Continuous Update                  */

+  LIS3MDL_BDU_BLOCKED = 0x40        /**< Block data updated after reading.  */

+}lis3mdl_bdu_t;

+

+/**

+ * @brief   LIS3MDL endianness

+ */

+typedef enum {

+  LIS3MDL_END_LITTLE = 0x00,        /**< Little endian.                     */ 

+  LIS3MDL_END_BIG = 0x02            /**< Big endian.                        */

+}lis3mdl_end_t;

+

+/**

+ * @brief   Driver state machine possible states.

+ */

+typedef enum {

+  LIS3MDL_UNINIT = 0,               /**< Not initialized.                   */

+  LIS3MDL_STOP = 1,                 /**< Stopped.                           */

+  LIS3MDL_READY = 2,                /**< Ready.                             */

+} lis3mdl_state_t;

+

+/**

+ * @brief   LIS3MDL configuration structure.

+ */

+typedef struct {

+

+#if (LIS3MDL_USE_SPI) || defined(__DOXYGEN__)

+  /**

+   * @brief SPI driver associated to this LIS3MDL.

+   */

+  SPIDriver                 *spip;

+  /**

+   * @brief SPI configuration associated to this LIS3MDL.

+   */

+  const SPIConfig           *spicfg;

+#endif /* LIS3MDL_USE_SPI */

+#if (LIS3MDL_USE_I2C) || defined(__DOXYGEN__)

+  /**

+   * @brief I2C driver associated to this LIS3MDL.

+   */

+  I2CDriver                 *i2cp;

+  /**

+   * @brief I2C configuration associated to this LIS3MDL.

+   */

+  const I2CConfig           *i2ccfg;

+#endif /* LIS3MDL_USE_I2C */

+  /**

+   * @brief  LIS3MDL slave address

+   */

+  lis3mdl_sad_t              slaveaddress;

+  /**

+   * @brief   LIS3MDL full scale

+   */

+  lis3mdl_fs_t               fullscale;

+  /**

+   * @brief   LIS3MDL output data rate

+   */

+  lis3mdl_odr_t              outputdatarate;

+  /**

+   * @brief  LIS3MDL low power mode configuration

+   */

+  lis3mdl_lp_t               lowpowermode;

+  /**

+   * @brief  LIS3MDL conversion mode

+   */

+  lis3mdl_md_t               conversionmode;

+  /**

+   * @brief  LIS3MDL operation mode for X and Y axes

+   */

+  lis3mdl_omxy_t             operationmodexy;

+  /**

+   * @brief  LIS3MDL operation mode for Z axis

+   */

+  lis3mdl_omz_t              operationmodez;

+  /**

+   * @brief   LIS3MDL temperature sensor enabling

+   */

+  lis3mdl_temp_t             temperature;

+  /**

+   * @brief  LIS3MDL block data update

+   */

+  lis3mdl_bdu_t              blockdataupdate;

+  /**

+   * @brief   LIS3MDL endianness

+   */

+  lis3mdl_end_t              endianness;

+} LIS3MDLConfig;

+

+/**

+ * @brief   Structure representing a LIS3MDL driver.

+ */

+typedef struct LIS3MDLDriver LIS3MDLDriver;

+

+/**

+ * @brief   @p LIS3MDL specific methods.

+ */

+#define _lis3mdl_methods                                                    \

+  _base_compass_methods                                                     \

+  /* Retrieve the temperature of LIS3MDL chip.*/                            \

+  msg_t (*get_temperature)(void *instance, float* temperature);

+

+

+/**

+ * @extends BaseCompassVMT

+ *

+ * @brief   @p LIS3MDL virtual methods table.

+ */

+struct LIS3MDLVMT {

+  _lis3mdl_methods

+};

+

+/**

+ * @brief   @p LIS3MDLDriver specific data.

+ */

+#define _lis3mdl_data                                                       \

+  _base_compass_data                                                        \

+  /* Driver state.*/                                                        \

+  lis3mdl_state_t           state;                                          \

+  /* Current configuration data.*/                                          \

+  const LIS3MDLConfig       *config;                                        \

+  /* Current sensitivity.*/                                                 \

+  float                     sensitivity[LIS3MDL_NUMBER_OF_AXES];            \

+  /* Bias data.*/                                                           \

+  int32_t                   bias[LIS3MDL_NUMBER_OF_AXES];

+

+/**

+ * @extends BaseCompass

+ *

+ * @brief   LIS3MDL 3-axis compass class.

+ * @details This class extends @p BaseCompass by adding physical

+ *          driver implementation.

+ */

+struct LIS3MDLDriver {

+  /** @brief BaseSensor Virtual Methods Table. */

+  const struct BaseSensorVMT *vmt_basesensor;

+  /** @brief BaseCompass Virtual Methods Table. */

+  const struct BaseCompassVMT *vmt_basecompass;

+  /** @brief LIS3MDL Virtual Methods Table. */

+  const struct LIS3MDLVMT *vmt_lis3mdl;

+  _lis3mdl_data

+};

+/** @} */

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+/**

+ * @brief   Get current MEMS temperature.

+ * @detail  This information is very useful especially for high accuracy IMU.

+ * @note    Temperature sensor must be enabled using a proper configuration.

+ *

+ * @param[in] ip        pointer to a @p BaseCompass class.

+ * @param[out] temp     the MEMS temperature as single precision floating.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ * @api

+ */

+#define compassGetTemp(ip, tpp)                                             \

+        (ip)->vmt_lis3mdl->get_temperature(ip, tpp)

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void lis3mdlObjectInit(LIS3MDLDriver *devp);

+  void lis3mdlStart(LIS3MDLDriver *devp, const LIS3MDLConfig *config);

+  void lis3mdlStop(LIS3MDLDriver *devp);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* _LIS3MDL_H_ */

+

+/** @} */

+

diff --git a/os/ex/ST/lsm303dlhc.c b/os/ex/ST/lsm303dlhc.c
index 74cde4caf..ab57cb5d9 100644
--- a/os/ex/ST/lsm303dlhc.c
+++ b/os/ex/ST/lsm303dlhc.c
@@ -1,904 +1,904 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lsm303dlhc.c
- * @brief   LSM303DLHC MEMS interface module code.
- *
- * @addtogroup lsm303dlhc
- * @{
- */
-
-#include "hal.h"
-#include "lsm303dlhc.h"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-#define  LSM303DLHC_ACC_SENS_2G             ((float)0.0610f)
-#define  LSM303DLHC_ACC_SENS_4G             ((float)0.1221f)
-#define  LSM303DLHC_ACC_SENS_8G             ((float)0.2442f)
-#define  LSM303DLHC_ACC_SENS_16G            ((float)0.4884f)
-
-#define  LSM303DLHC_COMP_SENS_XY_1_3GA      ((float)1100.0f)
-#define  LSM303DLHC_COMP_SENS_XY_1_9GA      ((float)855.0f)
-#define  LSM303DLHC_COMP_SENS_XY_2_5GA      ((float)670.0f)
-#define  LSM303DLHC_COMP_SENS_XY_4_0GA      ((float)450.0f)
-#define  LSM303DLHC_COMP_SENS_XY_4_7GA      ((float)400.0f)
-#define  LSM303DLHC_COMP_SENS_XY_5_6GA      ((float)330.0f)
-#define  LSM303DLHC_COMP_SENS_XY_8_1GA      ((float)230.0f)
-
-#define  LSM303DLHC_COMP_SENS_Z_1_3GA       ((float)980.0f)
-#define  LSM303DLHC_COMP_SENS_Z_1_9GA       ((float)765.0f)
-#define  LSM303DLHC_COMP_SENS_Z_2_5GA       ((float)600.0f)
-#define  LSM303DLHC_COMP_SENS_Z_4_0GA       ((float)400.0f)
-#define  LSM303DLHC_COMP_SENS_Z_4_7GA       ((float)355.0f)
-#define  LSM303DLHC_COMP_SENS_Z_5_6GA       ((float)295.0f)
-#define  LSM303DLHC_COMP_SENS_Z_8_1GA       ((float)205.0f)
-
-#define  LSM303DLHC_DI                      ((uint8_t)0xFF)
-#define  LSM303DLHC_DI_0                    ((uint8_t)0x01)
-#define  LSM303DLHC_DI_1                    ((uint8_t)0x02)
-#define  LSM303DLHC_DI_2                    ((uint8_t)0x04)
-#define  LSM303DLHC_DI_3                    ((uint8_t)0x08)
-#define  LSM303DLHC_DI_4                    ((uint8_t)0x10)
-#define  LSM303DLHC_DI_5                    ((uint8_t)0x20)
-#define  LSM303DLHC_DI_6                    ((uint8_t)0x40)
-#define  LSM303DLHC_DI_7                    ((uint8_t)0x80)
-
-#define  LSM303DLHC_AD_0                    ((uint8_t)0x01)
-#define  LSM303DLHC_AD_1                    ((uint8_t)0x02)
-#define  LSM303DLHC_AD_2                    ((uint8_t)0x04)
-#define  LSM303DLHC_AD_3                    ((uint8_t)0x08)
-#define  LSM303DLHC_AD_4                    ((uint8_t)0x10)
-#define  LSM303DLHC_AD_5                    ((uint8_t)0x20)
-#define  LSM303DLHC_AD_6                    ((uint8_t)0x40)
-#define  LSM303DLHC_RW                      ((uint8_t)0x80)
-
-#define  LSM303DLHC_AD_ACC_CTRL_REG1        ((uint8_t)0x20)
-#define  LSM303DLHC_AD_ACC_CTRL_REG2        ((uint8_t)0x21)
-#define  LSM303DLHC_AD_ACC_CTRL_REG3        ((uint8_t)0x22)
-#define  LSM303DLHC_AD_ACC_CTRL_REG4        ((uint8_t)0x23)
-#define  LSM303DLHC_AD_ACC_CTRL_REG5        ((uint8_t)0x24)
-#define  LSM303DLHC_AD_ACC_CTRL_REG6        ((uint8_t)0x25)
-#define  LSM303DLHC_AD_ACC_REFERENCE        ((uint8_t)0x26)
-#define  LSM303DLHC_AD_ACC_STATUS_REG       ((uint8_t)0x27)
-#define  LSM303DLHC_AD_ACC_OUT_X_L          ((uint8_t)0x28)
-#define  LSM303DLHC_AD_ACC_OUT_X_H          ((uint8_t)0x29)
-#define  LSM303DLHC_AD_ACC_OUT_Y_L          ((uint8_t)0x2A)
-#define  LSM303DLHC_AD_ACC_OUT_Y_H          ((uint8_t)0x2B)
-#define  LSM303DLHC_AD_ACC_OUT_Z_L          ((uint8_t)0x2C)
-#define  LSM303DLHC_AD_ACC_OUT_Z_H          ((uint8_t)0x2D)
-#define  LSM303DLHC_AD_ACC_FIFO_CTRL_REG    ((uint8_t)0x2E)
-#define  LSM303DLHC_AD_ACC_FIFO_SRC_REG     ((uint8_t)0x2F)
-#define  LSM303DLHC_AD_ACC_INT1_CFG         ((uint8_t)0x30)
-#define  LSM303DLHC_AD_ACC_INT1_SRC         ((uint8_t)0x31)
-#define  LSM303DLHC_AD_ACC_INT1_THS         ((uint8_t)0x32)
-#define  LSM303DLHC_AD_ACC_INT1_DURATION    ((uint8_t)0x33)
-#define  LSM303DLHC_AD_ACC_INT2_CFG         ((uint8_t)0x34)
-#define  LSM303DLHC_AD_ACC_INT2_SRC         ((uint8_t)0x35)
-#define  LSM303DLHC_AD_ACC_INT2_THS         ((uint8_t)0x36)
-#define  LSM303DLHC_AD_ACC_INT2_DURATION    ((uint8_t)0x37)
-#define  LSM303DLHC_AD_ACC_CLICK_CFG        ((uint8_t)0x38)
-#define  LSM303DLHC_AD_ACC_CLICK_SRC        ((uint8_t)0x39)
-#define  LSM303DLHC_AD_ACC_CLICK_THS        ((uint8_t)0x3A)
-#define  LSM303DLHC_AD_ACC_TIME_LIMIT       ((uint8_t)0x3B)
-#define  LSM303DLHC_AD_ACC_TIME_LATENCY     ((uint8_t)0x3C)
-#define  LSM303DLHC_AD_ACC_TIME_WINDOW      ((uint8_t)0x3D)
-#define  LSM303DLHC_AD_COMP_CRA_REG         ((uint8_t)0x00)
-#define  LSM303DLHC_AD_COMP_CRB_REG         ((uint8_t)0x01)
-#define  LSM303DLHC_AD_COMP_MR_REG          ((uint8_t)0x02)
-#define  LSM303DLHC_AD_COMP_OUT_X_H         ((uint8_t)0x03)
-#define  LSM303DLHC_AD_COMP_OUT_X_L         ((uint8_t)0x04)
-#define  LSM303DLHC_AD_COMP_OUT_Z_H         ((uint8_t)0x05)
-#define  LSM303DLHC_AD_COMP_OUT_Z_L         ((uint8_t)0x06)
-#define  LSM303DLHC_AD_COMP_OUT_Y_H         ((uint8_t)0x07)
-#define  LSM303DLHC_AD_COMP_OUT_Y_L         ((uint8_t)0x08)
-#define  LSM303DLHC_AD_COMP_SR_REG          ((uint8_t)0x09)
-#define  LSM303DLHC_AD_COMP_IRA_REG         ((uint8_t)0x0A)
-#define  LSM303DLHC_AD_COMP_IRB_REG         ((uint8_t)0x0B)
-#define  LSM303DLHC_AD_COMP_IRC_REG         ((uint8_t)0x0C)
-#define  LSM303DLHC_AD_COMP_TEMP_OUT_H      ((uint8_t)0x31)
-#define  LSM303DLHC_AD_COMP_TEMP_OUT_L      ((uint8_t)0x32)
-
-#define  LSM303DLHC_ACC_CTRL_REG4_FS_MASK   ((uint8_t)0x30)
-#define  LSM303DLHC_COMP_CTRL_REGB_FS_MASK  ((uint8_t)0xE0)
-
-#define  TO_G                              ((float)0.001f)
-#define  TO_SI                             ((float)0.00981f)
-
-/*===========================================================================*/
-/* Driver exported variables.                                                */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local variables and types.                                         */
-/*===========================================================================*/
-
-/**
- * @brief  Accelerometer Power Mode
- */
-typedef enum {
-  LSM303DLHC_ACC_PM_NORMAL = 0x00,  /**< Normal mode enabled                */
-  LSM303DLHC_ACC_PM_LP = 0x08       /**< Low Power mode enabled             */
-} lsm303dlhc_acc_pm_t;
-
-/**
- * @brief  Accelerometer and Compass Slave Address.
- */
-typedef enum {
-  LSM303DLHC_SAD_ACC = 0x19,        /**< SAD for accelerometer.             */
-  LSM303DLHC_SAD_COMP = 0x1E        /**< SAD for compass.                   */
-} lsm303dlhc_sad_t;
-/*===========================================================================*/
-/* Driver local functions.                                                   */
-/*===========================================================================*/
-
-#if (LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)
-/**
- * @brief   Reads registers value using I2C.
- * @pre     The I2C interface must be initialized and the driver started.
- * @note    IF_ADD_INC bit must be 1 in CTRL_REG8
- *
- * @param[in] i2cp       pointer to the I2C interface
- * @param[in] sad        slave address without R bit
- * @param[in] reg        first sub-register address
- * @return               the read value.
- */
-uint8_t lsm303dlhcI2CReadRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad, uint8_t reg,
-                               msg_t* msgp) {
-  msg_t msg;
-#if defined(STM32F103_MCUCONF)
-  uint8_t rxbuf[2];
-  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,
-                                   TIME_INFINITE);
-  if(msgp != NULL){
-    *msgp = msg;
-  }
-  return rxbuf[0];
-#else
-  uint8_t txbuf, rxbuf;
-  txbuf = reg;
-  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, &rxbuf, 1,
-                                        TIME_INFINITE);
-  if(msgp != NULL){
-    *msgp = msg;
-  }
-  return rxbuf;
-#endif
-}
-
-/**
- * @brief   Writes a value into a register using I2C.
- * @pre     The I2C interface must be initialized and the driver started.
- *
- * @param[in] i2cp       pointer to the I2C interface
- * @param[in] sad        slave address without R bit
- * @param[in] sub        sub-register address
- * @param[in] value      the value to be written
- * @return               the operation status.
- */
-msg_t lsm303dlhcI2CWriteRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad, uint8_t reg,
-                              uint8_t value) {
-  uint8_t rxbuf;
-  uint8_t txbuf[2];
-  if(sad == LSM303DLHC_SAD_ACC) {
-    switch (reg) {
-    default:
-      /* Reserved register must not be written, according to the datasheet
-       * this could permanently damage the device.
-       */
-      osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), reserved register");
-    case LSM303DLHC_AD_ACC_STATUS_REG:
-    case LSM303DLHC_AD_ACC_OUT_X_L:
-    case LSM303DLHC_AD_ACC_OUT_X_H:
-    case LSM303DLHC_AD_ACC_OUT_Y_L:
-    case LSM303DLHC_AD_ACC_OUT_Y_H:
-    case LSM303DLHC_AD_ACC_OUT_Z_L:
-    case LSM303DLHC_AD_ACC_OUT_Z_H:
-    case LSM303DLHC_AD_ACC_FIFO_SRC_REG:
-    case LSM303DLHC_AD_ACC_INT1_SRC:
-    case LSM303DLHC_AD_ACC_INT2_SRC:
-    /* Read only registers cannot be written, the command is ignored.*/
-      return MSG_RESET;
-    case LSM303DLHC_AD_ACC_CTRL_REG1:
-    case LSM303DLHC_AD_ACC_CTRL_REG2:
-    case LSM303DLHC_AD_ACC_CTRL_REG3:
-    case LSM303DLHC_AD_ACC_CTRL_REG4:
-    case LSM303DLHC_AD_ACC_CTRL_REG5:
-    case LSM303DLHC_AD_ACC_CTRL_REG6:
-    case LSM303DLHC_AD_ACC_REFERENCE:
-    case LSM303DLHC_AD_ACC_FIFO_CTRL_REG:
-    case LSM303DLHC_AD_ACC_INT1_CFG:
-    case LSM303DLHC_AD_ACC_INT1_THS:
-    case LSM303DLHC_AD_ACC_INT1_DURATION:
-    case LSM303DLHC_AD_ACC_INT2_CFG:
-    case LSM303DLHC_AD_ACC_INT2_THS:
-    case LSM303DLHC_AD_ACC_INT2_DURATION:
-    case LSM303DLHC_AD_ACC_CLICK_CFG:
-    case LSM303DLHC_AD_ACC_CLICK_SRC:
-    case LSM303DLHC_AD_ACC_CLICK_THS:
-    case LSM303DLHC_AD_ACC_TIME_LIMIT:
-    case LSM303DLHC_AD_ACC_TIME_LATENCY:
-    case LSM303DLHC_AD_ACC_TIME_WINDOW:
-      txbuf[0] = reg;
-      txbuf[1] = value;
-      return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
-      break;
-    }
-  }
-  if(sad == LSM303DLHC_SAD_COMP) {
-    switch (reg) {
-    default:
-      /* Reserved register must not be written, according to the datasheet
-       * this could permanently damage the device.
-       */
-      osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), reserved register");
-    case LSM303DLHC_AD_COMP_OUT_X_H:
-    case LSM303DLHC_AD_COMP_OUT_X_L:
-    case LSM303DLHC_AD_COMP_OUT_Z_H:
-    case LSM303DLHC_AD_COMP_OUT_Z_L:
-    case LSM303DLHC_AD_COMP_OUT_Y_H:
-    case LSM303DLHC_AD_COMP_OUT_Y_L:
-    case LSM303DLHC_AD_COMP_SR_REG:
-    case LSM303DLHC_AD_COMP_IRA_REG:
-    case LSM303DLHC_AD_COMP_IRB_REG:
-    case LSM303DLHC_AD_COMP_IRC_REG:
-    case LSM303DLHC_AD_COMP_TEMP_OUT_H:
-    case LSM303DLHC_AD_COMP_TEMP_OUT_L:
-    /* Read only registers cannot be written, the command is ignored.*/
-      return MSG_RESET;
-    case LSM303DLHC_AD_COMP_CRA_REG:
-    case LSM303DLHC_AD_COMP_CRB_REG:
-    case LSM303DLHC_AD_COMP_MR_REG:
-      txbuf[0] = reg;
-      txbuf[1] = value;
-      return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
-      break;
-    }
-  }
-  osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), wrong SAD");
-  return MSG_RESET;
-}
-#endif /* LSM303DLHC_USE_I2C */
-
-/*
- * Interface implementation.
- */
-static size_t acc_get_axes_number(void *ip) {
-
-  osalDbgCheck(ip != NULL);
-  return LSM303DLHC_ACC_NUMBER_OF_AXES;
-}
-
-static size_t comp_get_axes_number(void *ip) {
-
-  osalDbgCheck(ip != NULL);
-  return LSM303DLHC_COMP_NUMBER_OF_AXES;
-}
-
-static size_t sens_get_axes_number(void *ip) {
-  size_t size = 0;
-  
-  osalDbgCheck(ip != NULL);
-  if (((LSM303DLHCDriver *)ip)->config->acccfg != NULL)
-	size += acc_get_axes_number(ip);
-  if (((LSM303DLHCDriver *)ip)->config->compcfg != NULL)
-    size += comp_get_axes_number(ip);  
-  return size;
-}
-
-static msg_t acc_read_raw(void *ip, int32_t axes[]) {
-  int16_t tmp;
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM303DLHCDriver *)ip)->config->acccfg != NULL));
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
-              "acc_read_raw(), invalid state");
-
-#if LSM303DLHC_USE_I2C
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->config->i2cp->state == I2C_READY),
-                "acc_read_raw(), channel not ready");
-#if LSM303DLHC_SHARED_I2C
-  i2cAcquireBus(((LSM303DLHCDriver *)ip)->config->i2cp);
-  i2cStart(((LSM303DLHCDriver *)ip)->config->i2cp,
-           ((LSM303DLHCDriver *)ip)->config->i2ccfg);
-#endif /* LSM303DLHC_SHARED_I2C */
-  if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_X){
-    tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                 LSM303DLHC_SAD_ACC,
-                                 LSM303DLHC_AD_ACC_OUT_X_L, NULL);
-    tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                  LSM303DLHC_SAD_ACC,
-                                  LSM303DLHC_AD_ACC_OUT_X_H, NULL) << 8;
-    axes[0] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[0];
-  }
-  if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_Y){
-    tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                 LSM303DLHC_SAD_ACC,
-                                 LSM303DLHC_AD_ACC_OUT_Y_L, NULL);
-    tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                  LSM303DLHC_SAD_ACC,
-                                  LSM303DLHC_AD_ACC_OUT_Y_H, NULL) << 8;
-    axes[1] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[1];
-  }
-  if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_Z){
-    tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                 LSM303DLHC_SAD_ACC,
-                                 LSM303DLHC_AD_ACC_OUT_Z_L, NULL);
-    tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                  LSM303DLHC_SAD_ACC,
-                                  LSM303DLHC_AD_ACC_OUT_Z_H, NULL) << 8;
-    axes[2] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[2];
-  }
-#if LSM303DLHC_SHARED_I2C
-  i2cReleaseBus(((LSM303DLHCDriver *)ip)->config->i2cp);
-#endif /* LSM303DLHC_SHARED_I2C */
-#endif /* LSM303DLHC_USE_I2C */
-  return MSG_OK;
-}
-
-static msg_t comp_read_raw(void *ip, int32_t axes[]) {
-  int16_t tmp;
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM303DLHCDriver *)ip)->config->compcfg != NULL));
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
-              "comp_read_raw(), invalid state");
-
-#if LSM303DLHC_USE_I2C
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->config->i2cp->state == I2C_READY),
-                "comp_read_raw(), channel not ready");
-#if LSM303DLHC_SHARED_I2C
-  i2cAcquireBus(((LSM303DLHCDriver *)ip)->config->i2cp);
-  i2cStart(((LSM303DLHCDriver *)ip)->config->i2cp,
-           ((LSM303DLHCDriver *)ip)->config->i2ccfg);
-#endif /* LSM303DLHC_SHARED_I2C */
-  tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                               LSM303DLHC_SAD_COMP,
-                               LSM303DLHC_AD_COMP_OUT_X_L, NULL);
-  tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                LSM303DLHC_SAD_COMP,
-                                LSM303DLHC_AD_COMP_OUT_X_H, NULL) << 8;
-  axes[0] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[0];
-
-  tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                               LSM303DLHC_SAD_COMP,
-                               LSM303DLHC_AD_COMP_OUT_Y_L, NULL);
-  tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                LSM303DLHC_SAD_COMP,
-                                LSM303DLHC_AD_COMP_OUT_Y_H, NULL) << 8;
-  axes[1] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[1];
-
-  tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                               LSM303DLHC_SAD_COMP,
-                               LSM303DLHC_AD_COMP_OUT_Z_L, NULL);
-  tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
-                                LSM303DLHC_SAD_COMP,
-                                LSM303DLHC_AD_COMP_OUT_Z_H, NULL) << 8;
-  axes[2] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[2];
-#if LSM303DLHC_SHARED_I2C
-  i2cReleaseBus(((LSM303DLHCDriver *)ip)->config->i2cp);
-#endif /* LSM303DLHC_SHARED_I2C */
-#endif /* LSM303DLHC_USE_I2C */ 
-  return MSG_OK;
-}
-
-static msg_t sens_read_raw(void *ip, int32_t axes[]) {
-  int32_t* bp = axes;
-  msg_t msg;
-  if (((LSM303DLHCDriver *)ip)->config->acccfg != NULL) {
-	msg = acc_read_raw(ip, bp);
-	if(msg != MSG_OK)
-	  return msg;
-	bp += LSM303DLHC_ACC_NUMBER_OF_AXES;
-  }
-  if (((LSM303DLHCDriver *)ip)->config->compcfg != NULL) {
-    msg = comp_read_raw(ip, bp);
-  }
-  return msg;
-}
-	
-static msg_t acc_read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[LSM303DLHC_ACC_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM303DLHCDriver *)ip)->config->acccfg != NULL));
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
-              "acc_read_cooked(), invalid state");
-
-  msg = acc_read_raw(ip, raw);
-  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] * ((LSM303DLHCDriver *)ip)->accsensitivity[i];
-    if(((LSM303DLHCDriver *)ip)->config->acccfg->unit == LSM303DLHC_ACC_UNIT_G){
-      axes[i] *= TO_G;
-    }
-    else if(((LSM303DLHCDriver *)ip)->config->acccfg->unit == LSM303DLHC_ACC_UNIT_SI){
-      axes[i] *= TO_SI;
-    }
-  }
-  return msg;
-}
-
-static msg_t comp_read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[LSM303DLHC_COMP_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM303DLHCDriver *)ip)->config->compcfg != NULL));
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
-              "comp_read_cooked(), invalid state");
-
-  msg = comp_read_raw(ip, raw);
-  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] / ((LSM303DLHCDriver *)ip)->compsensitivity[i];
-  }
-  return msg;
-}
-
-static msg_t sens_read_cooked(void *ip, float axes[]) {
-  float* bp = axes;
-  msg_t msg;
-  if (((LSM303DLHCDriver *)ip)->config->acccfg != NULL) {
-	msg = acc_read_cooked(ip, bp);
-	if(msg != MSG_OK)
-	  return msg;
-	bp += LSM303DLHC_ACC_NUMBER_OF_AXES;
-  }
-  if (((LSM303DLHCDriver *)ip)->config->compcfg != NULL) {
-    msg = comp_read_cooked(ip, bp);
-  }
-  return msg;
-}
-
-static msg_t acc_set_bias(void *ip, int32_t *bp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (bp !=NULL));
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY) ||
-                (((LSM303DLHCDriver *)ip)->state == LSM303DLHC_STOP),
-                "acc_set_bias(), invalid state");
-  
-  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
-    ((LSM303DLHCDriver *)ip)->accbias[i] = bp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t comp_set_bias(void *ip, int32_t *bp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (bp !=NULL));
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY) ||
-                (((LSM303DLHCDriver *)ip)->state == LSM303DLHC_STOP),
-                "comp_set_bias(), invalid state");
-  
-  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-    ((LSM303DLHCDriver *)ip)->compbias[i] = bp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t acc_reset_bias(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY) ||
-                (((LSM303DLHCDriver *)ip)->state == LSM303DLHC_STOP),
-              "acc_reset_bias(), invalid state");
-
-  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-    ((LSM303DLHCDriver *)ip)->accbias[i] = 0;
-  return MSG_OK;
-}
-
-static msg_t comp_reset_bias(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY) ||
-                (((LSM303DLHCDriver *)ip)->state == LSM303DLHC_STOP),
-              "comp_reset_bias(), invalid state");
-
-  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)
-    ((LSM303DLHCDriver *)ip)->compbias[i] = 0;
-  return MSG_OK;
-}
-
-static msg_t acc_set_sensivity(void *ip, float *sp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (sp !=NULL));
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
-                "acc_set_sensivity(), invalid state");
-  
-  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
-    ((LSM303DLHCDriver *)ip)->accsensitivity[i] = sp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t comp_set_sensivity(void *ip, float *sp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (sp !=NULL));
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
-                "comp_set_sensivity(), invalid state");
-  
-  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-    ((LSM303DLHCDriver *)ip)->compsensitivity[i] = sp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t acc_reset_sensivity(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
-                "acc_reset_sensivity(), invalid state");
-
-  if(((LSM303DLHCDriver *)ip)->config->acccfg->fullscale == LSM303DLHC_ACC_FS_2G)
-    for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-      ((LSM303DLHCDriver *)ip)->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G;
-  else if(((LSM303DLHCDriver *)ip)->config->acccfg->fullscale == LSM303DLHC_ACC_FS_4G)
-	for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-      ((LSM303DLHCDriver *)ip)->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G;
-  else if(((LSM303DLHCDriver *)ip)->config->acccfg->fullscale == LSM303DLHC_ACC_FS_8G)
-	for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-      ((LSM303DLHCDriver *)ip)->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G;
-  else if(((LSM303DLHCDriver *)ip)->config->acccfg->fullscale == LSM303DLHC_ACC_FS_16G)
-	for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-      ((LSM303DLHCDriver *)ip)->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G;
-  else {
-    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");
-    return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
-static msg_t comp_reset_sensivity(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),
-                "comp_reset_sensivity(), invalid state");
-
-  if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_3_GA)
-    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-      if(i != 2) {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_3GA;
-      }
-      else {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_3GA;
-      }
-    }
-  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_9_GA)
-    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-      if(i != 2) {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_9GA;
-      }
-      else {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_9GA;
-      }
-    }
-  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2_5_GA)
-    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-      if(i != 2) {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2_5GA;
-      }
-      else {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2_5GA;
-      }
-    }
-  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_0_GA)
-    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-      if(i != 2) {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_0GA;
-      }
-      else {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_0GA;
-      }
-    }
-  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_7_GA)
-    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-      if(i != 2) {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_7GA;
-      }
-      else {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_7GA;
-      }
-    }
-  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5_6_GA)
-    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-      if(i != 2) {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5_6GA;
-      }
-      else {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5_6GA;
-      }
-    }
-  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8_1_GA)
-    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-      if(i != 2) {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8_1GA;
-      }
-      else {
-        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_8_1GA;
-      }
-    }
-  else {
-    osalDbgAssert(FALSE, "reset_sensivity(), compass full scale issue");
-    return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
-static const struct BaseSensorVMT vmt_basesensor = {
-  sens_get_axes_number, sens_read_raw, sens_read_cooked
-};
-
-static const struct BaseCompassVMT vmt_basecompass = {
-  comp_get_axes_number, comp_read_raw, comp_read_cooked,
-  comp_set_bias, comp_reset_bias, comp_set_sensivity, comp_reset_sensivity
-};
-
-static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {
-  acc_get_axes_number, acc_read_raw, acc_read_cooked,
-  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
-};
-
-static const struct LSM303DLHCACCVMT vmt_lsm303dlhcacc = {
-  acc_get_axes_number, acc_read_raw, acc_read_cooked,
-  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
-};
-
-static const struct LSM303DLHCCOMPVMT vmt_lsm303dlhccomp = {
-  comp_get_axes_number, comp_read_raw, comp_read_cooked,
-  comp_set_bias, comp_reset_bias, comp_set_sensivity, comp_reset_sensivity
-};
-
-/*===========================================================================*/
-/* Driver exported functions.                                                */
-/*===========================================================================*/
-
-/**
- * @brief   Initializes an instance.
- *
- * @param[out] devp     pointer to the @p LSM303DLHCDriver object
- *
- * @init
- */
-void lsm303dlhcObjectInit(LSM303DLHCDriver *devp) {
-  uint32_t i;
-  devp->vmt_basesensor = &vmt_basesensor;
-  devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;
-  devp->vmt_basecompass = &vmt_basecompass;
-  devp->vmt_lsm303dlhcacc = &vmt_lsm303dlhcacc;
-  devp->vmt_lsm303dlhccomp = &vmt_lsm303dlhccomp;
-  devp->config = NULL;
-  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-    devp->accbias[i] = 0;
-  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)
-    devp->compbias[i] = 0;
-  devp->state  = LSM303DLHC_STOP;
-}
-
-/**
- * @brief   Configures and activates LSM303DLHC Complex Driver peripheral.
- *
- * @param[in] devp      pointer to the @p LSM303DLHCDriver object
- * @param[in] config    pointer to the @p LSM303DLHCConfig object
- *
- * @api
- */
-void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
-  uint32_t i;
-  osalDbgCheck((devp != NULL) && (config != NULL));
-
-  osalDbgAssert((devp->state == LSM303DLHC_STOP) || (devp->state == LSM303DLHC_READY),
-              "lsm303dlhcStart(), invalid state");			  
-
-  devp->config = config;
-
-#if LSM303DLHC_USE_I2C
-#if	LSM303DLHC_SHARED_I2C
-  i2cAcquireBus((devp)->config->i2cp);
-#endif /* LSM303DLHC_SHARED_I2C */
-  i2cStart((devp)->config->i2cp,
-           (devp)->config->i2ccfg);
-  if((devp)->config->acccfg != NULL) {
-    lsm303dlhcI2CWriteRegister(devp->config->i2cp,
-                               LSM303DLHC_SAD_ACC,
-                               LSM303DLHC_AD_ACC_CTRL_REG1,
-                               devp->config->acccfg->axesenabling |
-                               devp->config->acccfg->outdatarate |
-                               devp->config->acccfg->lowpower);
-    lsm303dlhcI2CWriteRegister(devp->config->i2cp,
-                               LSM303DLHC_SAD_ACC,
-                               LSM303DLHC_AD_ACC_CTRL_REG4,
-                               devp->config->acccfg->endianess |
-                               devp->config->acccfg->fullscale |
-                               devp->config->acccfg->blockdataupdate |
-                               devp->config->acccfg->highresmode);
-  }
-  if((devp)->config->compcfg != NULL) {
-    lsm303dlhcI2CWriteRegister(devp->config->i2cp,
-                               LSM303DLHC_SAD_COMP,
-                               LSM303DLHC_AD_COMP_CRA_REG,
-                               devp->config->compcfg->outputdatarate);
-    lsm303dlhcI2CWriteRegister(devp->config->i2cp,
-                               LSM303DLHC_SAD_COMP,
-                               LSM303DLHC_AD_COMP_CRB_REG,
-                               devp->config->compcfg->fullscale);
-    lsm303dlhcI2CWriteRegister(devp->config->i2cp,
-                               LSM303DLHC_SAD_COMP,
-                               LSM303DLHC_AD_COMP_MR_REG,
-                               devp->config->compcfg->mode);
-  }
-#if	LSM303DLHC_SHARED_I2C
-  i2cReleaseBus((devp)->config->i2cp);
-#endif /* LSM303DLHC_SHARED_I2C */  
-#endif /* LSM303DLHC_USE_I2C */
-  /* Storing sensitivity information according to full scale value */
-  if((devp)->config->acccfg != NULL) {
-    if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_2G)
-      for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-        devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G;
-    else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_4G)
-      for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-        devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G;
-    else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_8G)
-      for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-        devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G;
-    else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_16G)
-      for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
-        devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G;
-    else
-      osalDbgAssert(FALSE, "lsm303dlhcStart(), accelerometer full scale issue");
-  }
-  if((devp)->config->compcfg != NULL) {
-    if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_3_GA)
-      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-        if(i != 2) {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_3GA;
-        }
-        else {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_3GA;
-        }
-      }
-    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_9_GA)
-      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-        if(i != 2) {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_9GA;
-        }
-        else {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_9GA;
-        }
-      }
-    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2_5_GA)
-      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-        if(i != 2) {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2_5GA;
-        }
-        else {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2_5GA;
-        }
-      }
-    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_0_GA)
-      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-        if(i != 2) {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_0GA;
-        }
-        else {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_0GA;
-        }
-      }
-    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_7_GA)
-      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-        if(i != 2) {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_7GA;
-        }
-        else {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_7GA;
-        }
-      }
-    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5_6_GA)
-      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-        if(i != 2) {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5_6GA;
-        }
-        else {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5_6GA;
-        }
-      }
-    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8_1_GA)
-      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
-        if(i != 2) {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8_1GA;
-        }
-        else {
-          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_8_1GA;
-        }
-      }
-    else
-      osalDbgAssert(FALSE, "lsm303dlhcStart(), compass full scale issue");
-  }
-  /* This is the Compass transient recovery time */
-  osalThreadSleepMilliseconds(5);
-
-  devp->state = LSM303DLHC_READY;
-} 
-
-/**
- * @brief   Deactivates the LSM303DLHC Complex Driver peripheral.
- *
- * @param[in] devp       pointer to the @p LSM303DLHCDriver object
- *
- * @api
- */
-void lsm303dlhcStop(LSM303DLHCDriver *devp) {
-
-  osalDbgCheck(devp != NULL);
-
-  osalDbgAssert((devp->state == LSM303DLHC_STOP) || (devp->state == LSM303DLHC_READY),
-                "lsm303dlhcStop(), invalid state");
-
-#if (LSM303DLHC_USE_I2C)
-  if (devp->state == LSM303DLHC_STOP) {
-#if	LSM303DLHC_SHARED_I2C
-    i2cAcquireBus((devp)->config->i2cp);
-    i2cStart((devp)->config->i2cp,
-             (devp)->config->i2ccfg);
-#endif /* LSM303DLHC_SHARED_I2C */
-    if((devp)->config->acccfg != NULL) {
-      lsm303dlhcI2CWriteRegister(devp->config->i2cp,
-                                 LSM303DLHC_SAD_ACC,
-                                 LSM303DLHC_AD_ACC_CTRL_REG1,
-                                 LSM303DLHC_ACC_AE_DISABLED |
-                                 LSM303DLHC_ACC_ODR_PD);
-    }
-    if((devp)->config->compcfg != NULL) {
-      lsm303dlhcI2CWriteRegister(devp->config->i2cp,
-                                 LSM303DLHC_SAD_COMP,
-                                 LSM303DLHC_AD_COMP_MR_REG,
-                                 LSM303DLHC_COMP_MD_SLEEP);
-    }
-    i2cStop((devp)->config->i2cp);
-#if	LSM303DLHC_SHARED_I2C
-    i2cReleaseBus((devp)->config->i2cp);
-#endif /* LSM303DLHC_SHARED_I2C */    
-  }			  
-#endif /* LSM303DLHC_USE_I2C */
-  devp->state = LSM303DLHC_STOP;
-}
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lsm303dlhc.c

+ * @brief   LSM303DLHC MEMS interface module code.

+ *

+ * @addtogroup lsm303dlhc

+ * @{

+ */

+

+#include "hal.h"

+#include "lsm303dlhc.h"

+

+/*===========================================================================*/

+/* Driver local definitions.                                                 */

+/*===========================================================================*/

+

+#define  LSM303DLHC_ACC_SENS_2G             ((float)0.0610f)

+#define  LSM303DLHC_ACC_SENS_4G             ((float)0.1221f)

+#define  LSM303DLHC_ACC_SENS_8G             ((float)0.2442f)

+#define  LSM303DLHC_ACC_SENS_16G            ((float)0.4884f)

+

+#define  LSM303DLHC_COMP_SENS_XY_1_3GA      ((float)1100.0f)

+#define  LSM303DLHC_COMP_SENS_XY_1_9GA      ((float)855.0f)

+#define  LSM303DLHC_COMP_SENS_XY_2_5GA      ((float)670.0f)

+#define  LSM303DLHC_COMP_SENS_XY_4_0GA      ((float)450.0f)

+#define  LSM303DLHC_COMP_SENS_XY_4_7GA      ((float)400.0f)

+#define  LSM303DLHC_COMP_SENS_XY_5_6GA      ((float)330.0f)

+#define  LSM303DLHC_COMP_SENS_XY_8_1GA      ((float)230.0f)

+

+#define  LSM303DLHC_COMP_SENS_Z_1_3GA       ((float)980.0f)

+#define  LSM303DLHC_COMP_SENS_Z_1_9GA       ((float)765.0f)

+#define  LSM303DLHC_COMP_SENS_Z_2_5GA       ((float)600.0f)

+#define  LSM303DLHC_COMP_SENS_Z_4_0GA       ((float)400.0f)

+#define  LSM303DLHC_COMP_SENS_Z_4_7GA       ((float)355.0f)

+#define  LSM303DLHC_COMP_SENS_Z_5_6GA       ((float)295.0f)

+#define  LSM303DLHC_COMP_SENS_Z_8_1GA       ((float)205.0f)

+

+#define  LSM303DLHC_DI                      ((uint8_t)0xFF)

+#define  LSM303DLHC_DI_0                    ((uint8_t)0x01)

+#define  LSM303DLHC_DI_1                    ((uint8_t)0x02)

+#define  LSM303DLHC_DI_2                    ((uint8_t)0x04)

+#define  LSM303DLHC_DI_3                    ((uint8_t)0x08)

+#define  LSM303DLHC_DI_4                    ((uint8_t)0x10)

+#define  LSM303DLHC_DI_5                    ((uint8_t)0x20)

+#define  LSM303DLHC_DI_6                    ((uint8_t)0x40)

+#define  LSM303DLHC_DI_7                    ((uint8_t)0x80)

+

+#define  LSM303DLHC_AD_0                    ((uint8_t)0x01)

+#define  LSM303DLHC_AD_1                    ((uint8_t)0x02)

+#define  LSM303DLHC_AD_2                    ((uint8_t)0x04)

+#define  LSM303DLHC_AD_3                    ((uint8_t)0x08)

+#define  LSM303DLHC_AD_4                    ((uint8_t)0x10)

+#define  LSM303DLHC_AD_5                    ((uint8_t)0x20)

+#define  LSM303DLHC_AD_6                    ((uint8_t)0x40)

+#define  LSM303DLHC_RW                      ((uint8_t)0x80)

+

+#define  LSM303DLHC_AD_ACC_CTRL_REG1        ((uint8_t)0x20)

+#define  LSM303DLHC_AD_ACC_CTRL_REG2        ((uint8_t)0x21)

+#define  LSM303DLHC_AD_ACC_CTRL_REG3        ((uint8_t)0x22)

+#define  LSM303DLHC_AD_ACC_CTRL_REG4        ((uint8_t)0x23)

+#define  LSM303DLHC_AD_ACC_CTRL_REG5        ((uint8_t)0x24)

+#define  LSM303DLHC_AD_ACC_CTRL_REG6        ((uint8_t)0x25)

+#define  LSM303DLHC_AD_ACC_REFERENCE        ((uint8_t)0x26)

+#define  LSM303DLHC_AD_ACC_STATUS_REG       ((uint8_t)0x27)

+#define  LSM303DLHC_AD_ACC_OUT_X_L          ((uint8_t)0x28)

+#define  LSM303DLHC_AD_ACC_OUT_X_H          ((uint8_t)0x29)

+#define  LSM303DLHC_AD_ACC_OUT_Y_L          ((uint8_t)0x2A)

+#define  LSM303DLHC_AD_ACC_OUT_Y_H          ((uint8_t)0x2B)

+#define  LSM303DLHC_AD_ACC_OUT_Z_L          ((uint8_t)0x2C)

+#define  LSM303DLHC_AD_ACC_OUT_Z_H          ((uint8_t)0x2D)

+#define  LSM303DLHC_AD_ACC_FIFO_CTRL_REG    ((uint8_t)0x2E)

+#define  LSM303DLHC_AD_ACC_FIFO_SRC_REG     ((uint8_t)0x2F)

+#define  LSM303DLHC_AD_ACC_INT1_CFG         ((uint8_t)0x30)

+#define  LSM303DLHC_AD_ACC_INT1_SRC         ((uint8_t)0x31)

+#define  LSM303DLHC_AD_ACC_INT1_THS         ((uint8_t)0x32)

+#define  LSM303DLHC_AD_ACC_INT1_DURATION    ((uint8_t)0x33)

+#define  LSM303DLHC_AD_ACC_INT2_CFG         ((uint8_t)0x34)

+#define  LSM303DLHC_AD_ACC_INT2_SRC         ((uint8_t)0x35)

+#define  LSM303DLHC_AD_ACC_INT2_THS         ((uint8_t)0x36)

+#define  LSM303DLHC_AD_ACC_INT2_DURATION    ((uint8_t)0x37)

+#define  LSM303DLHC_AD_ACC_CLICK_CFG        ((uint8_t)0x38)

+#define  LSM303DLHC_AD_ACC_CLICK_SRC        ((uint8_t)0x39)

+#define  LSM303DLHC_AD_ACC_CLICK_THS        ((uint8_t)0x3A)

+#define  LSM303DLHC_AD_ACC_TIME_LIMIT       ((uint8_t)0x3B)

+#define  LSM303DLHC_AD_ACC_TIME_LATENCY     ((uint8_t)0x3C)

+#define  LSM303DLHC_AD_ACC_TIME_WINDOW      ((uint8_t)0x3D)

+#define  LSM303DLHC_AD_COMP_CRA_REG         ((uint8_t)0x00)

+#define  LSM303DLHC_AD_COMP_CRB_REG         ((uint8_t)0x01)

+#define  LSM303DLHC_AD_COMP_MR_REG          ((uint8_t)0x02)

+#define  LSM303DLHC_AD_COMP_OUT_X_H         ((uint8_t)0x03)

+#define  LSM303DLHC_AD_COMP_OUT_X_L         ((uint8_t)0x04)

+#define  LSM303DLHC_AD_COMP_OUT_Z_H         ((uint8_t)0x05)

+#define  LSM303DLHC_AD_COMP_OUT_Z_L         ((uint8_t)0x06)

+#define  LSM303DLHC_AD_COMP_OUT_Y_H         ((uint8_t)0x07)

+#define  LSM303DLHC_AD_COMP_OUT_Y_L         ((uint8_t)0x08)

+#define  LSM303DLHC_AD_COMP_SR_REG          ((uint8_t)0x09)

+#define  LSM303DLHC_AD_COMP_IRA_REG         ((uint8_t)0x0A)

+#define  LSM303DLHC_AD_COMP_IRB_REG         ((uint8_t)0x0B)

+#define  LSM303DLHC_AD_COMP_IRC_REG         ((uint8_t)0x0C)

+#define  LSM303DLHC_AD_COMP_TEMP_OUT_H      ((uint8_t)0x31)

+#define  LSM303DLHC_AD_COMP_TEMP_OUT_L      ((uint8_t)0x32)

+

+#define  LSM303DLHC_ACC_CTRL_REG4_FS_MASK   ((uint8_t)0x30)

+#define  LSM303DLHC_COMP_CTRL_REGB_FS_MASK  ((uint8_t)0xE0)

+

+#define  TO_G                              ((float)0.001f)

+#define  TO_SI                             ((float)0.00981f)

+

+/*===========================================================================*/

+/* Driver exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local variables and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @brief  Accelerometer Power Mode

+ */

+typedef enum {

+  LSM303DLHC_ACC_PM_NORMAL = 0x00,  /**< Normal mode enabled                */

+  LSM303DLHC_ACC_PM_LP = 0x08       /**< Low Power mode enabled             */

+} lsm303dlhc_acc_pm_t;

+

+/**

+ * @brief  Accelerometer and Compass Slave Address.

+ */

+typedef enum {

+  LSM303DLHC_SAD_ACC = 0x19,        /**< SAD for accelerometer.             */

+  LSM303DLHC_SAD_COMP = 0x1E        /**< SAD for compass.                   */

+} lsm303dlhc_sad_t;

+/*===========================================================================*/

+/* Driver local functions.                                                   */

+/*===========================================================================*/

+

+#if (LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)

+/**

+ * @brief   Reads registers value using I2C.

+ * @pre     The I2C interface must be initialized and the driver started.

+ * @note    IF_ADD_INC bit must be 1 in CTRL_REG8

+ *

+ * @param[in] i2cp       pointer to the I2C interface

+ * @param[in] sad        slave address without R bit

+ * @param[in] reg        first sub-register address

+ * @return               the read value.

+ */

+uint8_t lsm303dlhcI2CReadRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad, uint8_t reg,

+                               msg_t* msgp) {

+  msg_t msg;

+#if defined(STM32F103_MCUCONF)

+  uint8_t rxbuf[2];

+  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,

+                                   TIME_INFINITE);

+  if(msgp != NULL){

+    *msgp = msg;

+  }

+  return rxbuf[0];

+#else

+  uint8_t txbuf, rxbuf;

+  txbuf = reg;

+  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, &rxbuf, 1,

+                                        TIME_INFINITE);

+  if(msgp != NULL){

+    *msgp = msg;

+  }

+  return rxbuf;

+#endif

+}

+

+/**

+ * @brief   Writes a value into a register using I2C.

+ * @pre     The I2C interface must be initialized and the driver started.

+ *

+ * @param[in] i2cp       pointer to the I2C interface

+ * @param[in] sad        slave address without R bit

+ * @param[in] sub        sub-register address

+ * @param[in] value      the value to be written

+ * @return               the operation status.

+ */

+msg_t lsm303dlhcI2CWriteRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad, uint8_t reg,

+                              uint8_t value) {

+  uint8_t rxbuf;

+  uint8_t txbuf[2];

+  if(sad == LSM303DLHC_SAD_ACC) {

+    switch (reg) {

+    default:

+      /* Reserved register must not be written, according to the datasheet

+       * this could permanently damage the device.

+       */

+      osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), reserved register");

+    case LSM303DLHC_AD_ACC_STATUS_REG:

+    case LSM303DLHC_AD_ACC_OUT_X_L:

+    case LSM303DLHC_AD_ACC_OUT_X_H:

+    case LSM303DLHC_AD_ACC_OUT_Y_L:

+    case LSM303DLHC_AD_ACC_OUT_Y_H:

+    case LSM303DLHC_AD_ACC_OUT_Z_L:

+    case LSM303DLHC_AD_ACC_OUT_Z_H:

+    case LSM303DLHC_AD_ACC_FIFO_SRC_REG:

+    case LSM303DLHC_AD_ACC_INT1_SRC:

+    case LSM303DLHC_AD_ACC_INT2_SRC:

+    /* Read only registers cannot be written, the command is ignored.*/

+      return MSG_RESET;

+    case LSM303DLHC_AD_ACC_CTRL_REG1:

+    case LSM303DLHC_AD_ACC_CTRL_REG2:

+    case LSM303DLHC_AD_ACC_CTRL_REG3:

+    case LSM303DLHC_AD_ACC_CTRL_REG4:

+    case LSM303DLHC_AD_ACC_CTRL_REG5:

+    case LSM303DLHC_AD_ACC_CTRL_REG6:

+    case LSM303DLHC_AD_ACC_REFERENCE:

+    case LSM303DLHC_AD_ACC_FIFO_CTRL_REG:

+    case LSM303DLHC_AD_ACC_INT1_CFG:

+    case LSM303DLHC_AD_ACC_INT1_THS:

+    case LSM303DLHC_AD_ACC_INT1_DURATION:

+    case LSM303DLHC_AD_ACC_INT2_CFG:

+    case LSM303DLHC_AD_ACC_INT2_THS:

+    case LSM303DLHC_AD_ACC_INT2_DURATION:

+    case LSM303DLHC_AD_ACC_CLICK_CFG:

+    case LSM303DLHC_AD_ACC_CLICK_SRC:

+    case LSM303DLHC_AD_ACC_CLICK_THS:

+    case LSM303DLHC_AD_ACC_TIME_LIMIT:

+    case LSM303DLHC_AD_ACC_TIME_LATENCY:

+    case LSM303DLHC_AD_ACC_TIME_WINDOW:

+      txbuf[0] = reg;

+      txbuf[1] = value;

+      return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);

+      break;

+    }

+  }

+  if(sad == LSM303DLHC_SAD_COMP) {

+    switch (reg) {

+    default:

+      /* Reserved register must not be written, according to the datasheet

+       * this could permanently damage the device.

+       */

+      osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), reserved register");

+    case LSM303DLHC_AD_COMP_OUT_X_H:

+    case LSM303DLHC_AD_COMP_OUT_X_L:

+    case LSM303DLHC_AD_COMP_OUT_Z_H:

+    case LSM303DLHC_AD_COMP_OUT_Z_L:

+    case LSM303DLHC_AD_COMP_OUT_Y_H:

+    case LSM303DLHC_AD_COMP_OUT_Y_L:

+    case LSM303DLHC_AD_COMP_SR_REG:

+    case LSM303DLHC_AD_COMP_IRA_REG:

+    case LSM303DLHC_AD_COMP_IRB_REG:

+    case LSM303DLHC_AD_COMP_IRC_REG:

+    case LSM303DLHC_AD_COMP_TEMP_OUT_H:

+    case LSM303DLHC_AD_COMP_TEMP_OUT_L:

+    /* Read only registers cannot be written, the command is ignored.*/

+      return MSG_RESET;

+    case LSM303DLHC_AD_COMP_CRA_REG:

+    case LSM303DLHC_AD_COMP_CRB_REG:

+    case LSM303DLHC_AD_COMP_MR_REG:

+      txbuf[0] = reg;

+      txbuf[1] = value;

+      return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);

+      break;

+    }

+  }

+  osalDbgAssert(FALSE, "lsm303dlhcWriteRegisterI2C(), wrong SAD");

+  return MSG_RESET;

+}

+#endif /* LSM303DLHC_USE_I2C */

+

+/*

+ * Interface implementation.

+ */

+static size_t acc_get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return LSM303DLHC_ACC_NUMBER_OF_AXES;

+}

+

+static size_t comp_get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return LSM303DLHC_COMP_NUMBER_OF_AXES;

+}

+

+static size_t sens_get_axes_number(void *ip) {

+  size_t size = 0;

+  

+  osalDbgCheck(ip != NULL);

+  if (((LSM303DLHCDriver *)ip)->config->acccfg != NULL)

+	size += acc_get_axes_number(ip);

+  if (((LSM303DLHCDriver *)ip)->config->compcfg != NULL)

+    size += comp_get_axes_number(ip);  

+  return size;

+}

+

+static msg_t acc_read_raw(void *ip, int32_t axes[]) {

+  int16_t tmp;

+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&

+              (((LSM303DLHCDriver *)ip)->config->acccfg != NULL));

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),

+              "acc_read_raw(), invalid state");

+

+#if LSM303DLHC_USE_I2C

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->config->i2cp->state == I2C_READY),

+                "acc_read_raw(), channel not ready");

+#if LSM303DLHC_SHARED_I2C

+  i2cAcquireBus(((LSM303DLHCDriver *)ip)->config->i2cp);

+  i2cStart(((LSM303DLHCDriver *)ip)->config->i2cp,

+           ((LSM303DLHCDriver *)ip)->config->i2ccfg);

+#endif /* LSM303DLHC_SHARED_I2C */

+  if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_X){

+    tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                 LSM303DLHC_SAD_ACC,

+                                 LSM303DLHC_AD_ACC_OUT_X_L, NULL);

+    tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                  LSM303DLHC_SAD_ACC,

+                                  LSM303DLHC_AD_ACC_OUT_X_H, NULL) << 8;

+    axes[0] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[0];

+  }

+  if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_Y){

+    tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                 LSM303DLHC_SAD_ACC,

+                                 LSM303DLHC_AD_ACC_OUT_Y_L, NULL);

+    tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                  LSM303DLHC_SAD_ACC,

+                                  LSM303DLHC_AD_ACC_OUT_Y_H, NULL) << 8;

+    axes[1] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[1];

+  }

+  if(((LSM303DLHCDriver *)ip)->config->acccfg->axesenabling & LSM303DLHC_ACC_AE_Z){

+    tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                 LSM303DLHC_SAD_ACC,

+                                 LSM303DLHC_AD_ACC_OUT_Z_L, NULL);

+    tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                  LSM303DLHC_SAD_ACC,

+                                  LSM303DLHC_AD_ACC_OUT_Z_H, NULL) << 8;

+    axes[2] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->accbias[2];

+  }

+#if LSM303DLHC_SHARED_I2C

+  i2cReleaseBus(((LSM303DLHCDriver *)ip)->config->i2cp);

+#endif /* LSM303DLHC_SHARED_I2C */

+#endif /* LSM303DLHC_USE_I2C */

+  return MSG_OK;

+}

+

+static msg_t comp_read_raw(void *ip, int32_t axes[]) {

+  int16_t tmp;

+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&

+              (((LSM303DLHCDriver *)ip)->config->compcfg != NULL));

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),

+              "comp_read_raw(), invalid state");

+

+#if LSM303DLHC_USE_I2C

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->config->i2cp->state == I2C_READY),

+                "comp_read_raw(), channel not ready");

+#if LSM303DLHC_SHARED_I2C

+  i2cAcquireBus(((LSM303DLHCDriver *)ip)->config->i2cp);

+  i2cStart(((LSM303DLHCDriver *)ip)->config->i2cp,

+           ((LSM303DLHCDriver *)ip)->config->i2ccfg);

+#endif /* LSM303DLHC_SHARED_I2C */

+  tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                               LSM303DLHC_SAD_COMP,

+                               LSM303DLHC_AD_COMP_OUT_X_L, NULL);

+  tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                LSM303DLHC_SAD_COMP,

+                                LSM303DLHC_AD_COMP_OUT_X_H, NULL) << 8;

+  axes[0] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[0];

+

+  tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                               LSM303DLHC_SAD_COMP,

+                               LSM303DLHC_AD_COMP_OUT_Y_L, NULL);

+  tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                LSM303DLHC_SAD_COMP,

+                                LSM303DLHC_AD_COMP_OUT_Y_H, NULL) << 8;

+  axes[1] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[1];

+

+  tmp = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                               LSM303DLHC_SAD_COMP,

+                               LSM303DLHC_AD_COMP_OUT_Z_L, NULL);

+  tmp += lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,

+                                LSM303DLHC_SAD_COMP,

+                                LSM303DLHC_AD_COMP_OUT_Z_H, NULL) << 8;

+  axes[2] = (int32_t)tmp - ((LSM303DLHCDriver *)ip)->compbias[2];

+#if LSM303DLHC_SHARED_I2C

+  i2cReleaseBus(((LSM303DLHCDriver *)ip)->config->i2cp);

+#endif /* LSM303DLHC_SHARED_I2C */

+#endif /* LSM303DLHC_USE_I2C */ 

+  return MSG_OK;

+}

+

+static msg_t sens_read_raw(void *ip, int32_t axes[]) {

+  int32_t* bp = axes;

+  msg_t msg;

+  if (((LSM303DLHCDriver *)ip)->config->acccfg != NULL) {

+	msg = acc_read_raw(ip, bp);

+	if(msg != MSG_OK)

+	  return msg;

+	bp += LSM303DLHC_ACC_NUMBER_OF_AXES;

+  }

+  if (((LSM303DLHCDriver *)ip)->config->compcfg != NULL) {

+    msg = comp_read_raw(ip, bp);

+  }

+  return msg;

+}

+	

+static msg_t acc_read_cooked(void *ip, float axes[]) {

+  uint32_t i;

+  int32_t raw[LSM303DLHC_ACC_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&

+              (((LSM303DLHCDriver *)ip)->config->acccfg != NULL));

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),

+              "acc_read_cooked(), invalid state");

+

+  msg = acc_read_raw(ip, raw);

+  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES ; i++){

+    axes[i] = raw[i] * ((LSM303DLHCDriver *)ip)->accsensitivity[i];

+    if(((LSM303DLHCDriver *)ip)->config->acccfg->unit == LSM303DLHC_ACC_UNIT_G){

+      axes[i] *= TO_G;

+    }

+    else if(((LSM303DLHCDriver *)ip)->config->acccfg->unit == LSM303DLHC_ACC_UNIT_SI){

+      axes[i] *= TO_SI;

+    }

+  }

+  return msg;

+}

+

+static msg_t comp_read_cooked(void *ip, float axes[]) {

+  uint32_t i;

+  int32_t raw[LSM303DLHC_COMP_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&

+              (((LSM303DLHCDriver *)ip)->config->compcfg != NULL));

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),

+              "comp_read_cooked(), invalid state");

+

+  msg = comp_read_raw(ip, raw);

+  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES ; i++){

+    axes[i] = raw[i] / ((LSM303DLHCDriver *)ip)->compsensitivity[i];

+  }

+  return msg;

+}

+

+static msg_t sens_read_cooked(void *ip, float axes[]) {

+  float* bp = axes;

+  msg_t msg;

+  if (((LSM303DLHCDriver *)ip)->config->acccfg != NULL) {

+	msg = acc_read_cooked(ip, bp);

+	if(msg != MSG_OK)

+	  return msg;

+	bp += LSM303DLHC_ACC_NUMBER_OF_AXES;

+  }

+  if (((LSM303DLHCDriver *)ip)->config->compcfg != NULL) {

+    msg = comp_read_cooked(ip, bp);

+  }

+  return msg;

+}

+

+static msg_t acc_set_bias(void *ip, int32_t *bp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (bp !=NULL));

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY) ||

+                (((LSM303DLHCDriver *)ip)->state == LSM303DLHC_STOP),

+                "acc_set_bias(), invalid state");

+  

+  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {

+    ((LSM303DLHCDriver *)ip)->accbias[i] = bp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t comp_set_bias(void *ip, int32_t *bp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (bp !=NULL));

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY) ||

+                (((LSM303DLHCDriver *)ip)->state == LSM303DLHC_STOP),

+                "comp_set_bias(), invalid state");

+  

+  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+    ((LSM303DLHCDriver *)ip)->compbias[i] = bp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t acc_reset_bias(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY) ||

+                (((LSM303DLHCDriver *)ip)->state == LSM303DLHC_STOP),

+              "acc_reset_bias(), invalid state");

+

+  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+    ((LSM303DLHCDriver *)ip)->accbias[i] = 0;

+  return MSG_OK;

+}

+

+static msg_t comp_reset_bias(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY) ||

+                (((LSM303DLHCDriver *)ip)->state == LSM303DLHC_STOP),

+              "comp_reset_bias(), invalid state");

+

+  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)

+    ((LSM303DLHCDriver *)ip)->compbias[i] = 0;

+  return MSG_OK;

+}

+

+static msg_t acc_set_sensivity(void *ip, float *sp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (sp !=NULL));

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),

+                "acc_set_sensivity(), invalid state");

+  

+  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {

+    ((LSM303DLHCDriver *)ip)->accsensitivity[i] = sp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t comp_set_sensivity(void *ip, float *sp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (sp !=NULL));

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),

+                "comp_set_sensivity(), invalid state");

+  

+  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+    ((LSM303DLHCDriver *)ip)->compsensitivity[i] = sp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t acc_reset_sensivity(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),

+                "acc_reset_sensivity(), invalid state");

+

+  if(((LSM303DLHCDriver *)ip)->config->acccfg->fullscale == LSM303DLHC_ACC_FS_2G)

+    for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+      ((LSM303DLHCDriver *)ip)->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G;

+  else if(((LSM303DLHCDriver *)ip)->config->acccfg->fullscale == LSM303DLHC_ACC_FS_4G)

+	for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+      ((LSM303DLHCDriver *)ip)->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G;

+  else if(((LSM303DLHCDriver *)ip)->config->acccfg->fullscale == LSM303DLHC_ACC_FS_8G)

+	for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+      ((LSM303DLHCDriver *)ip)->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G;

+  else if(((LSM303DLHCDriver *)ip)->config->acccfg->fullscale == LSM303DLHC_ACC_FS_16G)

+	for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+      ((LSM303DLHCDriver *)ip)->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G;

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");

+    return MSG_RESET;

+  }

+  return MSG_OK;

+}

+

+static msg_t comp_reset_sensivity(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM303DLHCDriver *)ip)->state == LSM303DLHC_READY),

+                "comp_reset_sensivity(), invalid state");

+

+  if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_3_GA)

+    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_3GA;

+      }

+      else {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_3GA;

+      }

+    }

+  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_9_GA)

+    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_9GA;

+      }

+      else {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_9GA;

+      }

+    }

+  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2_5_GA)

+    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2_5GA;

+      }

+      else {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2_5GA;

+      }

+    }

+  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_0_GA)

+    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_0GA;

+      }

+      else {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_0GA;

+      }

+    }

+  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_7_GA)

+    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_7GA;

+      }

+      else {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_7GA;

+      }

+    }

+  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5_6_GA)

+    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5_6GA;

+      }

+      else {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5_6GA;

+      }

+    }

+  else if(((LSM303DLHCDriver *)ip)->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8_1_GA)

+    for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+      if(i != 2) {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8_1GA;

+      }

+      else {

+        ((LSM303DLHCDriver *)ip)->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_8_1GA;

+      }

+    }

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), compass full scale issue");

+    return MSG_RESET;

+  }

+  return MSG_OK;

+}

+

+static const struct BaseSensorVMT vmt_basesensor = {

+  sens_get_axes_number, sens_read_raw, sens_read_cooked

+};

+

+static const struct BaseCompassVMT vmt_basecompass = {

+  comp_get_axes_number, comp_read_raw, comp_read_cooked,

+  comp_set_bias, comp_reset_bias, comp_set_sensivity, comp_reset_sensivity

+};

+

+static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {

+  acc_get_axes_number, acc_read_raw, acc_read_cooked,

+  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity

+};

+

+static const struct LSM303DLHCACCVMT vmt_lsm303dlhcacc = {

+  acc_get_axes_number, acc_read_raw, acc_read_cooked,

+  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity

+};

+

+static const struct LSM303DLHCCOMPVMT vmt_lsm303dlhccomp = {

+  comp_get_axes_number, comp_read_raw, comp_read_cooked,

+  comp_set_bias, comp_reset_bias, comp_set_sensivity, comp_reset_sensivity

+};

+

+/*===========================================================================*/

+/* Driver exported functions.                                                */

+/*===========================================================================*/

+

+/**

+ * @brief   Initializes an instance.

+ *

+ * @param[out] devp     pointer to the @p LSM303DLHCDriver object

+ *

+ * @init

+ */

+void lsm303dlhcObjectInit(LSM303DLHCDriver *devp) {

+  uint32_t i;

+  devp->vmt_basesensor = &vmt_basesensor;

+  devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;

+  devp->vmt_basecompass = &vmt_basecompass;

+  devp->vmt_lsm303dlhcacc = &vmt_lsm303dlhcacc;

+  devp->vmt_lsm303dlhccomp = &vmt_lsm303dlhccomp;

+  devp->config = NULL;

+  for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+    devp->accbias[i] = 0;

+  for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)

+    devp->compbias[i] = 0;

+  devp->state  = LSM303DLHC_STOP;

+}

+

+/**

+ * @brief   Configures and activates LSM303DLHC Complex Driver peripheral.

+ *

+ * @param[in] devp      pointer to the @p LSM303DLHCDriver object

+ * @param[in] config    pointer to the @p LSM303DLHCConfig object

+ *

+ * @api

+ */

+void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {

+  uint32_t i;

+  osalDbgCheck((devp != NULL) && (config != NULL));

+

+  osalDbgAssert((devp->state == LSM303DLHC_STOP) || (devp->state == LSM303DLHC_READY),

+              "lsm303dlhcStart(), invalid state");			  

+

+  devp->config = config;

+

+#if LSM303DLHC_USE_I2C

+#if	LSM303DLHC_SHARED_I2C

+  i2cAcquireBus((devp)->config->i2cp);

+#endif /* LSM303DLHC_SHARED_I2C */

+  i2cStart((devp)->config->i2cp,

+           (devp)->config->i2ccfg);

+  if((devp)->config->acccfg != NULL) {

+    lsm303dlhcI2CWriteRegister(devp->config->i2cp,

+                               LSM303DLHC_SAD_ACC,

+                               LSM303DLHC_AD_ACC_CTRL_REG1,

+                               devp->config->acccfg->axesenabling |

+                               devp->config->acccfg->outdatarate |

+                               devp->config->acccfg->lowpower);

+    lsm303dlhcI2CWriteRegister(devp->config->i2cp,

+                               LSM303DLHC_SAD_ACC,

+                               LSM303DLHC_AD_ACC_CTRL_REG4,

+                               devp->config->acccfg->endianess |

+                               devp->config->acccfg->fullscale |

+                               devp->config->acccfg->blockdataupdate |

+                               devp->config->acccfg->highresmode);

+  }

+  if((devp)->config->compcfg != NULL) {

+    lsm303dlhcI2CWriteRegister(devp->config->i2cp,

+                               LSM303DLHC_SAD_COMP,

+                               LSM303DLHC_AD_COMP_CRA_REG,

+                               devp->config->compcfg->outputdatarate);

+    lsm303dlhcI2CWriteRegister(devp->config->i2cp,

+                               LSM303DLHC_SAD_COMP,

+                               LSM303DLHC_AD_COMP_CRB_REG,

+                               devp->config->compcfg->fullscale);

+    lsm303dlhcI2CWriteRegister(devp->config->i2cp,

+                               LSM303DLHC_SAD_COMP,

+                               LSM303DLHC_AD_COMP_MR_REG,

+                               devp->config->compcfg->mode);

+  }

+#if	LSM303DLHC_SHARED_I2C

+  i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM303DLHC_SHARED_I2C */  

+#endif /* LSM303DLHC_USE_I2C */

+  /* Storing sensitivity information according to full scale value */

+  if((devp)->config->acccfg != NULL) {

+    if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_2G)

+      for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+        devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G;

+    else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_4G)

+      for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+        devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G;

+    else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_8G)

+      for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+        devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G;

+    else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_16G)

+      for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

+        devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G;

+    else

+      osalDbgAssert(FALSE, "lsm303dlhcStart(), accelerometer full scale issue");

+  }

+  if((devp)->config->compcfg != NULL) {

+    if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_3_GA)

+      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_3GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_3GA;

+        }

+      }

+    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1_9_GA)

+      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1_9GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1_9GA;

+        }

+      }

+    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2_5_GA)

+      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2_5GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2_5GA;

+        }

+      }

+    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_0_GA)

+      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_0GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_0GA;

+        }

+      }

+    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4_7_GA)

+      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4_7GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4_7GA;

+        }

+      }

+    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5_6_GA)

+      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5_6GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5_6GA;

+        }

+      }

+    else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8_1_GA)

+      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {

+        if(i != 2) {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8_1GA;

+        }

+        else {

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_8_1GA;

+        }

+      }

+    else

+      osalDbgAssert(FALSE, "lsm303dlhcStart(), compass full scale issue");

+  }

+  /* This is the Compass transient recovery time */

+  osalThreadSleepMilliseconds(5);

+

+  devp->state = LSM303DLHC_READY;

+} 

+

+/**

+ * @brief   Deactivates the LSM303DLHC Complex Driver peripheral.

+ *

+ * @param[in] devp       pointer to the @p LSM303DLHCDriver object

+ *

+ * @api

+ */

+void lsm303dlhcStop(LSM303DLHCDriver *devp) {

+

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == LSM303DLHC_STOP) || (devp->state == LSM303DLHC_READY),

+                "lsm303dlhcStop(), invalid state");

+

+#if (LSM303DLHC_USE_I2C)

+  if (devp->state == LSM303DLHC_STOP) {

+#if	LSM303DLHC_SHARED_I2C

+    i2cAcquireBus((devp)->config->i2cp);

+    i2cStart((devp)->config->i2cp,

+             (devp)->config->i2ccfg);

+#endif /* LSM303DLHC_SHARED_I2C */

+    if((devp)->config->acccfg != NULL) {

+      lsm303dlhcI2CWriteRegister(devp->config->i2cp,

+                                 LSM303DLHC_SAD_ACC,

+                                 LSM303DLHC_AD_ACC_CTRL_REG1,

+                                 LSM303DLHC_ACC_AE_DISABLED |

+                                 LSM303DLHC_ACC_ODR_PD);

+    }

+    if((devp)->config->compcfg != NULL) {

+      lsm303dlhcI2CWriteRegister(devp->config->i2cp,

+                                 LSM303DLHC_SAD_COMP,

+                                 LSM303DLHC_AD_COMP_MR_REG,

+                                 LSM303DLHC_COMP_MD_SLEEP);

+    }

+    i2cStop((devp)->config->i2cp);

+#if	LSM303DLHC_SHARED_I2C

+    i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM303DLHC_SHARED_I2C */    

+  }			  

+#endif /* LSM303DLHC_USE_I2C */

+  devp->state = LSM303DLHC_STOP;

+}

+/** @} */

diff --git a/os/ex/ST/lsm303dlhc.h b/os/ex/ST/lsm303dlhc.h
index 2fe8d70b3..17fd55ec2 100644
--- a/os/ex/ST/lsm303dlhc.h
+++ b/os/ex/ST/lsm303dlhc.h
@@ -1,441 +1,441 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-*/
-
-/**
- * @file    lsm303dlhc.h
- * @brief   LSM303DLHC MEMS interface module header.
- *
- * @{
- */
-#ifndef _LSM303DLHC_H_
-#define _LSM303DLHC_H_
-
-#include "hal_accelerometer.h"
-#include "hal_compass.h"
-
-/*===========================================================================*/
-/* Driver constants.                                                         */
-/*===========================================================================*/
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem number of axes.
- */
-#define  LSM303DLHC_ACC_NUMBER_OF_AXES         ((size_t)   3U)
-
-/**
- * @brief   LSM303DLHC compass subsystem number of axes.
- */
-#define  LSM303DLHC_COMP_NUMBER_OF_AXES        ((size_t)   3U)
-
-/*===========================================================================*/
-/* Driver pre-compile time settings.                                         */
-/*===========================================================================*/
-
-/**
- * @name    Configuration options
- * @{
- */
-/**
- * @brief   LSM303DLHC SPI interface selector.
- * @details If set to @p TRUE the support for SPI is included.
- * @note    The default is @p FALSE.
- */
-#if !defined(LSM303DLHC_USE_SPI) || defined(__DOXYGEN__)
-#define LSM303DLHC_USE_SPI                     FALSE
-#endif
-
-/**
- * @brief   LSM303DLHC I2C interface selector.
- * @details If set to @p TRUE the support for I2C is included.
- * @note    The default is @p TRUE.
- */
-#if !defined(LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)
-#define LSM303DLHC_USE_I2C                     TRUE
-#endif
-
-/**
- * @brief   LSM303DLHC shared I2C switch.
- * @details If set to @p TRUE the device acquires I2C bus ownership
- *          on each transaction.
- * @note    The default is @p FALSE. Requires I2C_USE_MUTUAL_EXCLUSION.
- */
-#if !defined(LSM303DLHC_SHARED_I2C) || defined(__DOXYGEN__)
-#define LSM303DLHC_SHARED_I2C                  FALSE
-#endif
-/** @} */
-
-/*===========================================================================*/
-/* Derived constants and error checks.                                       */
-/*===========================================================================*/
-
-#if LSM303DLHC_USE_SPI && LSM303DLHC_USE_I2C
-#error "LSM303DLHC_USE_SPI and LSM303DLHC_USE_I2C cannot be both true"
-#endif
-
-#if LSM303DLHC_USE_SPI && !HAL_USE_SPI
-#error "LSM303DLHC_USE_SPI requires HAL_USE_SPI"
-#endif
-
-#if LSM303DLHC_USE_I2C && !HAL_USE_I2C
-#error "LSM303DLHC_USE_I2C requires HAL_USE_I2C"
-#endif
-
-#if LSM303DLHC_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION
-#error "LSM303DLHC_SHARED_I2C requires I2C_USE_MUTUAL_EXCLUSION"
-#endif
-
-/*===========================================================================*/
-/* Driver data structures and types.                                         */
-/*===========================================================================*/
-
-/**
- * @name    LSM303DLHC accelerometer subsystem data structures and types.
- * @{
- */
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem full scale.
- */
-typedef enum {
-  LSM303DLHC_ACC_FS_2G   = 0x00,    /**< Full scale ±2g.                    */
-  LSM303DLHC_ACC_FS_4G   = 0x10,    /**< Full scale ±4g.                    */
-  LSM303DLHC_ACC_FS_8G   = 0x20,    /**< Full scale ±8g.                    */
-  LSM303DLHC_ACC_FS_16G  = 0x30     /**< Full scale ±16g.                   */
-} lsm303dlhc_acc_fs_t;
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem output data rate.
- */
-typedef enum {
-  LSM303DLHC_ACC_ODR_PD = 0x00,     /**< Power down                         */
-  LSM303DLHC_ACC_ODR_1Hz = 0x10,    /**< ODR 1 Hz                           */
-  LSM303DLHC_ACC_ODR_10Hz = 0x20,   /**< ODR 10 Hz                          */
-  LSM303DLHC_ACC_ODR_25Hz = 0x30,   /**< ODR 25 Hz                          */
-  LSM303DLHC_ACC_ODR_50Hz = 0x40,   /**< ODR 50 Hz                          */
-  LSM303DLHC_ACC_ODR_100Hz = 0x50,  /**< ODR 100 Hz                         */
-  LSM303DLHC_ACC_ODR_200Hz = 0x60,  /**< ODR 200 Hz                         */
-  LSM303DLHC_ACC_ODR_400Hz = 0x70,  /**< ODR 400 Hz                         */
-  LSM303DLHC_ACC_ODR_1620Hz = 0x80, /**< ODR 1620 Hz (LP only)              */
-  LSM303DLHC_ACC_ODR_1344Hz = 0x90  /**< ODR 1344 Hz or 5376 Hz in LP       */
-} lsm303dlhc_acc_odr_t;
-
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem axes enabling.
- */
-typedef enum {
-  LSM303DLHC_ACC_AE_DISABLED = 0x00,/**< All axes disabled.                 */
-  LSM303DLHC_ACC_AE_X = 0x01,       /**< Only X-axis enabled.               */
-  LSM303DLHC_ACC_AE_Y = 0x02,       /**< Only Y-axis enabled.               */
-  LSM303DLHC_ACC_AE_XY = 0x03,      /**< X and Y axes enabled.              */
-  LSM303DLHC_ACC_AE_Z = 0x04,       /**< Only Z-axis enabled.               */
-  LSM303DLHC_ACC_AE_XZ = 0x05,      /**< X and Z axes enabled.              */
-  LSM303DLHC_ACC_AE_YZ = 0x06,      /**< Y and Z axes enabled.              */
-  LSM303DLHC_ACC_AE_XYZ = 0x07      /**< All axes enabled.                  */
-} lsm303dlhc_acc_ae_t;
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem low power mode.
- */
-typedef enum {
-  LSM303DLHC_ACC_LP_DISABLED = 0x00,/**< Low power mode disabled.           */
-  LSM303DLHC_ACC_LP_ENABLED = 0x40  /**< Low power mode enabled.            */
-} lsm303dlhc_acc_lp_t;
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem high resolution mode.
- */
-typedef enum {
-  LSM303DLHC_ACC_HR_DISABLED = 0x00,/**< High resolution mode disabled.     */
-  LSM303DLHC_ACC_HR_ENABLED = 0x08  /**< High resolution mode enabled.      */
-} lsm303dlhc_acc_hr_t;
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem block data update.
- */
-typedef enum {
-  LSM303DLHC_ACC_BDU_CONT = 0x00,   /**< Continuous update                  */
-  LSM303DLHC_ACC_BDU_BLOCK = 0x80   /**< Update blocked                     */
-} lsm303dlhc_acc_bdu_t;
-
-/**
- * @brief  LSM303DLHC accelerometer endianness.
- */
-typedef enum {
-  LSM303DLHC_ACC_END_LITTLE = 0x00, /**< Little Endian                      */
-  LSM303DLHC_ACC_END_BIG = 0x40     /**< Big Endian                         */
-} lsm303dlhc_acc_end_t;
-
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem unit.
- */
-typedef enum {
-  LSM303DLHC_ACC_UNIT_G = 0x00,     /**< Cooked data in g.                  */
-  LSM303DLHC_ACC_UNIT_MG = 0x01,    /**< Cooked data in mg.                 */
-  LSM303DLHC_ACC_UNIT_SI = 0x02,    /**< Cooked data in m/s^2.              */
-} lsm303dlhc_acc_unit_t;
-
-/**
- * @brief   LSM303DLHC accelerometer subsystem configuration structure.
- */
-typedef struct {
-  /**
-   * @brief   LSM303DLHC accelerometer subsystem full scale.
-   */
-  lsm303dlhc_acc_fs_t          fullscale;
-  /**
-   * @brief   LSM303DLHC accelerometer subsystem output data rate.
-   */
-  lsm303dlhc_acc_odr_t         outdatarate;
-  /**
-   * @brief   LSM303DLHC accelerometer subsystem axes enabling.
-   */
-  lsm303dlhc_acc_ae_t          axesenabling;
-  /**
-   * @brief   LSM303DLHC accelerometer subsystem low power mode.
-   */
-  lsm303dlhc_acc_lp_t          lowpower;
-  /**
-   * @brief   LSM303DLHC accelerometer subsystem high resolution mode.
-   */
-  lsm303dlhc_acc_hr_t          highresmode;
-  /**
-   * @brief   LSM303DLHC accelerometer subsystem block data update.
-   */
-  lsm303dlhc_acc_bdu_t         blockdataupdate;
-  /**
-   * @brief  LSM303DLHC accelerometer endianness.
-   */
-  lsm303dlhc_acc_end_t         endianess;
-  /**
-   * @brief   LSM303DLHC accelerometer subsystem unit.
-   */
-  lsm303dlhc_acc_unit_t        unit;
-} LSM303DLHCAccConfig;
-/** @} */
-
-/**
- * @name    LSM303DLHC compass subsystem data structures and types.
- * @{
- */
-/**
- * @brief   LSM303DLHC compass subsystem full scale.
- */
-typedef enum {
-  LSM303DLHC_COMP_FS_1_3_GA = 0x20, /**< Full scale ±1.3 Gauss              */
-  LSM303DLHC_COMP_FS_1_9_GA = 0x40, /**< Full scale ±1.9 Gauss              */
-  LSM303DLHC_COMP_FS_2_5_GA = 0x60, /**< Full scale ±2.5 Gauss              */
-  LSM303DLHC_COMP_FS_4_0_GA = 0x80, /**< Full scale ±4.0 Gauss              */
-  LSM303DLHC_COMP_FS_4_7_GA = 0xA0, /**< Full scale ±4.7 Gauss              */
-  LSM303DLHC_COMP_FS_5_6_GA = 0xC0, /**< Full scale ±5.6 Gauss              */
-  LSM303DLHC_COMP_FS_8_1_GA = 0xE0  /**< Full scale ±8.1 Gauss              */
-} lsm303dlhc_comp_fs_t;
-
-/**
- * @brief   LSM303DLHC compass subsystem output data rate.
- */
-typedef enum {
-  LSM303DLHC_COMP_ODR_0_75HZ = 0x00,/**< ODR 0.75 Hz                        */
-  LSM303DLHC_COMP_ODR_1_5HZ = 0x04, /**< ODR 1.5 Hz                         */
-  LSM303DLHC_COMP_ODR_3_0HZ = 0x08, /**< ODR 3 Hz                           */
-  LSM303DLHC_COMP_ODR_7_5HZ = 0x0C, /**< ODR 7.5 Hz                         */
-  LSM303DLHC_COMP_ODR_15HZ = 0x10,  /**< ODR 15 Hz                          */
-  LSM303DLHC_COMP_ODR_30HZ = 0x14,  /**< ODR 30 Hz                          */
-  LSM303DLHC_COMP_ODR_75HZ = 0x18,  /**< ODR 75 Hz                          */
-  LSM303DLHC_COMP_ODR_220HZ = 0x1C  /**< ODR 220 Hz                         */
-} lsm303dlhc_comp_odr_t;
-
-/**
- * @brief   LSM303DLHC compass subsystem working mode.
- */
-typedef enum {
-  LSM303DLHC_COMP_MD_CONT = 0x00,   /**< Continuous-Conversion Mode         */
-  LSM303DLHC_COMP_MD_BLOCK = 0x01,  /**< Single-Conversion Mode             */
-  LSM303DLHC_COMP_MD_SLEEP = 0x02   /**< Sleep Mode                         */
-} lsm303dlhc_comp_md_t;
-
-/**
- * @brief   LSM303DLHC compass subsystem configuration structure.
- */
-typedef struct {
-  /**
-   * @brief   LSM303DLHC compass subsystem full scale.
-   */
-  lsm303dlhc_comp_fs_t         fullscale;
-  /**
-   * @brief   LSM303DLHC compass subsystem output data rate.
-   */
-  lsm303dlhc_comp_odr_t        outputdatarate;
-  /**
-   * @brief   LSM303DLHC compass subsystem working mode.
-   */
-  lsm303dlhc_comp_md_t         mode;
-} LSM303DLHCCompConfig;
-/** @} */
-
-/**
- * @name    LSM303DLHC main system data structures and types.
- * @{
- */
-
-/**
- * @brief   Driver state machine possible states.
- */
-typedef enum {
-  LSM303DLHC_UNINIT = 0,            /**< Not initialized.                   */
-  LSM303DLHC_STOP = 1,              /**< Stopped.                           */
-  LSM303DLHC_READY = 2,             /**< Ready.                             */
-} lsm303dlhc_state_t;
-
-/**
- * @brief   LSM303DLHC configuration structure.
- */
-typedef struct {
-#if (LSM303DLHC_USE_SPI) || defined(__DOXYGEN__)
-  /**
-   * @brief SPI driver associated to this LSM303DLHC.
-   */
-  SPIDriver                 *spip;
-  /**
-   * @brief SPI configuration associated to this LSM303DLHC accelerometer
-   *        subsystem.
-   */
-  const SPIConfig           *accspicfg;
-  /**
-   * @brief SPI configuration associated to this LSM303DLHC compass
-   *        subsystem.
-   */
-  const SPIConfig           *compspicfg;
-#endif /* LSM303DLHC_USE_SPI */
-#if (LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)
-  /**
-   * @brief I2C driver associated to this LSM303DLHC.
-   */
-  I2CDriver                 *i2cp;
-  /**
-   * @brief I2C configuration associated to this LSM303DLHC accelerometer
-   *        subsystem.
-   */
-  const I2CConfig           *i2ccfg;
-#endif /* LSM303DLHC_USE_I2C */
-  /**
-   * @brief   LSM303DLHC accelerometer subsystem configuration structure
-   */
-  const LSM303DLHCAccConfig    *acccfg;
-  /**
-   * @brief   LSM303DLHC compass subsystem configuration structure
-   */
-  const LSM303DLHCCompConfig   *compcfg;
-} LSM303DLHCConfig;
-
-/**
- * @brief   Structure representing a LSM303DLHC driver.
- */
-typedef struct LSM303DLHCDriver LSM303DLHCDriver;
-
-/**
- * @brief   @p LSM303DLHC accelerometer subsystem specific methods.
- */
-#define _lsm303dlhc_acc_methods                                             \
-  _base_accelerometer_methods
-
-/**
- * @brief   @p LSM303DLHC compass subsystem specific methods.
- */
-#define _lsm303dlhc_comp_methods                                            \
-  _base_compass_methods
-
-/**
- * @extends BaseAccelerometerVMT
- *
- * @brief   @p LSM303DLHC accelerometer virtual methods table.
- */
-struct LSM303DLHCACCVMT {
-  _lsm303dlhc_acc_methods
-};
-
-/**
- * @extends BaseCompassVMT
- *
- * @brief   @p LSM303DLHC compass virtual methods table.
- */
-struct LSM303DLHCCOMPVMT {
-  _lsm303dlhc_comp_methods
-};
-
-/**
- * @brief   @p LSM303DLHCDriver specific data.
- */
-#define _lsm303dlhc_data                                                    \
-  _base_accelerometer_data                                                  \
-  _base_compass_data                                                        \
-  /* Driver state.*/                                                        \
-  lsm303dlhc_state_t           state;                                       \
-  /* Current configuration data.*/                                          \
-  const LSM303DLHCConfig       *config;                                     \
-  /* Current accelerometer sensitivity.*/                                   \
-  float                     accsensitivity[LSM303DLHC_ACC_NUMBER_OF_AXES];  \
-  /* Accelerometer bias data.*/                                             \
-  int32_t                   accbias[LSM303DLHC_ACC_NUMBER_OF_AXES];         \
-  /* Current compass sensitivity.*/                                         \
-  float                     compsensitivity[LSM303DLHC_COMP_NUMBER_OF_AXES];\
-  /* Bias data.*/                                                           \
-  int32_t                   compbias[LSM303DLHC_COMP_NUMBER_OF_AXES];
-
-/**
- * @brief   LSM303DLHC 6-axis accelerometer/compass class.
- */
-struct LSM303DLHCDriver {
-  /** @brief BaseSensor Virtual Methods Table. */
-  const struct BaseSensorVMT *vmt_basesensor;
-  /** @brief BaseAccelerometer Virtual Methods Table. */
-  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;
-  /** @brief BaseCompass Virtual Methods Table. */
-  const struct BaseCompassVMT *vmt_basecompass;
-  /** @brief LSM303DLHC Accelerometer Virtual Methods Table. */
-  const struct LSM303DLHCACCVMT *vmt_lsm303dlhcacc;
-  /** @brief LSM303DLHC Compass Virtual Methods Table. */
-  const struct LSM303DLHCCOMPVMT *vmt_lsm303dlhccomp;
-  _lsm303dlhc_data
-};
-/** @} */
-
-/*===========================================================================*/
-/* Driver macros.                                                            */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* External declarations.                                                    */
-/*===========================================================================*/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-  void lsm303dlhcObjectInit(LSM303DLHCDriver *devp);
-  void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config);
-  void lsm303dlhcStop(LSM303DLHCDriver *devp);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _LSM303DLHC_H_ */
-
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+

+*/

+

+/**

+ * @file    lsm303dlhc.h

+ * @brief   LSM303DLHC MEMS interface module header.

+ *

+ * @{

+ */

+#ifndef _LSM303DLHC_H_

+#define _LSM303DLHC_H_

+

+#include "hal_accelerometer.h"

+#include "hal_compass.h"

+

+/*===========================================================================*/

+/* Driver constants.                                                         */

+/*===========================================================================*/

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem number of axes.

+ */

+#define  LSM303DLHC_ACC_NUMBER_OF_AXES         ((size_t)   3U)

+

+/**

+ * @brief   LSM303DLHC compass subsystem number of axes.

+ */

+#define  LSM303DLHC_COMP_NUMBER_OF_AXES        ((size_t)   3U)

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    Configuration options

+ * @{

+ */

+/**

+ * @brief   LSM303DLHC SPI interface selector.

+ * @details If set to @p TRUE the support for SPI is included.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(LSM303DLHC_USE_SPI) || defined(__DOXYGEN__)

+#define LSM303DLHC_USE_SPI                     FALSE

+#endif

+

+/**

+ * @brief   LSM303DLHC I2C interface selector.

+ * @details If set to @p TRUE the support for I2C is included.

+ * @note    The default is @p TRUE.

+ */

+#if !defined(LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)

+#define LSM303DLHC_USE_I2C                     TRUE

+#endif

+

+/**

+ * @brief   LSM303DLHC shared I2C switch.

+ * @details If set to @p TRUE the device acquires I2C bus ownership

+ *          on each transaction.

+ * @note    The default is @p FALSE. Requires I2C_USE_MUTUAL_EXCLUSION.

+ */

+#if !defined(LSM303DLHC_SHARED_I2C) || defined(__DOXYGEN__)

+#define LSM303DLHC_SHARED_I2C                  FALSE

+#endif

+/** @} */

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+#if LSM303DLHC_USE_SPI && LSM303DLHC_USE_I2C

+#error "LSM303DLHC_USE_SPI and LSM303DLHC_USE_I2C cannot be both true"

+#endif

+

+#if LSM303DLHC_USE_SPI && !HAL_USE_SPI

+#error "LSM303DLHC_USE_SPI requires HAL_USE_SPI"

+#endif

+

+#if LSM303DLHC_USE_I2C && !HAL_USE_I2C

+#error "LSM303DLHC_USE_I2C requires HAL_USE_I2C"

+#endif

+

+#if LSM303DLHC_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION

+#error "LSM303DLHC_SHARED_I2C requires I2C_USE_MUTUAL_EXCLUSION"

+#endif

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    LSM303DLHC accelerometer subsystem data structures and types.

+ * @{

+ */

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem full scale.

+ */

+typedef enum {

+  LSM303DLHC_ACC_FS_2G   = 0x00,    /**< Full scale ±2g.                    */

+  LSM303DLHC_ACC_FS_4G   = 0x10,    /**< Full scale ±4g.                    */

+  LSM303DLHC_ACC_FS_8G   = 0x20,    /**< Full scale ±8g.                    */

+  LSM303DLHC_ACC_FS_16G  = 0x30     /**< Full scale ±16g.                   */

+} lsm303dlhc_acc_fs_t;

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem output data rate.

+ */

+typedef enum {

+  LSM303DLHC_ACC_ODR_PD = 0x00,     /**< Power down                         */

+  LSM303DLHC_ACC_ODR_1Hz = 0x10,    /**< ODR 1 Hz                           */

+  LSM303DLHC_ACC_ODR_10Hz = 0x20,   /**< ODR 10 Hz                          */

+  LSM303DLHC_ACC_ODR_25Hz = 0x30,   /**< ODR 25 Hz                          */

+  LSM303DLHC_ACC_ODR_50Hz = 0x40,   /**< ODR 50 Hz                          */

+  LSM303DLHC_ACC_ODR_100Hz = 0x50,  /**< ODR 100 Hz                         */

+  LSM303DLHC_ACC_ODR_200Hz = 0x60,  /**< ODR 200 Hz                         */

+  LSM303DLHC_ACC_ODR_400Hz = 0x70,  /**< ODR 400 Hz                         */

+  LSM303DLHC_ACC_ODR_1620Hz = 0x80, /**< ODR 1620 Hz (LP only)              */

+  LSM303DLHC_ACC_ODR_1344Hz = 0x90  /**< ODR 1344 Hz or 5376 Hz in LP       */

+} lsm303dlhc_acc_odr_t;

+

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem axes enabling.

+ */

+typedef enum {

+  LSM303DLHC_ACC_AE_DISABLED = 0x00,/**< All axes disabled.                 */

+  LSM303DLHC_ACC_AE_X = 0x01,       /**< Only X-axis enabled.               */

+  LSM303DLHC_ACC_AE_Y = 0x02,       /**< Only Y-axis enabled.               */

+  LSM303DLHC_ACC_AE_XY = 0x03,      /**< X and Y axes enabled.              */

+  LSM303DLHC_ACC_AE_Z = 0x04,       /**< Only Z-axis enabled.               */

+  LSM303DLHC_ACC_AE_XZ = 0x05,      /**< X and Z axes enabled.              */

+  LSM303DLHC_ACC_AE_YZ = 0x06,      /**< Y and Z axes enabled.              */

+  LSM303DLHC_ACC_AE_XYZ = 0x07      /**< All axes enabled.                  */

+} lsm303dlhc_acc_ae_t;

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem low power mode.

+ */

+typedef enum {

+  LSM303DLHC_ACC_LP_DISABLED = 0x00,/**< Low power mode disabled.           */

+  LSM303DLHC_ACC_LP_ENABLED = 0x40  /**< Low power mode enabled.            */

+} lsm303dlhc_acc_lp_t;

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem high resolution mode.

+ */

+typedef enum {

+  LSM303DLHC_ACC_HR_DISABLED = 0x00,/**< High resolution mode disabled.     */

+  LSM303DLHC_ACC_HR_ENABLED = 0x08  /**< High resolution mode enabled.      */

+} lsm303dlhc_acc_hr_t;

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem block data update.

+ */

+typedef enum {

+  LSM303DLHC_ACC_BDU_CONT = 0x00,   /**< Continuous update                  */

+  LSM303DLHC_ACC_BDU_BLOCK = 0x80   /**< Update blocked                     */

+} lsm303dlhc_acc_bdu_t;

+

+/**

+ * @brief  LSM303DLHC accelerometer endianness.

+ */

+typedef enum {

+  LSM303DLHC_ACC_END_LITTLE = 0x00, /**< Little Endian                      */

+  LSM303DLHC_ACC_END_BIG = 0x40     /**< Big Endian                         */

+} lsm303dlhc_acc_end_t;

+

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem unit.

+ */

+typedef enum {

+  LSM303DLHC_ACC_UNIT_G = 0x00,     /**< Cooked data in g.                  */

+  LSM303DLHC_ACC_UNIT_MG = 0x01,    /**< Cooked data in mg.                 */

+  LSM303DLHC_ACC_UNIT_SI = 0x02,    /**< Cooked data in m/s^2.              */

+} lsm303dlhc_acc_unit_t;

+

+/**

+ * @brief   LSM303DLHC accelerometer subsystem configuration structure.

+ */

+typedef struct {

+  /**

+   * @brief   LSM303DLHC accelerometer subsystem full scale.

+   */

+  lsm303dlhc_acc_fs_t          fullscale;

+  /**

+   * @brief   LSM303DLHC accelerometer subsystem output data rate.

+   */

+  lsm303dlhc_acc_odr_t         outdatarate;

+  /**

+   * @brief   LSM303DLHC accelerometer subsystem axes enabling.

+   */

+  lsm303dlhc_acc_ae_t          axesenabling;

+  /**

+   * @brief   LSM303DLHC accelerometer subsystem low power mode.

+   */

+  lsm303dlhc_acc_lp_t          lowpower;

+  /**

+   * @brief   LSM303DLHC accelerometer subsystem high resolution mode.

+   */

+  lsm303dlhc_acc_hr_t          highresmode;

+  /**

+   * @brief   LSM303DLHC accelerometer subsystem block data update.

+   */

+  lsm303dlhc_acc_bdu_t         blockdataupdate;

+  /**

+   * @brief  LSM303DLHC accelerometer endianness.

+   */

+  lsm303dlhc_acc_end_t         endianess;

+  /**

+   * @brief   LSM303DLHC accelerometer subsystem unit.

+   */

+  lsm303dlhc_acc_unit_t        unit;

+} LSM303DLHCAccConfig;

+/** @} */

+

+/**

+ * @name    LSM303DLHC compass subsystem data structures and types.

+ * @{

+ */

+/**

+ * @brief   LSM303DLHC compass subsystem full scale.

+ */

+typedef enum {

+  LSM303DLHC_COMP_FS_1_3_GA = 0x20, /**< Full scale ±1.3 Gauss              */

+  LSM303DLHC_COMP_FS_1_9_GA = 0x40, /**< Full scale ±1.9 Gauss              */

+  LSM303DLHC_COMP_FS_2_5_GA = 0x60, /**< Full scale ±2.5 Gauss              */

+  LSM303DLHC_COMP_FS_4_0_GA = 0x80, /**< Full scale ±4.0 Gauss              */

+  LSM303DLHC_COMP_FS_4_7_GA = 0xA0, /**< Full scale ±4.7 Gauss              */

+  LSM303DLHC_COMP_FS_5_6_GA = 0xC0, /**< Full scale ±5.6 Gauss              */

+  LSM303DLHC_COMP_FS_8_1_GA = 0xE0  /**< Full scale ±8.1 Gauss              */

+} lsm303dlhc_comp_fs_t;

+

+/**

+ * @brief   LSM303DLHC compass subsystem output data rate.

+ */

+typedef enum {

+  LSM303DLHC_COMP_ODR_0_75HZ = 0x00,/**< ODR 0.75 Hz                        */

+  LSM303DLHC_COMP_ODR_1_5HZ = 0x04, /**< ODR 1.5 Hz                         */

+  LSM303DLHC_COMP_ODR_3_0HZ = 0x08, /**< ODR 3 Hz                           */

+  LSM303DLHC_COMP_ODR_7_5HZ = 0x0C, /**< ODR 7.5 Hz                         */

+  LSM303DLHC_COMP_ODR_15HZ = 0x10,  /**< ODR 15 Hz                          */

+  LSM303DLHC_COMP_ODR_30HZ = 0x14,  /**< ODR 30 Hz                          */

+  LSM303DLHC_COMP_ODR_75HZ = 0x18,  /**< ODR 75 Hz                          */

+  LSM303DLHC_COMP_ODR_220HZ = 0x1C  /**< ODR 220 Hz                         */

+} lsm303dlhc_comp_odr_t;

+

+/**

+ * @brief   LSM303DLHC compass subsystem working mode.

+ */

+typedef enum {

+  LSM303DLHC_COMP_MD_CONT = 0x00,   /**< Continuous-Conversion Mode         */

+  LSM303DLHC_COMP_MD_BLOCK = 0x01,  /**< Single-Conversion Mode             */

+  LSM303DLHC_COMP_MD_SLEEP = 0x02   /**< Sleep Mode                         */

+} lsm303dlhc_comp_md_t;

+

+/**

+ * @brief   LSM303DLHC compass subsystem configuration structure.

+ */

+typedef struct {

+  /**

+   * @brief   LSM303DLHC compass subsystem full scale.

+   */

+  lsm303dlhc_comp_fs_t         fullscale;

+  /**

+   * @brief   LSM303DLHC compass subsystem output data rate.

+   */

+  lsm303dlhc_comp_odr_t        outputdatarate;

+  /**

+   * @brief   LSM303DLHC compass subsystem working mode.

+   */

+  lsm303dlhc_comp_md_t         mode;

+} LSM303DLHCCompConfig;

+/** @} */

+

+/**

+ * @name    LSM303DLHC main system data structures and types.

+ * @{

+ */

+

+/**

+ * @brief   Driver state machine possible states.

+ */

+typedef enum {

+  LSM303DLHC_UNINIT = 0,            /**< Not initialized.                   */

+  LSM303DLHC_STOP = 1,              /**< Stopped.                           */

+  LSM303DLHC_READY = 2,             /**< Ready.                             */

+} lsm303dlhc_state_t;

+

+/**

+ * @brief   LSM303DLHC configuration structure.

+ */

+typedef struct {

+#if (LSM303DLHC_USE_SPI) || defined(__DOXYGEN__)

+  /**

+   * @brief SPI driver associated to this LSM303DLHC.

+   */

+  SPIDriver                 *spip;

+  /**

+   * @brief SPI configuration associated to this LSM303DLHC accelerometer

+   *        subsystem.

+   */

+  const SPIConfig           *accspicfg;

+  /**

+   * @brief SPI configuration associated to this LSM303DLHC compass

+   *        subsystem.

+   */

+  const SPIConfig           *compspicfg;

+#endif /* LSM303DLHC_USE_SPI */

+#if (LSM303DLHC_USE_I2C) || defined(__DOXYGEN__)

+  /**

+   * @brief I2C driver associated to this LSM303DLHC.

+   */

+  I2CDriver                 *i2cp;

+  /**

+   * @brief I2C configuration associated to this LSM303DLHC accelerometer

+   *        subsystem.

+   */

+  const I2CConfig           *i2ccfg;

+#endif /* LSM303DLHC_USE_I2C */

+  /**

+   * @brief   LSM303DLHC accelerometer subsystem configuration structure

+   */

+  const LSM303DLHCAccConfig    *acccfg;

+  /**

+   * @brief   LSM303DLHC compass subsystem configuration structure

+   */

+  const LSM303DLHCCompConfig   *compcfg;

+} LSM303DLHCConfig;

+

+/**

+ * @brief   Structure representing a LSM303DLHC driver.

+ */

+typedef struct LSM303DLHCDriver LSM303DLHCDriver;

+

+/**

+ * @brief   @p LSM303DLHC accelerometer subsystem specific methods.

+ */

+#define _lsm303dlhc_acc_methods                                             \

+  _base_accelerometer_methods

+

+/**

+ * @brief   @p LSM303DLHC compass subsystem specific methods.

+ */

+#define _lsm303dlhc_comp_methods                                            \

+  _base_compass_methods

+

+/**

+ * @extends BaseAccelerometerVMT

+ *

+ * @brief   @p LSM303DLHC accelerometer virtual methods table.

+ */

+struct LSM303DLHCACCVMT {

+  _lsm303dlhc_acc_methods

+};

+

+/**

+ * @extends BaseCompassVMT

+ *

+ * @brief   @p LSM303DLHC compass virtual methods table.

+ */

+struct LSM303DLHCCOMPVMT {

+  _lsm303dlhc_comp_methods

+};

+

+/**

+ * @brief   @p LSM303DLHCDriver specific data.

+ */

+#define _lsm303dlhc_data                                                    \

+  _base_accelerometer_data                                                  \

+  _base_compass_data                                                        \

+  /* Driver state.*/                                                        \

+  lsm303dlhc_state_t           state;                                       \

+  /* Current configuration data.*/                                          \

+  const LSM303DLHCConfig       *config;                                     \

+  /* Current accelerometer sensitivity.*/                                   \

+  float                     accsensitivity[LSM303DLHC_ACC_NUMBER_OF_AXES];  \

+  /* Accelerometer bias data.*/                                             \

+  int32_t                   accbias[LSM303DLHC_ACC_NUMBER_OF_AXES];         \

+  /* Current compass sensitivity.*/                                         \

+  float                     compsensitivity[LSM303DLHC_COMP_NUMBER_OF_AXES];\

+  /* Bias data.*/                                                           \

+  int32_t                   compbias[LSM303DLHC_COMP_NUMBER_OF_AXES];

+

+/**

+ * @brief   LSM303DLHC 6-axis accelerometer/compass class.

+ */

+struct LSM303DLHCDriver {

+  /** @brief BaseSensor Virtual Methods Table. */

+  const struct BaseSensorVMT *vmt_basesensor;

+  /** @brief BaseAccelerometer Virtual Methods Table. */

+  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;

+  /** @brief BaseCompass Virtual Methods Table. */

+  const struct BaseCompassVMT *vmt_basecompass;

+  /** @brief LSM303DLHC Accelerometer Virtual Methods Table. */

+  const struct LSM303DLHCACCVMT *vmt_lsm303dlhcacc;

+  /** @brief LSM303DLHC Compass Virtual Methods Table. */

+  const struct LSM303DLHCCOMPVMT *vmt_lsm303dlhccomp;

+  _lsm303dlhc_data

+};

+/** @} */

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void lsm303dlhcObjectInit(LSM303DLHCDriver *devp);

+  void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config);

+  void lsm303dlhcStop(LSM303DLHCDriver *devp);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* _LSM303DLHC_H_ */

+

+/** @} */

diff --git a/os/ex/ST/lsm6ds0.c b/os/ex/ST/lsm6ds0.c
index caa0d875d..054cca679 100644
--- a/os/ex/ST/lsm6ds0.c
+++ b/os/ex/ST/lsm6ds0.c
@@ -1,846 +1,846 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lsm6ds0.c
- * @brief   LSM6DS0 MEMS interface module code.
- *
- * @addtogroup lsm6ds0
- * @{
- */
-
-#include "hal.h"
-#include "lsm6ds0.h"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-#define  LSM6DS0_ACC_SENS_2G                ((float)0.061f)
-#define  LSM6DS0_ACC_SENS_4G                ((float)0.122f)
-#define  LSM6DS0_ACC_SENS_8G                ((float)0.244f)
-#define  LSM6DS0_ACC_SENS_16G               ((float)0.732f)
-
-#define  LSM6DS0_GYRO_SENS_245DPS           ((float)0.00875f)
-#define  LSM6DS0_GYRO_SENS_500DPS           ((float)0.01750f)
-#define  LSM6DS0_GYRO_SENS_2000DPS          ((float)0.07000f)
-
-#define  LSM6DS0_TEMP_SENS                  ((float)16.0f)
-#define  LSM6DS0_TEMP_SENS_OFF              ((float)25.0f)
-
-#define  LSM6DS0_DI                         ((uint8_t)0xFF)
-#define  LSM6DS0_DI_0                       ((uint8_t)0x01)
-#define  LSM6DS0_DI_1                       ((uint8_t)0x02)
-#define  LSM6DS0_DI_2                       ((uint8_t)0x04)
-#define  LSM6DS0_DI_3                       ((uint8_t)0x08)
-#define  LSM6DS0_DI_4                       ((uint8_t)0x10)
-#define  LSM6DS0_DI_5                       ((uint8_t)0x20)
-#define  LSM6DS0_DI_6                       ((uint8_t)0x40)
-#define  LSM6DS0_DI_7                       ((uint8_t)0x80)
-
-#define  LSM6DS0_AD_0                       ((uint8_t)0x01)
-#define  LSM6DS0_AD_1                       ((uint8_t)0x02)
-#define  LSM6DS0_AD_2                       ((uint8_t)0x04)
-#define  LSM6DS0_AD_3                       ((uint8_t)0x08)
-#define  LSM6DS0_AD_4                       ((uint8_t)0x10)
-#define  LSM6DS0_AD_5                       ((uint8_t)0x20)
-#define  LSM6DS0_AD_6                       ((uint8_t)0x40)
-
-#define  LSM6DS0_RW                         ((uint8_t)0x80)
-
-#define  LSM6DS0_AD_ACT_THS                 ((uint8_t)0x04)
-#define  LSM6DS0_AD_ACT_DUR                 ((uint8_t)0x05)
-#define  LSM6DS0_AD_INT_GEN_CFG_XL          ((uint8_t)0x06)
-#define  LSM6DS0_AD_INT_GEN_THS_X_XL        ((uint8_t)0x07)
-#define  LSM6DS0_AD_INT_GEN_THS_Y_XL        ((uint8_t)0x08)
-#define  LSM6DS0_AD_INT_GEN_THS_Z_XL        ((uint8_t)0x09)
-#define  LSM6DS0_AD_INT_GEN_DUR_XL          ((uint8_t)0x0A)
-#define  LSM6DS0_AD_REFERENCE_G             ((uint8_t)0x0B)
-#define  LSM6DS0_AD_INT_CTRL                ((uint8_t)0x0C)
-#define  LSM6DS0_AD_WHO_AM_I                ((uint8_t)0x0F)
-#define  LSM6DS0_AD_CTRL_REG1_G             ((uint8_t)0x10)
-#define  LSM6DS0_AD_CTRL_REG2_G             ((uint8_t)0x11)
-#define  LSM6DS0_AD_CTRL_REG3_G             ((uint8_t)0x12)
-#define  LSM6DS0_AD_ORIENT_CFG_G            ((uint8_t)0x13)
-#define  LSM6DS0_AD_INT_GEN_SRC_G           ((uint8_t)0x14)
-#define  LSM6DS0_AD_OUT_TEMP_L              ((uint8_t)0x15)
-#define  LSM6DS0_AD_OUT_TEMP_H              ((uint8_t)0x16)
-#define  LSM6DS0_AD_STATUS_REG1             ((uint8_t)0x17)
-#define  LSM6DS0_AD_OUT_X_L_G               ((uint8_t)0x18)
-#define  LSM6DS0_AD_OUT_X_H_G               ((uint8_t)0x19)
-#define  LSM6DS0_AD_OUT_Y_L_G               ((uint8_t)0x1A)
-#define  LSM6DS0_AD_OUT_Y_H_G               ((uint8_t)0x1B)
-#define  LSM6DS0_AD_OUT_Z_L_G               ((uint8_t)0x1C)
-#define  LSM6DS0_AD_OUT_Z_H_G               ((uint8_t)0x1D)
-#define  LSM6DS0_AD_CTRL_REG4               ((uint8_t)0x1E)
-#define  LSM6DS0_AD_CTRL_REG5_XL            ((uint8_t)0x1F)
-#define  LSM6DS0_AD_CTRL_REG6_XL            ((uint8_t)0x20)
-#define  LSM6DS0_AD_CTRL_REG7_XL            ((uint8_t)0x21)
-#define  LSM6DS0_AD_CTRL_REG8               ((uint8_t)0x22)
-#define  LSM6DS0_AD_CTRL_REG9               ((uint8_t)0x23)
-#define  LSM6DS0_AD_CTRL_REG10              ((uint8_t)0x24)
-#define  LSM6DS0_AD_INT_GEN_SRC_XL          ((uint8_t)0x26)
-#define  LSM6DS0_AD_STATUS_REG2             ((uint8_t)0x27)
-#define  LSM6DS0_AD_OUT_X_L_XL              ((uint8_t)0x28)
-#define  LSM6DS0_AD_OUT_X_H_XL              ((uint8_t)0x29)
-#define  LSM6DS0_AD_OUT_Y_L_XL              ((uint8_t)0x2A)
-#define  LSM6DS0_AD_OUT_Y_H_XL              ((uint8_t)0x2B)
-#define  LSM6DS0_AD_OUT_Z_L_XL              ((uint8_t)0x2C)
-#define  LSM6DS0_AD_OUT_Z_H_XL              ((uint8_t)0x2D)
-#define  LSM6DS0_AD_FIFO_CTRL               ((uint8_t)0x2E)
-#define  LSM6DS0_AD_FIFO_SRC                ((uint8_t)0x2F)
-#define  LSM6DS0_AD_INT_GEN_CFG_G           ((uint8_t)0x30)
-#define  LSM6DS0_AD_INT_GEN_THS_XH_G        ((uint8_t)0x31)
-#define  LSM6DS0_AD_INT_GEN_THS_XL_G        ((uint8_t)0x32)
-#define  LSM6DS0_AD_INT_GEN_THS_YH_G        ((uint8_t)0x33)
-#define  LSM6DS0_AD_INT_GEN_THS_YL_G        ((uint8_t)0x34)
-#define  LSM6DS0_AD_INT_GEN_THS_ZH_G        ((uint8_t)0x35)
-#define  LSM6DS0_AD_INT_GEN_THS_ZL_G        ((uint8_t)0x36)
-#define  LSM6DS0_AD_INT_GEN_DUR_G           ((uint8_t)0x37)
-
-#define  LSM6DS0_CTRL_REG1_G_FS_MASK        ((uint8_t)0x18)
-#define  LSM6DS0_CTRL_REG6_XL_FS_MASK       ((uint8_t)0x18)
-
-#define  TO_G                               ((float)0.001f)
-#define  TO_SI                              ((float)0.00981f)
-/*===========================================================================*/
-/* Driver exported variables.                                                */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local variables and types.                                         */
-/*===========================================================================*/
-
-/**
- * @brief   LSM6DS0 address increment mode.
- */
-typedef enum {
-  LSM6DS0_IF_ADD_INC_DIS = 0x00,    /**< Address increment disabled.        */
-  LSM6DS0_IF_ADD_INC_EN = 0x04      /**< Address increment disabled.        */
-}lsm6ds0_id_add_inc_t;
-
-/**
- * @brief   LSM6DS0 gyroscope subsystem sleep mode.
- */
-typedef enum {
-  LSM6DS0_GYRO_SLP_DISABLED = 0x00, /**< Gyroscope in normal mode.          */
-  LSM6DS0_GYRO_SLP_ENABLED  = 0x40  /**< Gyroscope in sleep mode.           */
-}lsm6ds0_gyro_slp_t;
-
-/*===========================================================================*/
-/* Driver local functions.                                                   */
-/*===========================================================================*/
-
-#if (LSM6DS0_USE_I2C) || defined(__DOXYGEN__)
-/**
- * @brief   Reads registers value using I2C.
- * @pre     The I2C interface must be initialized and the driver started.
- * @note    IF_ADD_INC bit must be 1 in CTRL_REG8
- *
- * @param[in] i2cp       pointer to the I2C interface
- * @param[in] sad        slave address without R bit
- * @param[in] reg        first sub-register address
- * @return               the read value.
- */
-uint8_t lsm6ds0I2CReadRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,
-                               msg_t* msgp) {
-  msg_t msg;
-#if defined(STM32F103_MCUCONF)
-  uint8_t rxbuf[2];
-  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,
-                                   TIME_INFINITE);
-  if(msgp != NULL){
-    *msgp = msg;
-  }
-  return rxbuf[0];
-#else
-  uint8_t txbuf, rxbuf;
-  txbuf = reg;
-  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, &rxbuf, 1,
-                                        TIME_INFINITE);
-  if(msgp != NULL){
-    *msgp = msg;
-  }
-  return rxbuf;
-#endif
-}
-
-/**
- * @brief   Writes a value into a register using I2C.
- * @pre     The I2C interface must be initialized and the driver started.
- *
- * @param[in] i2cp       pointer to the I2C interface
- * @param[in] sad        slave address without R bit
- * @param[in] sub        sub-register address
- * @param[in] value      the value to be written
- * @return               the operation status.
- */
-msg_t lsm6ds0I2CWriteRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,
-                              uint8_t value) {
-  uint8_t rxbuf;
-  uint8_t txbuf[2];
-  switch (reg) {
-  default:
-    /* Reserved register must not be written, according to the datasheet
-     * this could permanently damage the device.
-     */
-    osalDbgAssert(FALSE, "lsm6ds0WriteRegisterI2C(), reserved register");
-  case LSM6DS0_AD_WHO_AM_I:
-  case LSM6DS0_AD_INT_GEN_SRC_G:
-  case LSM6DS0_AD_OUT_TEMP_L:
-  case LSM6DS0_AD_OUT_TEMP_H:
-  case LSM6DS0_AD_STATUS_REG1:
-  case LSM6DS0_AD_OUT_X_L_G:
-  case LSM6DS0_AD_OUT_X_H_G:
-  case LSM6DS0_AD_OUT_Y_L_G:
-  case LSM6DS0_AD_OUT_Y_H_G:
-  case LSM6DS0_AD_OUT_Z_L_G:
-  case LSM6DS0_AD_OUT_Z_H_G:
-  case LSM6DS0_AD_INT_GEN_SRC_XL:
-  case LSM6DS0_AD_STATUS_REG2:
-  case LSM6DS0_AD_OUT_X_L_XL:
-  case LSM6DS0_AD_OUT_X_H_XL:
-  case LSM6DS0_AD_OUT_Y_L_XL:
-  case LSM6DS0_AD_OUT_Y_H_XL:
-  case LSM6DS0_AD_OUT_Z_L_XL:
-  case LSM6DS0_AD_OUT_Z_H_XL:
-  case LSM6DS0_AD_FIFO_SRC:
-  /* Read only registers cannot be written, the command is ignored.*/
-    return MSG_RESET;
-  case LSM6DS0_AD_ACT_THS:
-  case LSM6DS0_AD_ACT_DUR:
-  case LSM6DS0_AD_INT_GEN_CFG_XL:
-  case LSM6DS0_AD_INT_GEN_THS_X_XL:
-  case LSM6DS0_AD_INT_GEN_THS_Y_XL:
-  case LSM6DS0_AD_INT_GEN_THS_Z_XL:
-  case LSM6DS0_AD_INT_GEN_DUR_XL:
-  case LSM6DS0_AD_REFERENCE_G:
-  case LSM6DS0_AD_INT_CTRL:
-  case LSM6DS0_AD_CTRL_REG1_G:
-  case LSM6DS0_AD_CTRL_REG2_G:
-  case LSM6DS0_AD_CTRL_REG3_G:
-  case LSM6DS0_AD_ORIENT_CFG_G:
-  case LSM6DS0_AD_CTRL_REG4:
-  case LSM6DS0_AD_CTRL_REG5_XL:
-  case LSM6DS0_AD_CTRL_REG6_XL:
-  case LSM6DS0_AD_CTRL_REG7_XL:
-  case LSM6DS0_AD_CTRL_REG8:
-  case LSM6DS0_AD_CTRL_REG9:
-  case LSM6DS0_AD_CTRL_REG10:
-  case LSM6DS0_AD_FIFO_CTRL:
-  case LSM6DS0_AD_INT_GEN_CFG_G:
-  case LSM6DS0_AD_INT_GEN_THS_XH_G:
-  case LSM6DS0_AD_INT_GEN_THS_XL_G:
-  case LSM6DS0_AD_INT_GEN_THS_YH_G:
-  case LSM6DS0_AD_INT_GEN_THS_YL_G:
-  case LSM6DS0_AD_INT_GEN_THS_ZH_G:
-  case LSM6DS0_AD_INT_GEN_THS_ZL_G:
-  case LSM6DS0_AD_INT_GEN_DUR_G:
-    txbuf[0] = reg;
-    txbuf[1] = value;
-    return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
-    break;
-  }
-}
-#endif /* LSM6DS0_USE_I2C */
-
-/*
- * Interface implementation.
- */
-static size_t acc_get_axes_number(void *ip) {
-
-  osalDbgCheck(ip != NULL);
-  return LSM6DS0_ACC_NUMBER_OF_AXES;
-}
-
-static size_t gyro_get_axes_number(void *ip) {
-
-  osalDbgCheck(ip != NULL);
-  return LSM6DS0_GYRO_NUMBER_OF_AXES;
-}
-
-static size_t sens_get_axes_number(void *ip) {
-  size_t size = 0;
-  
-  osalDbgCheck(ip != NULL);
-  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL)
-	size += acc_get_axes_number(ip);
-  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL)
-    size += gyro_get_axes_number(ip);  
-  return size;
-}
-
-static msg_t acc_read_raw(void *ip, int32_t axes[]) {
-  int16_t tmp;
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-              "acc_read_raw(), invalid state");
-
-#if LSM6DS0_USE_I2C
-  osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY),
-                "acc_read_raw(), channel not ready");
-#if LSM6DS0_SHARED_I2C
-  i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
-  i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
-           ((LSM6DS0Driver *)ip)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_X){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                 LSM6DS0_AD_OUT_X_L_XL, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                  LSM6DS0_AD_OUT_X_H_XL, NULL) << 8;
-    axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[0];
-  }
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Y){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                 LSM6DS0_AD_OUT_Y_L_XL, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                  LSM6DS0_AD_OUT_Y_H_XL, NULL) << 8;
-    axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[1];
-  }
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Z){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                 LSM6DS0_AD_OUT_Z_L_XL, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                  LSM6DS0_AD_OUT_Z_H_XL, NULL) << 8;
-    axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[2];
-  }
-#if LSM6DS0_SHARED_I2C
-  i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
-#endif /* LSM6DS0_USE_I2C */
-  return MSG_OK;
-}
-
-static msg_t gyro_read_raw(void *ip, int32_t axes[]) {
-  int16_t tmp;
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-              "gyro_read_raw(), invalid state");
-
-#if LSM6DS0_USE_I2C
-  osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY),
-                "gyro_read_raw(), channel not ready");
-#if LSM6DS0_SHARED_I2C
-  i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
-  i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
-           ((LSM6DS0Driver *)ip)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-  if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_X){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                   LSM6DS0_AD_OUT_X_L_G, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                    LSM6DS0_AD_OUT_X_H_G, NULL) << 8;
-	axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[0];
-  }
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Y){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                   LSM6DS0_AD_OUT_Y_L_G, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                    LSM6DS0_AD_OUT_Y_H_G, NULL) << 8;
-    axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[1];
-  }
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Z){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                   LSM6DS0_AD_OUT_Z_L_G, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                    LSM6DS0_AD_OUT_Z_H_G, NULL) << 8;
-    axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[2];
-  }
-#if LSM6DS0_SHARED_I2C
-  i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
-#endif /* LSM6DS0_USE_I2C */ 
-  return MSG_OK;
-}
-
-static msg_t sens_read_raw(void *ip, int32_t axes[]) {
-  int32_t* bp = axes;
-  msg_t msg;
-  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL) {
-	msg = acc_read_raw(ip, bp);
-	if(msg != MSG_OK)
-	  return msg;
-	bp += LSM6DS0_ACC_NUMBER_OF_AXES;
-  }
-  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL) {
-    msg = gyro_read_raw(ip, bp);
-  }
-  return msg;
-}
-	
-static msg_t acc_read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-              "acc_read_cooked(), invalid state");
-
-  msg = acc_read_raw(ip, raw);
-  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->accsensitivity[i];
-    if(((LSM6DS0Driver *)ip)->config->acccfg->unit == LSM6DS0_ACC_UNIT_G){
-      axes[i] *= TO_G;
-    }
-    else if(((LSM6DS0Driver *)ip)->config->acccfg->unit == LSM6DS0_ACC_UNIT_SI){
-      axes[i] *= TO_SI;
-    }
-  }
-  return msg;
-}
-
-static msg_t gyro_read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-              "gyro_read_cooked(), invalid state");
-
-  msg = gyro_read_raw(ip, raw);
-  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->gyrosensitivity[i];
-  }
-  return msg;
-}
-
-static msg_t sens_read_cooked(void *ip, float axes[]) {
-  float* bp = axes;
-  msg_t msg;
-  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL) {
-	msg = acc_read_cooked(ip, bp);
-	if(msg != MSG_OK)
-	  return msg;
-	bp += LSM6DS0_ACC_NUMBER_OF_AXES;
-  }
-  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL) {
-    msg = gyro_read_cooked(ip, bp);
-  }
-  return msg;
-}
-
-static msg_t gyro_sample_bias(void *ip) {
-  uint32_t i, j;
-  int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
-  int32_t buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
-  msg_t msg;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-                "sample_bias(), invalid state");
-
-  for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){
-    msg = gyro_read_raw(ip, raw);
-	if(msg != MSG_OK)
-	  return msg;
-    for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){
-      buff[j] += raw[j];
-    }
-    osalThreadSleepMicroseconds(LSM6DS0_GYRO_BIAS_SETTLING_uS);
-  }
-
-  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
-    ((LSM6DS0Driver *)ip)->gyrobias[i] = buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES;
-  }
-  return msg;
-}
-
-static msg_t acc_set_bias(void *ip, int32_t *bp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (bp !=NULL));
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
-                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
-                "acc_set_bias(), invalid state");
-  
-  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
-    ((LSM6DS0Driver *)ip)->accbias[i] = bp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t gyro_set_bias(void *ip, int32_t *bp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (bp !=NULL));
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
-                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
-                "gyro_set_bias(), invalid state");
-  
-  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
-    ((LSM6DS0Driver *)ip)->gyrobias[i] = bp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t acc_reset_bias(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
-                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
-              "acc_reset_bias(), invalid state");
-
-  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-    ((LSM6DS0Driver *)ip)->accbias[i] = 0;
-  return MSG_OK;
-}
-
-static msg_t gyro_reset_bias(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
-                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
-              "gyro_reset_bias(), invalid state");
-
-  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
-    ((LSM6DS0Driver *)ip)->gyrobias[i] = 0;
-  return MSG_OK;
-}
-
-static msg_t acc_set_sensivity(void *ip, float *sp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (sp !=NULL));
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-                "acc_set_sensivity(), invalid state");
-  
-  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
-    ((LSM6DS0Driver *)ip)->accsensitivity[i] = sp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t gyro_set_sensivity(void *ip, float *sp) {
-  uint32_t i;
-  
-  osalDbgCheck((ip != NULL) && (sp !=NULL));
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-                "gyro_set_sensivity(), invalid state");
-  
-  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
-    ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = sp[i];
-  }
-  return MSG_OK;
-}
-
-static msg_t acc_reset_sensivity(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-                "acc_reset_sensivity(), invalid state");
-
-  if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_2G)
-    for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
-  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G)
-	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
-  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G)
-	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
-  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)
-	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
-  else {
-    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");
-    return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
-static msg_t gyro_reset_sensivity(void *ip) {
-  uint32_t i;
-
-  osalDbgCheck(ip != NULL);
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-                "gyro_reset_sensivity(), invalid state");
-
-  if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)
-    for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-      ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
-  else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DSP)
-	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-      ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
-  else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)
-	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-      ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
-  else {
-    osalDbgAssert(FALSE, "reset_sensivity(), gyroscope full scale issue");
-    return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
-static msg_t sens_get_temperature(void *ip, float* tempp) {
-  int16_t temp;
-#if LSM6DS0_USE_I2C
-  osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY),
-                "gyro_read_raw(), channel not ready");
-#if LSM6DS0_SHARED_I2C
-  i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
-  i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
-           ((LSM6DS0Driver *)ip)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-  temp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                  LSM6DS0_AD_OUT_TEMP_L, NULL);
-  temp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                   LSM6DS0_AD_OUT_TEMP_H, NULL) << 8;
-#if LSM6DS0_SHARED_I2C
-  i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
-#endif /* LSM6DS0_USE_I2C */
-  *tempp = ((float)temp / LSM6DS0_TEMP_SENS) + LSM6DS0_TEMP_SENS_OFF;
-  return MSG_OK;
-}
-
-static const struct BaseSensorVMT vmt_basesensor = {
-  sens_get_axes_number, sens_read_raw, sens_read_cooked
-};
-
-static const struct BaseGyroscopeVMT vmt_basegyroscope = {
-  gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,
-  gyro_sample_bias, gyro_set_bias, gyro_reset_bias,
-  gyro_set_sensivity, gyro_reset_sensivity
-};
-
-static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {
-  acc_get_axes_number, acc_read_raw, acc_read_cooked,
-  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
-};
-
-static const struct LSM6DS0ACCVMT vmt_lsm6ds0acc = {
-  acc_get_axes_number, acc_read_raw, acc_read_cooked,
-  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
-};
-
-static const struct LSM6DS0GYROVMT vmt_lsm6ds0gyro = {
-  gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,
-  gyro_sample_bias, gyro_set_bias, gyro_reset_bias,
-  gyro_set_sensivity, gyro_reset_sensivity, sens_get_temperature
-};
-
-/*===========================================================================*/
-/* Driver exported functions.                                                */
-/*===========================================================================*/
-
-/**
- * @brief   Initializes an instance.
- *
- * @param[out] devp     pointer to the @p LSM6DS0Driver object
- *
- * @init
- */
-void lsm6ds0ObjectInit(LSM6DS0Driver *devp) {
-  uint32_t i;
-  devp->vmt_basesensor = &vmt_basesensor;
-  devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;
-  devp->vmt_basegyroscope = &vmt_basegyroscope;
-  devp->vmt_lsm6ds0acc = &vmt_lsm6ds0acc;
-  devp->vmt_lsm6ds0gyro = &vmt_lsm6ds0gyro;
-  devp->config = NULL;
-  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-    devp->accbias[i] = 0;
-  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
-    devp->gyrobias[i] = 0;
-  devp->state  = LSM6DS0_STOP;
-}
-
-/**
- * @brief   Configures and activates LSM6DS0 Complex Driver peripheral.
- *
- * @param[in] devp      pointer to the @p LSM6DS0Driver object
- * @param[in] config    pointer to the @p LSM6DS0Config object
- *
- * @api
- */
-void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
-  uint32_t i;
-  osalDbgCheck((devp != NULL) && (config != NULL));
-
-  osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
-              "lsm6ds0Start(), invalid state");			  
-
-  devp->config = config;
-
-#if LSM6DS0_USE_I2C
-#if	LSM6DS0_SHARED_I2C
-  i2cAcquireBus((devp)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
-  i2cStart((devp)->config->i2cp,
-           (devp)->config->i2ccfg);
-  if((devp)->config->acccfg != NULL) {
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG5_XL,
-                            devp->config->acccfg->decmode |
-                            devp->config->acccfg->axesenabling);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG6_XL,
-                            devp->config->acccfg->outdatarate |
-                            devp->config->acccfg->fullscale );
-  }
-  if((devp)->config->gyrocfg != NULL) {
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG1_G,
-                            devp->config->gyrocfg->fullscale |
-                            devp->config->gyrocfg->outdatarate);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG2_G,
-                            devp->config->gyrocfg->outsel);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG3_G,
-                            devp->config->gyrocfg->hpfenable |
-                            devp->config->gyrocfg->lowmodecfg |
-                            devp->config->gyrocfg->hpcfg);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG4,
-                            devp->config->gyrocfg->axesenabling);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG9,
-                            LSM6DS0_GYRO_SLP_DISABLED);
-  }
-  lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                          devp->config->slaveaddress,
-                          LSM6DS0_AD_CTRL_REG8,
-                          devp->config->endianness |
-                          devp->config->blockdataupdate);
-#if	LSM6DS0_SHARED_I2C
-  i2cReleaseBus((devp)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */  
-#endif /* LSM6DS0_USE_I2C */
-  /* Storing sensitivity information according to full scale value */
-  if((devp)->config->acccfg != NULL) {
-    if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_2G)
-      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
-    else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G)
-      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
-    else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G)
-      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
-    else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)
-      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
-        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
-    else
-      osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
-  }
-  if((devp)->config->gyrocfg != NULL) {
-    if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)
-      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
-        devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
-    else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DSP)
-      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
-        devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
-    else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)
-      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
-        devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
-    else
-      osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
-  }
-  /* This is the Gyroscope transient recovery time */
-  osalThreadSleepMilliseconds(5);
-
-  devp->state = LSM6DS0_READY;
-} 
-
-/**
- * @brief   Deactivates the LSM6DS0 Complex Driver peripheral.
- *
- * @param[in] devp       pointer to the @p LSM6DS0Driver object
- *
- * @api
- */
-void lsm6ds0Stop(LSM6DS0Driver *devp) {
-
-  osalDbgCheck(devp != NULL);
-
-  osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
-                "lsm6ds0Stop(), invalid state");
-
-#if (LSM6DS0_USE_I2C)
-  if (devp->state == LSM6DS0_STOP) {
-#if	LSM6DS0_SHARED_I2C
-    i2cAcquireBus((devp)->config->i2cp);
-    i2cStart((devp)->config->i2cp,
-             (devp)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-    if((devp)->config->acccfg != NULL) {
-      lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                              devp->config->slaveaddress,
-                              LSM6DS0_AD_CTRL_REG6_XL,
-                              LSM6DS0_ACC_ODR_PD);
-    }
-    if((devp)->config->gyrocfg != NULL) {
-      lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                              devp->config->slaveaddress,
-                              LSM6DS0_AD_CTRL_REG9,
-                              LSM6DS0_GYRO_SLP_ENABLED);
-    }
-    i2cStop((devp)->config->i2cp);
-#if	LSM6DS0_SHARED_I2C
-    i2cReleaseBus((devp)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */    
-  }			  
-#endif /* LSM6DS0_USE_I2C */
-  devp->state = LSM6DS0_STOP;
-}
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lsm6ds0.c

+ * @brief   LSM6DS0 MEMS interface module code.

+ *

+ * @addtogroup lsm6ds0

+ * @{

+ */

+

+#include "hal.h"

+#include "lsm6ds0.h"

+

+/*===========================================================================*/

+/* Driver local definitions.                                                 */

+/*===========================================================================*/

+

+#define  LSM6DS0_ACC_SENS_2G                ((float)0.061f)

+#define  LSM6DS0_ACC_SENS_4G                ((float)0.122f)

+#define  LSM6DS0_ACC_SENS_8G                ((float)0.244f)

+#define  LSM6DS0_ACC_SENS_16G               ((float)0.732f)

+

+#define  LSM6DS0_GYRO_SENS_245DPS           ((float)0.00875f)

+#define  LSM6DS0_GYRO_SENS_500DPS           ((float)0.01750f)

+#define  LSM6DS0_GYRO_SENS_2000DPS          ((float)0.07000f)

+

+#define  LSM6DS0_TEMP_SENS                  ((float)16.0f)

+#define  LSM6DS0_TEMP_SENS_OFF              ((float)25.0f)

+

+#define  LSM6DS0_DI                         ((uint8_t)0xFF)

+#define  LSM6DS0_DI_0                       ((uint8_t)0x01)

+#define  LSM6DS0_DI_1                       ((uint8_t)0x02)

+#define  LSM6DS0_DI_2                       ((uint8_t)0x04)

+#define  LSM6DS0_DI_3                       ((uint8_t)0x08)

+#define  LSM6DS0_DI_4                       ((uint8_t)0x10)

+#define  LSM6DS0_DI_5                       ((uint8_t)0x20)

+#define  LSM6DS0_DI_6                       ((uint8_t)0x40)

+#define  LSM6DS0_DI_7                       ((uint8_t)0x80)

+

+#define  LSM6DS0_AD_0                       ((uint8_t)0x01)

+#define  LSM6DS0_AD_1                       ((uint8_t)0x02)

+#define  LSM6DS0_AD_2                       ((uint8_t)0x04)

+#define  LSM6DS0_AD_3                       ((uint8_t)0x08)

+#define  LSM6DS0_AD_4                       ((uint8_t)0x10)

+#define  LSM6DS0_AD_5                       ((uint8_t)0x20)

+#define  LSM6DS0_AD_6                       ((uint8_t)0x40)

+

+#define  LSM6DS0_RW                         ((uint8_t)0x80)

+

+#define  LSM6DS0_AD_ACT_THS                 ((uint8_t)0x04)

+#define  LSM6DS0_AD_ACT_DUR                 ((uint8_t)0x05)

+#define  LSM6DS0_AD_INT_GEN_CFG_XL          ((uint8_t)0x06)

+#define  LSM6DS0_AD_INT_GEN_THS_X_XL        ((uint8_t)0x07)

+#define  LSM6DS0_AD_INT_GEN_THS_Y_XL        ((uint8_t)0x08)

+#define  LSM6DS0_AD_INT_GEN_THS_Z_XL        ((uint8_t)0x09)

+#define  LSM6DS0_AD_INT_GEN_DUR_XL          ((uint8_t)0x0A)

+#define  LSM6DS0_AD_REFERENCE_G             ((uint8_t)0x0B)

+#define  LSM6DS0_AD_INT_CTRL                ((uint8_t)0x0C)

+#define  LSM6DS0_AD_WHO_AM_I                ((uint8_t)0x0F)

+#define  LSM6DS0_AD_CTRL_REG1_G             ((uint8_t)0x10)

+#define  LSM6DS0_AD_CTRL_REG2_G             ((uint8_t)0x11)

+#define  LSM6DS0_AD_CTRL_REG3_G             ((uint8_t)0x12)

+#define  LSM6DS0_AD_ORIENT_CFG_G            ((uint8_t)0x13)

+#define  LSM6DS0_AD_INT_GEN_SRC_G           ((uint8_t)0x14)

+#define  LSM6DS0_AD_OUT_TEMP_L              ((uint8_t)0x15)

+#define  LSM6DS0_AD_OUT_TEMP_H              ((uint8_t)0x16)

+#define  LSM6DS0_AD_STATUS_REG1             ((uint8_t)0x17)

+#define  LSM6DS0_AD_OUT_X_L_G               ((uint8_t)0x18)

+#define  LSM6DS0_AD_OUT_X_H_G               ((uint8_t)0x19)

+#define  LSM6DS0_AD_OUT_Y_L_G               ((uint8_t)0x1A)

+#define  LSM6DS0_AD_OUT_Y_H_G               ((uint8_t)0x1B)

+#define  LSM6DS0_AD_OUT_Z_L_G               ((uint8_t)0x1C)

+#define  LSM6DS0_AD_OUT_Z_H_G               ((uint8_t)0x1D)

+#define  LSM6DS0_AD_CTRL_REG4               ((uint8_t)0x1E)

+#define  LSM6DS0_AD_CTRL_REG5_XL            ((uint8_t)0x1F)

+#define  LSM6DS0_AD_CTRL_REG6_XL            ((uint8_t)0x20)

+#define  LSM6DS0_AD_CTRL_REG7_XL            ((uint8_t)0x21)

+#define  LSM6DS0_AD_CTRL_REG8               ((uint8_t)0x22)

+#define  LSM6DS0_AD_CTRL_REG9               ((uint8_t)0x23)

+#define  LSM6DS0_AD_CTRL_REG10              ((uint8_t)0x24)

+#define  LSM6DS0_AD_INT_GEN_SRC_XL          ((uint8_t)0x26)

+#define  LSM6DS0_AD_STATUS_REG2             ((uint8_t)0x27)

+#define  LSM6DS0_AD_OUT_X_L_XL              ((uint8_t)0x28)

+#define  LSM6DS0_AD_OUT_X_H_XL              ((uint8_t)0x29)

+#define  LSM6DS0_AD_OUT_Y_L_XL              ((uint8_t)0x2A)

+#define  LSM6DS0_AD_OUT_Y_H_XL              ((uint8_t)0x2B)

+#define  LSM6DS0_AD_OUT_Z_L_XL              ((uint8_t)0x2C)

+#define  LSM6DS0_AD_OUT_Z_H_XL              ((uint8_t)0x2D)

+#define  LSM6DS0_AD_FIFO_CTRL               ((uint8_t)0x2E)

+#define  LSM6DS0_AD_FIFO_SRC                ((uint8_t)0x2F)

+#define  LSM6DS0_AD_INT_GEN_CFG_G           ((uint8_t)0x30)

+#define  LSM6DS0_AD_INT_GEN_THS_XH_G        ((uint8_t)0x31)

+#define  LSM6DS0_AD_INT_GEN_THS_XL_G        ((uint8_t)0x32)

+#define  LSM6DS0_AD_INT_GEN_THS_YH_G        ((uint8_t)0x33)

+#define  LSM6DS0_AD_INT_GEN_THS_YL_G        ((uint8_t)0x34)

+#define  LSM6DS0_AD_INT_GEN_THS_ZH_G        ((uint8_t)0x35)

+#define  LSM6DS0_AD_INT_GEN_THS_ZL_G        ((uint8_t)0x36)

+#define  LSM6DS0_AD_INT_GEN_DUR_G           ((uint8_t)0x37)

+

+#define  LSM6DS0_CTRL_REG1_G_FS_MASK        ((uint8_t)0x18)

+#define  LSM6DS0_CTRL_REG6_XL_FS_MASK       ((uint8_t)0x18)

+

+#define  TO_G                               ((float)0.001f)

+#define  TO_SI                              ((float)0.00981f)

+/*===========================================================================*/

+/* Driver exported variables.                                                */

+/*===========================================================================*/

+

+/*===========================================================================*/

+/* Driver local variables and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @brief   LSM6DS0 address increment mode.

+ */

+typedef enum {

+  LSM6DS0_IF_ADD_INC_DIS = 0x00,    /**< Address increment disabled.        */

+  LSM6DS0_IF_ADD_INC_EN = 0x04      /**< Address increment disabled.        */

+}lsm6ds0_id_add_inc_t;

+

+/**

+ * @brief   LSM6DS0 gyroscope subsystem sleep mode.

+ */

+typedef enum {

+  LSM6DS0_GYRO_SLP_DISABLED = 0x00, /**< Gyroscope in normal mode.          */

+  LSM6DS0_GYRO_SLP_ENABLED  = 0x40  /**< Gyroscope in sleep mode.           */

+}lsm6ds0_gyro_slp_t;

+

+/*===========================================================================*/

+/* Driver local functions.                                                   */

+/*===========================================================================*/

+

+#if (LSM6DS0_USE_I2C) || defined(__DOXYGEN__)

+/**

+ * @brief   Reads registers value using I2C.

+ * @pre     The I2C interface must be initialized and the driver started.

+ * @note    IF_ADD_INC bit must be 1 in CTRL_REG8

+ *

+ * @param[in] i2cp       pointer to the I2C interface

+ * @param[in] sad        slave address without R bit

+ * @param[in] reg        first sub-register address

+ * @return               the read value.

+ */

+uint8_t lsm6ds0I2CReadRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,

+                               msg_t* msgp) {

+  msg_t msg;

+#if defined(STM32F103_MCUCONF)

+  uint8_t rxbuf[2];

+  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,

+                                   TIME_INFINITE);

+  if(msgp != NULL){

+    *msgp = msg;

+  }

+  return rxbuf[0];

+#else

+  uint8_t txbuf, rxbuf;

+  txbuf = reg;

+  msg = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, &rxbuf, 1,

+                                        TIME_INFINITE);

+  if(msgp != NULL){

+    *msgp = msg;

+  }

+  return rxbuf;

+#endif

+}

+

+/**

+ * @brief   Writes a value into a register using I2C.

+ * @pre     The I2C interface must be initialized and the driver started.

+ *

+ * @param[in] i2cp       pointer to the I2C interface

+ * @param[in] sad        slave address without R bit

+ * @param[in] sub        sub-register address

+ * @param[in] value      the value to be written

+ * @return               the operation status.

+ */

+msg_t lsm6ds0I2CWriteRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,

+                              uint8_t value) {

+  uint8_t rxbuf;

+  uint8_t txbuf[2];

+  switch (reg) {

+  default:

+    /* Reserved register must not be written, according to the datasheet

+     * this could permanently damage the device.

+     */

+    osalDbgAssert(FALSE, "lsm6ds0WriteRegisterI2C(), reserved register");

+  case LSM6DS0_AD_WHO_AM_I:

+  case LSM6DS0_AD_INT_GEN_SRC_G:

+  case LSM6DS0_AD_OUT_TEMP_L:

+  case LSM6DS0_AD_OUT_TEMP_H:

+  case LSM6DS0_AD_STATUS_REG1:

+  case LSM6DS0_AD_OUT_X_L_G:

+  case LSM6DS0_AD_OUT_X_H_G:

+  case LSM6DS0_AD_OUT_Y_L_G:

+  case LSM6DS0_AD_OUT_Y_H_G:

+  case LSM6DS0_AD_OUT_Z_L_G:

+  case LSM6DS0_AD_OUT_Z_H_G:

+  case LSM6DS0_AD_INT_GEN_SRC_XL:

+  case LSM6DS0_AD_STATUS_REG2:

+  case LSM6DS0_AD_OUT_X_L_XL:

+  case LSM6DS0_AD_OUT_X_H_XL:

+  case LSM6DS0_AD_OUT_Y_L_XL:

+  case LSM6DS0_AD_OUT_Y_H_XL:

+  case LSM6DS0_AD_OUT_Z_L_XL:

+  case LSM6DS0_AD_OUT_Z_H_XL:

+  case LSM6DS0_AD_FIFO_SRC:

+  /* Read only registers cannot be written, the command is ignored.*/

+    return MSG_RESET;

+  case LSM6DS0_AD_ACT_THS:

+  case LSM6DS0_AD_ACT_DUR:

+  case LSM6DS0_AD_INT_GEN_CFG_XL:

+  case LSM6DS0_AD_INT_GEN_THS_X_XL:

+  case LSM6DS0_AD_INT_GEN_THS_Y_XL:

+  case LSM6DS0_AD_INT_GEN_THS_Z_XL:

+  case LSM6DS0_AD_INT_GEN_DUR_XL:

+  case LSM6DS0_AD_REFERENCE_G:

+  case LSM6DS0_AD_INT_CTRL:

+  case LSM6DS0_AD_CTRL_REG1_G:

+  case LSM6DS0_AD_CTRL_REG2_G:

+  case LSM6DS0_AD_CTRL_REG3_G:

+  case LSM6DS0_AD_ORIENT_CFG_G:

+  case LSM6DS0_AD_CTRL_REG4:

+  case LSM6DS0_AD_CTRL_REG5_XL:

+  case LSM6DS0_AD_CTRL_REG6_XL:

+  case LSM6DS0_AD_CTRL_REG7_XL:

+  case LSM6DS0_AD_CTRL_REG8:

+  case LSM6DS0_AD_CTRL_REG9:

+  case LSM6DS0_AD_CTRL_REG10:

+  case LSM6DS0_AD_FIFO_CTRL:

+  case LSM6DS0_AD_INT_GEN_CFG_G:

+  case LSM6DS0_AD_INT_GEN_THS_XH_G:

+  case LSM6DS0_AD_INT_GEN_THS_XL_G:

+  case LSM6DS0_AD_INT_GEN_THS_YH_G:

+  case LSM6DS0_AD_INT_GEN_THS_YL_G:

+  case LSM6DS0_AD_INT_GEN_THS_ZH_G:

+  case LSM6DS0_AD_INT_GEN_THS_ZL_G:

+  case LSM6DS0_AD_INT_GEN_DUR_G:

+    txbuf[0] = reg;

+    txbuf[1] = value;

+    return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);

+    break;

+  }

+}

+#endif /* LSM6DS0_USE_I2C */

+

+/*

+ * Interface implementation.

+ */

+static size_t acc_get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return LSM6DS0_ACC_NUMBER_OF_AXES;

+}

+

+static size_t gyro_get_axes_number(void *ip) {

+

+  osalDbgCheck(ip != NULL);

+  return LSM6DS0_GYRO_NUMBER_OF_AXES;

+}

+

+static size_t sens_get_axes_number(void *ip) {

+  size_t size = 0;

+  

+  osalDbgCheck(ip != NULL);

+  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL)

+	size += acc_get_axes_number(ip);

+  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL)

+    size += gyro_get_axes_number(ip);  

+  return size;

+}

+

+static msg_t acc_read_raw(void *ip, int32_t axes[]) {

+  int16_t tmp;

+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&

+              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+              "acc_read_raw(), invalid state");

+

+#if LSM6DS0_USE_I2C

+  osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY),

+                "acc_read_raw(), channel not ready");

+#if LSM6DS0_SHARED_I2C

+  i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);

+  i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,

+           ((LSM6DS0Driver *)ip)->config->i2ccfg);

+#endif /* LSM6DS0_SHARED_I2C */

+  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_X){

+    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                 LSM6DS0_AD_OUT_X_L_XL, NULL);

+    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                  LSM6DS0_AD_OUT_X_H_XL, NULL) << 8;

+    axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[0];

+  }

+  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Y){

+    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                 LSM6DS0_AD_OUT_Y_L_XL, NULL);

+    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                  LSM6DS0_AD_OUT_Y_H_XL, NULL) << 8;

+    axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[1];

+  }

+  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Z){

+    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                 LSM6DS0_AD_OUT_Z_L_XL, NULL);

+    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                  LSM6DS0_AD_OUT_Z_H_XL, NULL) << 8;

+    axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[2];

+  }

+#if LSM6DS0_SHARED_I2C

+  i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);

+#endif /* LSM6DS0_SHARED_I2C */

+#endif /* LSM6DS0_USE_I2C */

+  return MSG_OK;

+}

+

+static msg_t gyro_read_raw(void *ip, int32_t axes[]) {

+  int16_t tmp;

+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&

+              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+              "gyro_read_raw(), invalid state");

+

+#if LSM6DS0_USE_I2C

+  osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY),

+                "gyro_read_raw(), channel not ready");

+#if LSM6DS0_SHARED_I2C

+  i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);

+  i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,

+           ((LSM6DS0Driver *)ip)->config->i2ccfg);

+#endif /* LSM6DS0_SHARED_I2C */

+  if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_X){

+    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                   LSM6DS0_AD_OUT_X_L_G, NULL);

+    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                    LSM6DS0_AD_OUT_X_H_G, NULL) << 8;

+	axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[0];

+  }

+  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Y){

+    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                   LSM6DS0_AD_OUT_Y_L_G, NULL);

+    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                    LSM6DS0_AD_OUT_Y_H_G, NULL) << 8;

+    axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[1];

+  }

+  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Z){

+    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                   LSM6DS0_AD_OUT_Z_L_G, NULL);

+    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                    LSM6DS0_AD_OUT_Z_H_G, NULL) << 8;

+    axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[2];

+  }

+#if LSM6DS0_SHARED_I2C

+  i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);

+#endif /* LSM6DS0_SHARED_I2C */

+#endif /* LSM6DS0_USE_I2C */ 

+  return MSG_OK;

+}

+

+static msg_t sens_read_raw(void *ip, int32_t axes[]) {

+  int32_t* bp = axes;

+  msg_t msg;

+  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL) {

+	msg = acc_read_raw(ip, bp);

+	if(msg != MSG_OK)

+	  return msg;

+	bp += LSM6DS0_ACC_NUMBER_OF_AXES;

+  }

+  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL) {

+    msg = gyro_read_raw(ip, bp);

+  }

+  return msg;

+}

+	

+static msg_t acc_read_cooked(void *ip, float axes[]) {

+  uint32_t i;

+  int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&

+              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+              "acc_read_cooked(), invalid state");

+

+  msg = acc_read_raw(ip, raw);

+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES ; i++){

+    axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->accsensitivity[i];

+    if(((LSM6DS0Driver *)ip)->config->acccfg->unit == LSM6DS0_ACC_UNIT_G){

+      axes[i] *= TO_G;

+    }

+    else if(((LSM6DS0Driver *)ip)->config->acccfg->unit == LSM6DS0_ACC_UNIT_SI){

+      axes[i] *= TO_SI;

+    }

+  }

+  return msg;

+}

+

+static msg_t gyro_read_cooked(void *ip, float axes[]) {

+  uint32_t i;

+  int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];

+  msg_t msg;

+

+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&

+              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+              "gyro_read_cooked(), invalid state");

+

+  msg = gyro_read_raw(ip, raw);

+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES ; i++){

+    axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->gyrosensitivity[i];

+  }

+  return msg;

+}

+

+static msg_t sens_read_cooked(void *ip, float axes[]) {

+  float* bp = axes;

+  msg_t msg;

+  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL) {

+	msg = acc_read_cooked(ip, bp);

+	if(msg != MSG_OK)

+	  return msg;

+	bp += LSM6DS0_ACC_NUMBER_OF_AXES;

+  }

+  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL) {

+    msg = gyro_read_cooked(ip, bp);

+  }

+  return msg;

+}

+

+static msg_t gyro_sample_bias(void *ip) {

+  uint32_t i, j;

+  int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];

+  int32_t buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0, 0, 0};

+  msg_t msg;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+                "sample_bias(), invalid state");

+

+  for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){

+    msg = gyro_read_raw(ip, raw);

+	if(msg != MSG_OK)

+	  return msg;

+    for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){

+      buff[j] += raw[j];

+    }

+    osalThreadSleepMicroseconds(LSM6DS0_GYRO_BIAS_SETTLING_uS);

+  }

+

+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){

+    ((LSM6DS0Driver *)ip)->gyrobias[i] = buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES;

+  }

+  return msg;

+}

+

+static msg_t acc_set_bias(void *ip, int32_t *bp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (bp !=NULL));

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||

+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),

+                "acc_set_bias(), invalid state");

+  

+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {

+    ((LSM6DS0Driver *)ip)->accbias[i] = bp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t gyro_set_bias(void *ip, int32_t *bp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (bp !=NULL));

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||

+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),

+                "gyro_set_bias(), invalid state");

+  

+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {

+    ((LSM6DS0Driver *)ip)->gyrobias[i] = bp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t acc_reset_bias(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||

+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),

+              "acc_reset_bias(), invalid state");

+

+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+    ((LSM6DS0Driver *)ip)->accbias[i] = 0;

+  return MSG_OK;

+}

+

+static msg_t gyro_reset_bias(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||

+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),

+              "gyro_reset_bias(), invalid state");

+

+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)

+    ((LSM6DS0Driver *)ip)->gyrobias[i] = 0;

+  return MSG_OK;

+}

+

+static msg_t acc_set_sensivity(void *ip, float *sp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (sp !=NULL));

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+                "acc_set_sensivity(), invalid state");

+  

+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {

+    ((LSM6DS0Driver *)ip)->accsensitivity[i] = sp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t gyro_set_sensivity(void *ip, float *sp) {

+  uint32_t i;

+  

+  osalDbgCheck((ip != NULL) && (sp !=NULL));

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+                "gyro_set_sensivity(), invalid state");

+  

+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {

+    ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = sp[i];

+  }

+  return MSG_OK;

+}

+

+static msg_t acc_reset_sensivity(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+                "acc_reset_sensivity(), invalid state");

+

+  if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_2G)

+    for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;

+  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G)

+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;

+  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G)

+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;

+  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)

+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");

+    return MSG_RESET;

+  }

+  return MSG_OK;

+}

+

+static msg_t gyro_reset_sensivity(void *ip) {

+  uint32_t i;

+

+  osalDbgCheck(ip != NULL);

+

+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),

+                "gyro_reset_sensivity(), invalid state");

+

+  if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)

+    for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+      ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;

+  else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DSP)

+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+      ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;

+  else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)

+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+      ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;

+  else {

+    osalDbgAssert(FALSE, "reset_sensivity(), gyroscope full scale issue");

+    return MSG_RESET;

+  }

+  return MSG_OK;

+}

+

+static msg_t sens_get_temperature(void *ip, float* tempp) {

+  int16_t temp;

+#if LSM6DS0_USE_I2C

+  osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY),

+                "gyro_read_raw(), channel not ready");

+#if LSM6DS0_SHARED_I2C

+  i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);

+  i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,

+           ((LSM6DS0Driver *)ip)->config->i2ccfg);

+#endif /* LSM6DS0_SHARED_I2C */

+  temp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                  LSM6DS0_AD_OUT_TEMP_L, NULL);

+  temp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,

+                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,

+                                   LSM6DS0_AD_OUT_TEMP_H, NULL) << 8;

+#if LSM6DS0_SHARED_I2C

+  i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);

+#endif /* LSM6DS0_SHARED_I2C */

+#endif /* LSM6DS0_USE_I2C */

+  *tempp = ((float)temp / LSM6DS0_TEMP_SENS) + LSM6DS0_TEMP_SENS_OFF;

+  return MSG_OK;

+}

+

+static const struct BaseSensorVMT vmt_basesensor = {

+  sens_get_axes_number, sens_read_raw, sens_read_cooked

+};

+

+static const struct BaseGyroscopeVMT vmt_basegyroscope = {

+  gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,

+  gyro_sample_bias, gyro_set_bias, gyro_reset_bias,

+  gyro_set_sensivity, gyro_reset_sensivity

+};

+

+static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {

+  acc_get_axes_number, acc_read_raw, acc_read_cooked,

+  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity

+};

+

+static const struct LSM6DS0ACCVMT vmt_lsm6ds0acc = {

+  acc_get_axes_number, acc_read_raw, acc_read_cooked,

+  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity

+};

+

+static const struct LSM6DS0GYROVMT vmt_lsm6ds0gyro = {

+  gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,

+  gyro_sample_bias, gyro_set_bias, gyro_reset_bias,

+  gyro_set_sensivity, gyro_reset_sensivity, sens_get_temperature

+};

+

+/*===========================================================================*/

+/* Driver exported functions.                                                */

+/*===========================================================================*/

+

+/**

+ * @brief   Initializes an instance.

+ *

+ * @param[out] devp     pointer to the @p LSM6DS0Driver object

+ *

+ * @init

+ */

+void lsm6ds0ObjectInit(LSM6DS0Driver *devp) {

+  uint32_t i;

+  devp->vmt_basesensor = &vmt_basesensor;

+  devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;

+  devp->vmt_basegyroscope = &vmt_basegyroscope;

+  devp->vmt_lsm6ds0acc = &vmt_lsm6ds0acc;

+  devp->vmt_lsm6ds0gyro = &vmt_lsm6ds0gyro;

+  devp->config = NULL;

+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+    devp->accbias[i] = 0;

+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)

+    devp->gyrobias[i] = 0;

+  devp->state  = LSM6DS0_STOP;

+}

+

+/**

+ * @brief   Configures and activates LSM6DS0 Complex Driver peripheral.

+ *

+ * @param[in] devp      pointer to the @p LSM6DS0Driver object

+ * @param[in] config    pointer to the @p LSM6DS0Config object

+ *

+ * @api

+ */

+void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {

+  uint32_t i;

+  osalDbgCheck((devp != NULL) && (config != NULL));

+

+  osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),

+              "lsm6ds0Start(), invalid state");			  

+

+  devp->config = config;

+

+#if LSM6DS0_USE_I2C

+#if	LSM6DS0_SHARED_I2C

+  i2cAcquireBus((devp)->config->i2cp);

+#endif /* LSM6DS0_SHARED_I2C */

+  i2cStart((devp)->config->i2cp,

+           (devp)->config->i2ccfg);

+  if((devp)->config->acccfg != NULL) {

+    lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                            devp->config->slaveaddress,

+                            LSM6DS0_AD_CTRL_REG5_XL,

+                            devp->config->acccfg->decmode |

+                            devp->config->acccfg->axesenabling);

+    lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                            devp->config->slaveaddress,

+                            LSM6DS0_AD_CTRL_REG6_XL,

+                            devp->config->acccfg->outdatarate |

+                            devp->config->acccfg->fullscale );

+  }

+  if((devp)->config->gyrocfg != NULL) {

+    lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                            devp->config->slaveaddress,

+                            LSM6DS0_AD_CTRL_REG1_G,

+                            devp->config->gyrocfg->fullscale |

+                            devp->config->gyrocfg->outdatarate);

+    lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                            devp->config->slaveaddress,

+                            LSM6DS0_AD_CTRL_REG2_G,

+                            devp->config->gyrocfg->outsel);

+    lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                            devp->config->slaveaddress,

+                            LSM6DS0_AD_CTRL_REG3_G,

+                            devp->config->gyrocfg->hpfenable |

+                            devp->config->gyrocfg->lowmodecfg |

+                            devp->config->gyrocfg->hpcfg);

+    lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                            devp->config->slaveaddress,

+                            LSM6DS0_AD_CTRL_REG4,

+                            devp->config->gyrocfg->axesenabling);

+    lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                            devp->config->slaveaddress,

+                            LSM6DS0_AD_CTRL_REG9,

+                            LSM6DS0_GYRO_SLP_DISABLED);

+  }

+  lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                          devp->config->slaveaddress,

+                          LSM6DS0_AD_CTRL_REG8,

+                          devp->config->endianness |

+                          devp->config->blockdataupdate);

+#if	LSM6DS0_SHARED_I2C

+  i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM6DS0_SHARED_I2C */  

+#endif /* LSM6DS0_USE_I2C */

+  /* Storing sensitivity information according to full scale value */

+  if((devp)->config->acccfg != NULL) {

+    if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_2G)

+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;

+    else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G)

+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;

+    else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G)

+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;

+    else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)

+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)

+        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;

+    else

+      osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");

+  }

+  if((devp)->config->gyrocfg != NULL) {

+    if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)

+      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)

+        devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;

+    else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DSP)

+      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)

+        devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;

+    else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)

+      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)

+        devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;

+    else

+      osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");

+  }

+  /* This is the Gyroscope transient recovery time */

+  osalThreadSleepMilliseconds(5);

+

+  devp->state = LSM6DS0_READY;

+} 

+

+/**

+ * @brief   Deactivates the LSM6DS0 Complex Driver peripheral.

+ *

+ * @param[in] devp       pointer to the @p LSM6DS0Driver object

+ *

+ * @api

+ */

+void lsm6ds0Stop(LSM6DS0Driver *devp) {

+

+  osalDbgCheck(devp != NULL);

+

+  osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),

+                "lsm6ds0Stop(), invalid state");

+

+#if (LSM6DS0_USE_I2C)

+  if (devp->state == LSM6DS0_STOP) {

+#if	LSM6DS0_SHARED_I2C

+    i2cAcquireBus((devp)->config->i2cp);

+    i2cStart((devp)->config->i2cp,

+             (devp)->config->i2ccfg);

+#endif /* LSM6DS0_SHARED_I2C */

+    if((devp)->config->acccfg != NULL) {

+      lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                              devp->config->slaveaddress,

+                              LSM6DS0_AD_CTRL_REG6_XL,

+                              LSM6DS0_ACC_ODR_PD);

+    }

+    if((devp)->config->gyrocfg != NULL) {

+      lsm6ds0I2CWriteRegister(devp->config->i2cp,

+                              devp->config->slaveaddress,

+                              LSM6DS0_AD_CTRL_REG9,

+                              LSM6DS0_GYRO_SLP_ENABLED);

+    }

+    i2cStop((devp)->config->i2cp);

+#if	LSM6DS0_SHARED_I2C

+    i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM6DS0_SHARED_I2C */    

+  }			  

+#endif /* LSM6DS0_USE_I2C */

+  devp->state = LSM6DS0_STOP;

+}

+/** @} */

diff --git a/os/ex/ST/lsm6ds0.h b/os/ex/ST/lsm6ds0.h
index a43bf7ad9..a2cee28ec 100644
--- a/os/ex/ST/lsm6ds0.h
+++ b/os/ex/ST/lsm6ds0.h
@@ -1,604 +1,604 @@
-/*
-    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
-
-    This file is part of ChibiOS.
-
-    ChibiOS is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 3 of the License, or
-    (at your option) any later version.
-
-    ChibiOS is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-	
-*/
-
-/**
- * @file    lsm6ds0.h
- * @brief   LSM6DS0 MEMS interface module header.
- *
- * @{
- */
-#ifndef _LSM6DS0_H_
-#define _LSM6DS0_H_
-
-#include "hal_accelerometer.h"
-#include "hal_gyroscope.h"
-
-/*===========================================================================*/
-/* Driver constants.                                                         */
-/*===========================================================================*/
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem number of axes.
- */
-#define  LSM6DS0_ACC_NUMBER_OF_AXES         ((size_t)   3U)
-
-/**
- * @brief   LSM6DS0 gyroscope subsystem number of axes.
- */
-#define  LSM6DS0_GYRO_NUMBER_OF_AXES        ((size_t)   3U)
-
-/*===========================================================================*/
-/* Driver pre-compile time settings.                                         */
-/*===========================================================================*/
-
-/**
- * @name    Configuration options
- * @{
- */
-/**
- * @brief   LSM6DS0 SPI interface selector.
- * @details If set to @p TRUE the support for SPI is included.
- * @note    The default is @p FALSE.
- */
-#if !defined(LSM6DS0_USE_SPI) || defined(__DOXYGEN__)
-#define LSM6DS0_USE_SPI                     FALSE
-#endif
-
-/**
- * @brief   LSM6DS0 I2C interface selector.
- * @details If set to @p TRUE the support for I2C is included.
- * @note    The default is @p TRUE.
- */
-#if !defined(LSM6DS0_USE_I2C) || defined(__DOXYGEN__)
-#define LSM6DS0_USE_I2C                     TRUE
-#endif
-
-/**
- * @brief   LSM6DS0 shared I2C switch.
- * @details If set to @p TRUE the device acquires I2C bus ownership
- *          on each transaction.
- * @note    The default is @p FALSE. Requires I2C_USE_MUTUAL_EXCLUSION.
- */
-#if !defined(LSM6DS0_SHARED_I2C) || defined(__DOXYGEN__)
-#define LSM6DS0_SHARED_I2C                  FALSE
-#endif
-
-/**
- * @brief   Number of acquisitions for gyroscope bias removal.
- * @details This is the number of acquisitions performed to compute the
- *          bias. A repetition is required in order to remove noise.
- */
-#if !defined(LSM6DS0_GYRO_BIAS_ACQ_TIMES) || defined(__DOXYGEN__)
-#define LSM6DS0_GYRO_BIAS_ACQ_TIMES         50
-#endif
-
-/**
- * @brief   Settling time for gyroscope bias removal.
- * @details This is the time between each bias acquisition.
- */
-#if !defined(LSM6DS0_GYRO_BIAS_SETTLING_uS) || defined(__DOXYGEN__)
-#define LSM6DS0_GYRO_BIAS_SETTLING_uS       5000
-#endif
-/** @} */
-
-/*===========================================================================*/
-/* Derived constants and error checks.                                       */
-/*===========================================================================*/
-
-#if LSM6DS0_USE_SPI && LSM6DS0_USE_I2C
-#error "LSM6DS0_USE_SPI and LSM6DS0_USE_I2C cannot be both true"
-#endif
-
-#if LSM6DS0_USE_SPI && !HAL_USE_SPI
-#error "LSM6DS0_USE_SPI requires HAL_USE_SPI"
-#endif
-
-#if LSM6DS0_USE_I2C && !HAL_USE_I2C
-#error "LSM6DS0_USE_I2C requires HAL_USE_I2C"
-#endif
-
-#if LSM6DS0_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION
-#error "LSM6DS0_SHARED_SPI requires I2C_USE_MUTUAL_EXCLUSION"
-#endif
-
-/*===========================================================================*/
-/* Driver data structures and types.                                         */
-/*===========================================================================*/
-
-/**
- * @name    LSM6DS0 accelerometer subsystem data structures and types.
- * @{
- */
- 
-/**
- * @brief   LSM6DS0 accelerometer subsystem full scale.
- */
-typedef enum {
-  LSM6DS0_ACC_FS_2G = 0x00,         /**< Full scale ±2g.                    */
-  LSM6DS0_ACC_FS_4G = 0x10,         /**< Full scale ±4g.                    */
-  LSM6DS0_ACC_FS_8G = 0x18,         /**< Full scale ±8g.                    */
-  LSM6DS0_ACC_FS_16G = 0x08         /**< Full scale ±16g.                   */
-} lsm6ds0_acc_fs_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem output data rate.
- */
-typedef enum {
-  LSM6DS0_ACC_ODR_PD = 0x00,        /**< Power down                         */
-  LSM6DS0_ACC_ODR_10Hz = 0x20,      /**< ODR 10 Hz                          */
-  LSM6DS0_ACC_ODR_50Hz = 0x40,      /**< ODR 50 Hz                          */
-  LSM6DS0_ACC_ODR_119Hz = 0x60,     /**< ODR 119 Hz                         */
-  LSM6DS0_ACC_ODR_238Hz = 0x80,     /**< ODR 238 Hz                         */
-  LSM6DS0_ACC_ODR_476Hz = 0xA0,     /**< ODR 476 Hz                         */
-  LSM6DS0_ACC_ODR_952Hz = 0xC0      /**< ODR 952 Hz                         */
-} lsm6ds0_acc_odr_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem axes enabling.
- */
-typedef enum {
-  LSM6DS0_ACC_AE_DISABLED = 0x00,   /**< All axes disabled.                 */
-  LSM6DS0_ACC_AE_X = 0x08,          /**< Only X-axis enabled.               */
-  LSM6DS0_ACC_AE_Y = 0x10,          /**< Only Y-axis enabled.               */
-  LSM6DS0_ACC_AE_XY = 0x18,         /**< X and Y axes enabled.              */
-  LSM6DS0_ACC_AE_Z = 0x20,          /**< Only Z-axis enabled.               */
-  LSM6DS0_ACC_AE_XZ = 0x28,         /**< X and Z axes enabled.              */
-  LSM6DS0_ACC_AE_YZ = 0x30,         /**< Y and Z axes enabled.              */
-  LSM6DS0_ACC_AE_XYZ = 0x38         /**< All axes enabled.                  */
-} lsm6ds0_acc_ae_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.
- */
-typedef enum {
-  LSM6DS0_ACC_OBW_ODR = 0x00,      /**< Depending on ODR.                   */
-  LSM6DS0_ACC_OBW_AA = 0x04        /**< Same of the Anti aliasing.          */
-} lsm6ds0_acc_obw_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem high resolution mode.
- */
-typedef enum {
-  LSM6DS0_ACC_HR_DISABLED = 0x00,   /**< High resolution mode disabled.     */
-  LSM6DS0_ACC_HR_ENABLED = 0x80     /**< High resolution mode enabled.      */
-} lsm6ds0_acc_hr_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem filtered data selection.
- */
-typedef enum {
-  LSM6DS0_ACC_FDS_ENABLED = 0x00,   /**< Internal filter bypassed.          */
-  LSM6DS0_ACC_FDS_DISABLED = 0x04   /**< Internal filter not bypassed.      */
-} lsm6ds0_acc_fds_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem digital filter.
- */
-typedef enum {
-  LSM6DS0_ACC_DCF_400 = 0x00,       /**< Low pass cutoff freq. ODR/400 Hz.  */
-  LSM6DS0_ACC_DCF_100 = 0x20,       /**< Low pass cutoff freq. ODR/100 Hz.  */
-  LSM6DS0_ACC_DCF_50 = 0x60,        /**< Low pass cutoff freq. ODR/50 Hz.   */
-  LSM6DS0_ACC_DCF_9 = 0x40          /**< Low pass cutoff freq. ODR/9 Hz.    */
-} lsm6ds0_acc_dcf_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.
- */
-typedef enum {
-  LSM6DS0_ACC_AABW_408Hz = 0x00,    /**< Anti Aliasing filter BW 408Hz.     */
-  LSM6DS0_ACC_AABW_211Hz = 0x01,    /**< Anti Aliasing filter BW 211Hz.     */
-  LSM6DS0_ACC_AABW_105Hz = 0x02,    /**< Anti Aliasing filter BW 105Hz.     */
-  LSM6DS0_ACC_AABW_50Hz = 0x03,     /**< Anti Aliasing filter BW 50Hz.      */
-  LSM6DS0_ACC_AABW_AUTO = 0x04      /**< Anti Aliasing filter BW auto.      */
-} lsm6ds0_acc_aabw_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem decimation mode.
- */
-typedef enum {
-  LSM6DS0_ACC_DEC_DISABLED = 0x00,  /**< Decimation disabled.               */
-  LSM6DS0_ACC_DEC_X2 = 0x40,        /**< Output updated every 2 samples.    */
-  LSM6DS0_ACC_DEC_X4 = 0x80,        /**< Output updated every 4 samples.    */
-  LSM6DS0_ACC_DEC_X8 = 0xC0         /**< Output updated every 8 samples.    */
-} lsm6ds0_acc_dec_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem unit.
- */
-typedef enum {
-  LSM6DS0_ACC_UNIT_G = 0x00,        /**< Cooked data in g.                  */
-  LSM6DS0_ACC_UNIT_MG = 0x01,       /**< Cooked data in mg.                 */
-  LSM6DS0_ACC_UNIT_SI = 0x02,       /**< Cooked data in m/s^2.              */
-} lsm6ds0_acc_unit_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem configuration structure.
- */
-typedef struct {
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem full scale.
-   */
-  lsm6ds0_acc_fs_t          fullscale;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem output data rate.
-   */
-  lsm6ds0_acc_odr_t         outdatarate;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem axes enabling.
-   */
-  lsm6ds0_acc_ae_t          axesenabling;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem output bandwidth.
-   */
-  lsm6ds0_acc_obw_t         outbandwidth;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem high resolution mode.
-   */
-  lsm6ds0_acc_hr_t          highresmode;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem filtered data selection.
-   */
-  lsm6ds0_acc_fds_t         filtdatasel;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem digital filter.
-   */
-  lsm6ds0_acc_dcf_t         digifilter;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.
-   */
-  lsm6ds0_acc_aabw_t        aabandwidth;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem decimation mode.
-   */
-  lsm6ds0_acc_dec_t         decmode;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem unit.
-   */
-  lsm6ds0_acc_unit_t        unit;
-} LSM6DS0AccConfig;
-/** @} */
-
-/**
- * @name    LSM6DS0 gyroscope subsystem data structures and types.
- * @{
- */
-/**
- * @brief   LSM6DS0 gyroscope subsystem full scale.
- */
-typedef enum {
-  LSM6DS0_GYRO_FS_245DSP  = 0x00,   /**< Full scale ±245 degree per second  */
-  LSM6DS0_GYRO_FS_500DSP  = 0x08,   /**< Full scale ±500 degree per second  */
-  LSM6DS0_GYRO_FS_2000DSP = 0x18    /**< Full scale ±2000 degree per second */
-} lsm6ds0_gyro_fs_t;
-
-/**
- * @brief   LSM6DS0 gyroscope subsystem output data rate.
- */
-typedef enum {
-  LSM6DS0_GYRO_ODR_PD = 0x00,
-  LSM6DS0_GYRO_ODR_95HZ_FC_25 = 0x10,
-  LSM6DS0_GYRO_ODR_14_9HZ_FC_5 = 0X20,
-  LSM6DS0_GYRO_ODR_59_5HZ_FC_16 = 0X40,
-  LSM6DS0_GYRO_ODR_119HZ_FC_14 = 0X60,
-  LSM6DS0_GYRO_ODR_119HZ_FC_31 = 0X61,
-  LSM6DS0_GYRO_ODR_238HZ_FC_14 = 0X80,
-  LSM6DS0_GYRO_ODR_238HZ_FC_29 = 0X81,
-  LSM6DS0_GYRO_ODR_238HZ_FC_63 = 0X82,
-  LSM6DS0_GYRO_ODR_238HZ_FC_78 = 0X83,
-  LSM6DS0_GYRO_ODR_476HZ_FC_21 = 0XA0,
-  LSM6DS0_GYRO_ODR_476HZ_FC_28 = 0XA1,
-  LSM6DS0_GYRO_ODR_476HZ_FC_57 = 0XA2,
-  LSM6DS0_GYRO_ODR_476HZ_FC_100 = 0XA3,
-  LSM6DS0_GYRO_ODR_952HZ_FC_33 = 0XC0,
-  LSM6DS0_GYRO_ODR_952HZ_FC_40 = 0XC1,
-  LSM6DS0_GYRO_ODR_952HZ_FC_58 = 0XC2,
-  LSM6DS0_GYRO_ODR_952HZ_FC_100 = 0XC3
-} lsm6ds0_gyro_odr_t;
-
-/**
- * @brief   LSM6DS0 gyroscope subsystem axes enabling.
- */
-typedef enum {
-  LSM6DS0_GYRO_AE_DISABLED = 0x00,  /**< All axes disabled.                 */
-  LSM6DS0_GYRO_AE_X = 0x08,         /**< Only X-axis enabled.               */
-  LSM6DS0_GYRO_AE_Y = 0x10,         /**< Only Y-axis enabled.               */
-  LSM6DS0_GYRO_AE_XY = 0x18,        /**< X and Y axes enabled.              */
-  LSM6DS0_GYRO_AE_Z = 0x20,         /**< Only Z-axis enabled.               */
-  LSM6DS0_GYRO_AE_XZ = 0x28,        /**< X and Z axes enabled.              */
-  LSM6DS0_GYRO_AE_YZ = 0x30,        /**< Y and Z axes enabled.              */
-  LSM6DS0_GYRO_AE_XYZ = 0x38        /**< All axes enabled.                  */
-} lsm6ds0_gyro_ae_t;
-
-/**
- * @brief   LSM6DS0 gyroscope subsystem low mode configuration.
- */
-typedef enum {
-  LSM6DS0_GYRO_LP_DISABLED = 0x00,  /**< Low power mode disabled.           */
-  LSM6DS0_GYRO_LP_ENABLED = 0x80    /**< Low power mode enabled.            */
-} lsm6ds0_gyro_lp_t;
-
-/**
- * @brief  LSM6DS0 gyroscope subsystem output selection.
- */
-typedef enum {
-  LSM6DS0_GYRO_OUT_SEL_0 = 0x00,    /**< Low pass filter 1.                 */
-  LSM6DS0_GYRO_OUT_SEL_1 = 0x01,    /**< High pass filter 1 if enabled.     */
-  LSM6DS0_GYRO_OUT_SEL_2 = 0x02     /**< Low pass filter 2.                 */
-} lsm6ds0_gyro_out_sel_t;
-
-/**
- * @brief  LSM6DS0 gyroscope subsystem high pass filter.
- */
-typedef enum {
-  LSM6DS0_GYRO_HP_DISABLED = 0x00,  /**< High pass filter disabled.         */
-  LSM6DS0_GYRO_HP_ENABLED = 0x40    /**< High pass filter enabled.          */
-} lsm6ds0_gyro_hp_t;
-
-/**
- * @brief  LSM6DS0 gyroscope subsystem high pass filter configuration.
- */
-typedef enum {
-  LSM6DS0_GYRO_HPCF_0 = 0x00,       /**< Refer to table 48 of RM            */
-  LSM6DS0_GYRO_HPCF_1 = 0x01, 
-  LSM6DS0_GYRO_HPCF_2 = 0x02, 
-  LSM6DS0_GYRO_HPCF_3 = 0x03, 
-  LSM6DS0_GYRO_HPCF_4 = 0x04, 
-  LSM6DS0_GYRO_HPCF_5 = 0x05, 
-  LSM6DS0_GYRO_HPCF_6 = 0x06, 
-  LSM6DS0_GYRO_HPCF_7 = 0x07, 
-  LSM6DS0_GYRO_HPCF_8 = 0x08, 
-  LSM6DS0_GYRO_HPCF_9 = 0x09 
-} lsm6ds0_gyro_hpcf_t;
-
-/**
- * @brief   LSM6DS0 gyroscope subsystem configuration structure.
- */
-typedef struct {
-  /**
-   * @brief   LSM6DS0 gyroscope subsystem full scale.
-   */
-  lsm6ds0_gyro_fs_t         fullscale;
-  /**
-   * @brief   LSM6DS0 gyroscope subsystem output data rate.
-   */
-  lsm6ds0_gyro_odr_t        outdatarate;
-  /**
-   * @brief   LSM6DS0 gyroscope subsystem axes enabling.
-   */
-  lsm6ds0_gyro_ae_t         axesenabling;
-  /**
-   * @brief   LSM6DS0 gyroscope subsystem low mode configuration.
-   */
-  lsm6ds0_gyro_lp_t         lowmodecfg;
-  /**
-   * @brief   LSM6DS0 gyroscope subsystem output selection.
-   */
-  lsm6ds0_gyro_out_sel_t    outsel;
-  /**
-   * @brief   LSM6DS0 gyroscope subsystem high pass filter.
-   */
-  lsm6ds0_gyro_hp_t         hpfenable;
-  /**
-   * @brief   LSM6DS0 gyroscope subsystem high pass filter configuration.
-   */
-  lsm6ds0_gyro_hpcf_t       hpcfg;
-} LSM6DS0GyroConfig;
-/** @} */
-
-/**
- * @name    LSM6DS0 main system data structures and types.
- * @{
- */
-/**
- * @brief  Accelerometer and Gyroscope Slave Address.
- */
-typedef enum {
-  LSM6DS0_SAD_GND = 0x6A,           /**< SAD pin connected to GND.          */
-  LSM6DS0_SAD_VCC = 0x6B            /**< SAD pin connected to VCC.          */
-} lsm6ds0_sad_t;
-
-/**
- * @brief   LSM6DS0 block data update.
- */
-typedef enum {
-  LSM6DS0_BDU_CONTINUOUS = 0x00,    /**< Block data continuously updated.   */
-  LSM6DS0_BDU_BLOCKED = 0x40        /**< Block data updated after reading.  */
-} lsm6ds0_bdu_t;
-
-/**
- * @brief   LSM6DS0 endianness.
- */
-typedef enum {
-  LSM6DS0_END_LITTLE = 0x00,        /**< Little endian.                     */
-  LSM6DS0_END_BIG = 0x20            /**< Big endian.                        */
-} lsm6ds0_end_t;
-
-/**
- * @brief   Driver state machine possible states.
- */
-typedef enum {
-  LSM6DS0_UNINIT = 0,               /**< Not initialized.                   */
-  LSM6DS0_STOP = 1,                 /**< Stopped.                           */
-  LSM6DS0_READY = 2,                /**< Ready.                             */
-} lsm6ds0_state_t;
-
-/**
- * @brief   LSM6DS0 configuration structure.
- */
-typedef struct {
-#if (LSM6DS0_USE_SPI) || defined(__DOXYGEN__)
-  /**
-   * @brief SPI driver associated to this LSM6DS0.
-   */
-  SPIDriver                 *spip;
-  /**
-   * @brief SPI configuration associated to this LSM6DS0 accelerometer
-   *        subsystem.
-   */
-  const SPIConfig           *accspicfg;
-  /**
-   * @brief SPI configuration associated to this LSM6DS0 compass
-   *        subsystem.
-   */
-  const SPIConfig           *gyrospicfg;
-#endif /* LSM6DS0_USE_SPI */
-#if (LSM6DS0_USE_I2C) || defined(__DOXYGEN__)
-  /**
-   * @brief I2C driver associated to this LSM6DS0.
-   */
-  I2CDriver                 *i2cp;
-  /**
-   * @brief I2C configuration associated to this LSM6DS0 accelerometer
-   *        subsystem.
-   */
-  const I2CConfig           *i2ccfg;
-#endif /* LSM6DS0_USE_I2C */
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem configuration structure
-   */
-  const LSM6DS0AccConfig    *acccfg;
-  /**
-   * @brief   LSM6DS0 gyroscope subsystem configuration structure
-   */
-  const LSM6DS0GyroConfig   *gyrocfg;
-  /**
-   * @brief  Accelerometer and Gyroscope Slave Address
-   */
-  lsm6ds0_sad_t             slaveaddress;
-  /**
-   * @brief   LSM6DS0 block data update
-   */
-  lsm6ds0_bdu_t             blockdataupdate;
-  /**
-   * @brief   LSM6DS0  endianness
-   */
-  lsm6ds0_end_t             endianness;
-} LSM6DS0Config;
-
-/**
- * @brief   Structure representing a LSM6DS0 driver.
- */
-typedef struct LSM6DS0Driver LSM6DS0Driver;
-
-/**
- * @brief   @p LSM6DS0 accelerometer subsystem specific methods.
- */
-#define _lsm6ds0_acc_methods                                                \
-  _base_accelerometer_methods
-  
-/**
- * @brief   @p LSM6DS0 gyroscope subsystem specific methods.
- */
-#define _lsm6ds0_gyro_methods                                               \
-  _base_gyroscope_methods                                                   \
-  /* Retrieve the temperature of LSM6DS0 chip.*/                            \
-  msg_t (*get_temperature)(void *instance, float* temperature);
-  
-/**
- * @extends BaseAccelerometerVMT
- *
- * @brief   @p LSM6DS0 accelerometer virtual methods table.
- */
-struct LSM6DS0ACCVMT {
-  _lsm6ds0_acc_methods
-};
-
-/**
- * @extends BaseCompassVMT
- *
- * @brief   @p LSM6DS0 gyroscope virtual methods table.
- */
-struct LSM6DS0GYROVMT {
-  _lsm6ds0_gyro_methods
-};
-
-/**
- * @brief   @p LSM6DS0Driver specific data.
- */
-#define _lsm6ds0_data                                                       \
-  _base_accelerometer_data                                                  \
-  _base_gyroscope_data                                                      \
-  /* Driver state.*/                                                        \
-  lsm6ds0_state_t           state;                                          \
-  /* Current configuration data.*/                                          \
-  const LSM6DS0Config       *config;                                        \
-  /* Current accelerometer sensitivity.*/                                   \
-  float                     accsensitivity[LSM6DS0_ACC_NUMBER_OF_AXES];     \
-  /* Accelerometer bias data.*/                                             \
-  int32_t                   accbias[LSM6DS0_ACC_NUMBER_OF_AXES];            \
-  /* Current gyroscope sensitivity.*/                                       \
-  float                     gyrosensitivity[LSM6DS0_GYRO_NUMBER_OF_AXES];   \
-  /* Bias data.*/                                                           \
-  int32_t                   gyrobias[LSM6DS0_GYRO_NUMBER_OF_AXES];
-  
-/**
- * @brief   LSM6DS0 6-axis accelerometer/gyroscope class.
- */
-struct LSM6DS0Driver {
-  /** @brief BaseSensor Virtual Methods Table. */
-  const struct BaseSensorVMT *vmt_basesensor;
-  /** @brief BaseAccelerometer Virtual Methods Table. */
-  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;
-  /** @brief BaseGyroscope Virtual Methods Table. */
-  const struct BaseGyroscopeVMT *vmt_basegyroscope;
-  /** @brief LSM6DS0 Accelerometer Virtual Methods Table. */
-  const struct LSM6DS0ACCVMT *vmt_lsm6ds0acc;
-  /** @brief LSM6DS0 Gyroscope Virtual Methods Table. */
-  const struct LSM6DS0GYROVMT *vmt_lsm6ds0gyro;
-  _lsm6ds0_data
-};
-/** @} */
-
-/*===========================================================================*/
-/* Driver macros.                                                            */
-/*===========================================================================*/
-
-/**
- * @brief   Get current MEMS temperature.
- * @detail  This information is very useful especially for high accuracy IMU
- *
- * @param[in] ip        pointer to a @p BaseGyroscope class.
- * @param[out] temp     the MEMS temperature as single precision floating.
- *
- * @return              The operation status.
- * @retval MSG_OK       if the function succeeded.
- * @retval MSG_RESET    if one or more errors occurred.
- * @api
- */
-#define gyroscopeGetTemp(ip, tpp)                                           \
-        (ip)->vmt_lsm6ds0gyro->get_temperature(ip, tpp)
-	
-/*===========================================================================*/
-/* External declarations.                                                    */
-/*===========================================================================*/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-  void lsm6ds0ObjectInit(LSM6DS0Driver *devp);
-  void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config);
-  void lsm6ds0Stop(LSM6DS0Driver *devp);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* _LSM6DS0_H_ */
-
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi

+

+    This file is part of ChibiOS.

+

+    ChibiOS is free software; you can redistribute it and/or modify

+    it under the terms of the GNU General Public License as published by

+    the Free Software Foundation; either version 3 of the License, or

+    (at your option) any later version.

+

+    ChibiOS is distributed in the hope that it will be useful,

+    but WITHOUT ANY WARRANTY; without even the implied warranty of

+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+    GNU General Public License for more details.

+

+    You should have received a copy of the GNU General Public License

+    along with this program.  If not, see <http://www.gnu.org/licenses/>.

+	

+*/

+

+/**

+ * @file    lsm6ds0.h

+ * @brief   LSM6DS0 MEMS interface module header.

+ *

+ * @{

+ */

+#ifndef _LSM6DS0_H_

+#define _LSM6DS0_H_

+

+#include "hal_accelerometer.h"

+#include "hal_gyroscope.h"

+

+/*===========================================================================*/

+/* Driver constants.                                                         */

+/*===========================================================================*/

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem number of axes.

+ */

+#define  LSM6DS0_ACC_NUMBER_OF_AXES         ((size_t)   3U)

+

+/**

+ * @brief   LSM6DS0 gyroscope subsystem number of axes.

+ */

+#define  LSM6DS0_GYRO_NUMBER_OF_AXES        ((size_t)   3U)

+

+/*===========================================================================*/

+/* Driver pre-compile time settings.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    Configuration options

+ * @{

+ */

+/**

+ * @brief   LSM6DS0 SPI interface selector.

+ * @details If set to @p TRUE the support for SPI is included.

+ * @note    The default is @p FALSE.

+ */

+#if !defined(LSM6DS0_USE_SPI) || defined(__DOXYGEN__)

+#define LSM6DS0_USE_SPI                     FALSE

+#endif

+

+/**

+ * @brief   LSM6DS0 I2C interface selector.

+ * @details If set to @p TRUE the support for I2C is included.

+ * @note    The default is @p TRUE.

+ */

+#if !defined(LSM6DS0_USE_I2C) || defined(__DOXYGEN__)

+#define LSM6DS0_USE_I2C                     TRUE

+#endif

+

+/**

+ * @brief   LSM6DS0 shared I2C switch.

+ * @details If set to @p TRUE the device acquires I2C bus ownership

+ *          on each transaction.

+ * @note    The default is @p FALSE. Requires I2C_USE_MUTUAL_EXCLUSION.

+ */

+#if !defined(LSM6DS0_SHARED_I2C) || defined(__DOXYGEN__)

+#define LSM6DS0_SHARED_I2C                  FALSE

+#endif

+

+/**

+ * @brief   Number of acquisitions for gyroscope bias removal.

+ * @details This is the number of acquisitions performed to compute the

+ *          bias. A repetition is required in order to remove noise.

+ */

+#if !defined(LSM6DS0_GYRO_BIAS_ACQ_TIMES) || defined(__DOXYGEN__)

+#define LSM6DS0_GYRO_BIAS_ACQ_TIMES         50

+#endif

+

+/**

+ * @brief   Settling time for gyroscope bias removal.

+ * @details This is the time between each bias acquisition.

+ */

+#if !defined(LSM6DS0_GYRO_BIAS_SETTLING_uS) || defined(__DOXYGEN__)

+#define LSM6DS0_GYRO_BIAS_SETTLING_uS       5000

+#endif

+/** @} */

+

+/*===========================================================================*/

+/* Derived constants and error checks.                                       */

+/*===========================================================================*/

+

+#if LSM6DS0_USE_SPI && LSM6DS0_USE_I2C

+#error "LSM6DS0_USE_SPI and LSM6DS0_USE_I2C cannot be both true"

+#endif

+

+#if LSM6DS0_USE_SPI && !HAL_USE_SPI

+#error "LSM6DS0_USE_SPI requires HAL_USE_SPI"

+#endif

+

+#if LSM6DS0_USE_I2C && !HAL_USE_I2C

+#error "LSM6DS0_USE_I2C requires HAL_USE_I2C"

+#endif

+

+#if LSM6DS0_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION

+#error "LSM6DS0_SHARED_SPI requires I2C_USE_MUTUAL_EXCLUSION"

+#endif

+

+/*===========================================================================*/

+/* Driver data structures and types.                                         */

+/*===========================================================================*/

+

+/**

+ * @name    LSM6DS0 accelerometer subsystem data structures and types.

+ * @{

+ */

+ 

+/**

+ * @brief   LSM6DS0 accelerometer subsystem full scale.

+ */

+typedef enum {

+  LSM6DS0_ACC_FS_2G = 0x00,         /**< Full scale ±2g.                    */

+  LSM6DS0_ACC_FS_4G = 0x10,         /**< Full scale ±4g.                    */

+  LSM6DS0_ACC_FS_8G = 0x18,         /**< Full scale ±8g.                    */

+  LSM6DS0_ACC_FS_16G = 0x08         /**< Full scale ±16g.                   */

+} lsm6ds0_acc_fs_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem output data rate.

+ */

+typedef enum {

+  LSM6DS0_ACC_ODR_PD = 0x00,        /**< Power down                         */

+  LSM6DS0_ACC_ODR_10Hz = 0x20,      /**< ODR 10 Hz                          */

+  LSM6DS0_ACC_ODR_50Hz = 0x40,      /**< ODR 50 Hz                          */

+  LSM6DS0_ACC_ODR_119Hz = 0x60,     /**< ODR 119 Hz                         */

+  LSM6DS0_ACC_ODR_238Hz = 0x80,     /**< ODR 238 Hz                         */

+  LSM6DS0_ACC_ODR_476Hz = 0xA0,     /**< ODR 476 Hz                         */

+  LSM6DS0_ACC_ODR_952Hz = 0xC0      /**< ODR 952 Hz                         */

+} lsm6ds0_acc_odr_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem axes enabling.

+ */

+typedef enum {

+  LSM6DS0_ACC_AE_DISABLED = 0x00,   /**< All axes disabled.                 */

+  LSM6DS0_ACC_AE_X = 0x08,          /**< Only X-axis enabled.               */

+  LSM6DS0_ACC_AE_Y = 0x10,          /**< Only Y-axis enabled.               */

+  LSM6DS0_ACC_AE_XY = 0x18,         /**< X and Y axes enabled.              */

+  LSM6DS0_ACC_AE_Z = 0x20,          /**< Only Z-axis enabled.               */

+  LSM6DS0_ACC_AE_XZ = 0x28,         /**< X and Z axes enabled.              */

+  LSM6DS0_ACC_AE_YZ = 0x30,         /**< Y and Z axes enabled.              */

+  LSM6DS0_ACC_AE_XYZ = 0x38         /**< All axes enabled.                  */

+} lsm6ds0_acc_ae_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.

+ */

+typedef enum {

+  LSM6DS0_ACC_OBW_ODR = 0x00,      /**< Depending on ODR.                   */

+  LSM6DS0_ACC_OBW_AA = 0x04        /**< Same of the Anti aliasing.          */

+} lsm6ds0_acc_obw_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem high resolution mode.

+ */

+typedef enum {

+  LSM6DS0_ACC_HR_DISABLED = 0x00,   /**< High resolution mode disabled.     */

+  LSM6DS0_ACC_HR_ENABLED = 0x80     /**< High resolution mode enabled.      */

+} lsm6ds0_acc_hr_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem filtered data selection.

+ */

+typedef enum {

+  LSM6DS0_ACC_FDS_ENABLED = 0x00,   /**< Internal filter bypassed.          */

+  LSM6DS0_ACC_FDS_DISABLED = 0x04   /**< Internal filter not bypassed.      */

+} lsm6ds0_acc_fds_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem digital filter.

+ */

+typedef enum {

+  LSM6DS0_ACC_DCF_400 = 0x00,       /**< Low pass cutoff freq. ODR/400 Hz.  */

+  LSM6DS0_ACC_DCF_100 = 0x20,       /**< Low pass cutoff freq. ODR/100 Hz.  */

+  LSM6DS0_ACC_DCF_50 = 0x60,        /**< Low pass cutoff freq. ODR/50 Hz.   */

+  LSM6DS0_ACC_DCF_9 = 0x40          /**< Low pass cutoff freq. ODR/9 Hz.    */

+} lsm6ds0_acc_dcf_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.

+ */

+typedef enum {

+  LSM6DS0_ACC_AABW_408Hz = 0x00,    /**< Anti Aliasing filter BW 408Hz.     */

+  LSM6DS0_ACC_AABW_211Hz = 0x01,    /**< Anti Aliasing filter BW 211Hz.     */

+  LSM6DS0_ACC_AABW_105Hz = 0x02,    /**< Anti Aliasing filter BW 105Hz.     */

+  LSM6DS0_ACC_AABW_50Hz = 0x03,     /**< Anti Aliasing filter BW 50Hz.      */

+  LSM6DS0_ACC_AABW_AUTO = 0x04      /**< Anti Aliasing filter BW auto.      */

+} lsm6ds0_acc_aabw_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem decimation mode.

+ */

+typedef enum {

+  LSM6DS0_ACC_DEC_DISABLED = 0x00,  /**< Decimation disabled.               */

+  LSM6DS0_ACC_DEC_X2 = 0x40,        /**< Output updated every 2 samples.    */

+  LSM6DS0_ACC_DEC_X4 = 0x80,        /**< Output updated every 4 samples.    */

+  LSM6DS0_ACC_DEC_X8 = 0xC0         /**< Output updated every 8 samples.    */

+} lsm6ds0_acc_dec_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem unit.

+ */

+typedef enum {

+  LSM6DS0_ACC_UNIT_G = 0x00,        /**< Cooked data in g.                  */

+  LSM6DS0_ACC_UNIT_MG = 0x01,       /**< Cooked data in mg.                 */

+  LSM6DS0_ACC_UNIT_SI = 0x02,       /**< Cooked data in m/s^2.              */

+} lsm6ds0_acc_unit_t;

+

+/**

+ * @brief   LSM6DS0 accelerometer subsystem configuration structure.

+ */

+typedef struct {

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem full scale.

+   */

+  lsm6ds0_acc_fs_t          fullscale;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem output data rate.

+   */

+  lsm6ds0_acc_odr_t         outdatarate;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem axes enabling.

+   */

+  lsm6ds0_acc_ae_t          axesenabling;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem output bandwidth.

+   */

+  lsm6ds0_acc_obw_t         outbandwidth;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem high resolution mode.

+   */

+  lsm6ds0_acc_hr_t          highresmode;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem filtered data selection.

+   */

+  lsm6ds0_acc_fds_t         filtdatasel;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem digital filter.

+   */

+  lsm6ds0_acc_dcf_t         digifilter;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.

+   */

+  lsm6ds0_acc_aabw_t        aabandwidth;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem decimation mode.

+   */

+  lsm6ds0_acc_dec_t         decmode;

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem unit.

+   */

+  lsm6ds0_acc_unit_t        unit;

+} LSM6DS0AccConfig;

+/** @} */

+

+/**

+ * @name    LSM6DS0 gyroscope subsystem data structures and types.

+ * @{

+ */

+/**

+ * @brief   LSM6DS0 gyroscope subsystem full scale.

+ */

+typedef enum {

+  LSM6DS0_GYRO_FS_245DSP  = 0x00,   /**< Full scale ±245 degree per second  */

+  LSM6DS0_GYRO_FS_500DSP  = 0x08,   /**< Full scale ±500 degree per second  */

+  LSM6DS0_GYRO_FS_2000DSP = 0x18    /**< Full scale ±2000 degree per second */

+} lsm6ds0_gyro_fs_t;

+

+/**

+ * @brief   LSM6DS0 gyroscope subsystem output data rate.

+ */

+typedef enum {

+  LSM6DS0_GYRO_ODR_PD = 0x00,

+  LSM6DS0_GYRO_ODR_95HZ_FC_25 = 0x10,

+  LSM6DS0_GYRO_ODR_14_9HZ_FC_5 = 0X20,

+  LSM6DS0_GYRO_ODR_59_5HZ_FC_16 = 0X40,

+  LSM6DS0_GYRO_ODR_119HZ_FC_14 = 0X60,

+  LSM6DS0_GYRO_ODR_119HZ_FC_31 = 0X61,

+  LSM6DS0_GYRO_ODR_238HZ_FC_14 = 0X80,

+  LSM6DS0_GYRO_ODR_238HZ_FC_29 = 0X81,

+  LSM6DS0_GYRO_ODR_238HZ_FC_63 = 0X82,

+  LSM6DS0_GYRO_ODR_238HZ_FC_78 = 0X83,

+  LSM6DS0_GYRO_ODR_476HZ_FC_21 = 0XA0,

+  LSM6DS0_GYRO_ODR_476HZ_FC_28 = 0XA1,

+  LSM6DS0_GYRO_ODR_476HZ_FC_57 = 0XA2,

+  LSM6DS0_GYRO_ODR_476HZ_FC_100 = 0XA3,

+  LSM6DS0_GYRO_ODR_952HZ_FC_33 = 0XC0,

+  LSM6DS0_GYRO_ODR_952HZ_FC_40 = 0XC1,

+  LSM6DS0_GYRO_ODR_952HZ_FC_58 = 0XC2,

+  LSM6DS0_GYRO_ODR_952HZ_FC_100 = 0XC3

+} lsm6ds0_gyro_odr_t;

+

+/**

+ * @brief   LSM6DS0 gyroscope subsystem axes enabling.

+ */

+typedef enum {

+  LSM6DS0_GYRO_AE_DISABLED = 0x00,  /**< All axes disabled.                 */

+  LSM6DS0_GYRO_AE_X = 0x08,         /**< Only X-axis enabled.               */

+  LSM6DS0_GYRO_AE_Y = 0x10,         /**< Only Y-axis enabled.               */

+  LSM6DS0_GYRO_AE_XY = 0x18,        /**< X and Y axes enabled.              */

+  LSM6DS0_GYRO_AE_Z = 0x20,         /**< Only Z-axis enabled.               */

+  LSM6DS0_GYRO_AE_XZ = 0x28,        /**< X and Z axes enabled.              */

+  LSM6DS0_GYRO_AE_YZ = 0x30,        /**< Y and Z axes enabled.              */

+  LSM6DS0_GYRO_AE_XYZ = 0x38        /**< All axes enabled.                  */

+} lsm6ds0_gyro_ae_t;

+

+/**

+ * @brief   LSM6DS0 gyroscope subsystem low mode configuration.

+ */

+typedef enum {

+  LSM6DS0_GYRO_LP_DISABLED = 0x00,  /**< Low power mode disabled.           */

+  LSM6DS0_GYRO_LP_ENABLED = 0x80    /**< Low power mode enabled.            */

+} lsm6ds0_gyro_lp_t;

+

+/**

+ * @brief  LSM6DS0 gyroscope subsystem output selection.

+ */

+typedef enum {

+  LSM6DS0_GYRO_OUT_SEL_0 = 0x00,    /**< Low pass filter 1.                 */

+  LSM6DS0_GYRO_OUT_SEL_1 = 0x01,    /**< High pass filter 1 if enabled.     */

+  LSM6DS0_GYRO_OUT_SEL_2 = 0x02     /**< Low pass filter 2.                 */

+} lsm6ds0_gyro_out_sel_t;

+

+/**

+ * @brief  LSM6DS0 gyroscope subsystem high pass filter.

+ */

+typedef enum {

+  LSM6DS0_GYRO_HP_DISABLED = 0x00,  /**< High pass filter disabled.         */

+  LSM6DS0_GYRO_HP_ENABLED = 0x40    /**< High pass filter enabled.          */

+} lsm6ds0_gyro_hp_t;

+

+/**

+ * @brief  LSM6DS0 gyroscope subsystem high pass filter configuration.

+ */

+typedef enum {

+  LSM6DS0_GYRO_HPCF_0 = 0x00,       /**< Refer to table 48 of RM            */

+  LSM6DS0_GYRO_HPCF_1 = 0x01, 

+  LSM6DS0_GYRO_HPCF_2 = 0x02, 

+  LSM6DS0_GYRO_HPCF_3 = 0x03, 

+  LSM6DS0_GYRO_HPCF_4 = 0x04, 

+  LSM6DS0_GYRO_HPCF_5 = 0x05, 

+  LSM6DS0_GYRO_HPCF_6 = 0x06, 

+  LSM6DS0_GYRO_HPCF_7 = 0x07, 

+  LSM6DS0_GYRO_HPCF_8 = 0x08, 

+  LSM6DS0_GYRO_HPCF_9 = 0x09 

+} lsm6ds0_gyro_hpcf_t;

+

+/**

+ * @brief   LSM6DS0 gyroscope subsystem configuration structure.

+ */

+typedef struct {

+  /**

+   * @brief   LSM6DS0 gyroscope subsystem full scale.

+   */

+  lsm6ds0_gyro_fs_t         fullscale;

+  /**

+   * @brief   LSM6DS0 gyroscope subsystem output data rate.

+   */

+  lsm6ds0_gyro_odr_t        outdatarate;

+  /**

+   * @brief   LSM6DS0 gyroscope subsystem axes enabling.

+   */

+  lsm6ds0_gyro_ae_t         axesenabling;

+  /**

+   * @brief   LSM6DS0 gyroscope subsystem low mode configuration.

+   */

+  lsm6ds0_gyro_lp_t         lowmodecfg;

+  /**

+   * @brief   LSM6DS0 gyroscope subsystem output selection.

+   */

+  lsm6ds0_gyro_out_sel_t    outsel;

+  /**

+   * @brief   LSM6DS0 gyroscope subsystem high pass filter.

+   */

+  lsm6ds0_gyro_hp_t         hpfenable;

+  /**

+   * @brief   LSM6DS0 gyroscope subsystem high pass filter configuration.

+   */

+  lsm6ds0_gyro_hpcf_t       hpcfg;

+} LSM6DS0GyroConfig;

+/** @} */

+

+/**

+ * @name    LSM6DS0 main system data structures and types.

+ * @{

+ */

+/**

+ * @brief  Accelerometer and Gyroscope Slave Address.

+ */

+typedef enum {

+  LSM6DS0_SAD_GND = 0x6A,           /**< SAD pin connected to GND.          */

+  LSM6DS0_SAD_VCC = 0x6B            /**< SAD pin connected to VCC.          */

+} lsm6ds0_sad_t;

+

+/**

+ * @brief   LSM6DS0 block data update.

+ */

+typedef enum {

+  LSM6DS0_BDU_CONTINUOUS = 0x00,    /**< Block data continuously updated.   */

+  LSM6DS0_BDU_BLOCKED = 0x40        /**< Block data updated after reading.  */

+} lsm6ds0_bdu_t;

+

+/**

+ * @brief   LSM6DS0 endianness.

+ */

+typedef enum {

+  LSM6DS0_END_LITTLE = 0x00,        /**< Little endian.                     */

+  LSM6DS0_END_BIG = 0x20            /**< Big endian.                        */

+} lsm6ds0_end_t;

+

+/**

+ * @brief   Driver state machine possible states.

+ */

+typedef enum {

+  LSM6DS0_UNINIT = 0,               /**< Not initialized.                   */

+  LSM6DS0_STOP = 1,                 /**< Stopped.                           */

+  LSM6DS0_READY = 2,                /**< Ready.                             */

+} lsm6ds0_state_t;

+

+/**

+ * @brief   LSM6DS0 configuration structure.

+ */

+typedef struct {

+#if (LSM6DS0_USE_SPI) || defined(__DOXYGEN__)

+  /**

+   * @brief SPI driver associated to this LSM6DS0.

+   */

+  SPIDriver                 *spip;

+  /**

+   * @brief SPI configuration associated to this LSM6DS0 accelerometer

+   *        subsystem.

+   */

+  const SPIConfig           *accspicfg;

+  /**

+   * @brief SPI configuration associated to this LSM6DS0 compass

+   *        subsystem.

+   */

+  const SPIConfig           *gyrospicfg;

+#endif /* LSM6DS0_USE_SPI */

+#if (LSM6DS0_USE_I2C) || defined(__DOXYGEN__)

+  /**

+   * @brief I2C driver associated to this LSM6DS0.

+   */

+  I2CDriver                 *i2cp;

+  /**

+   * @brief I2C configuration associated to this LSM6DS0 accelerometer

+   *        subsystem.

+   */

+  const I2CConfig           *i2ccfg;

+#endif /* LSM6DS0_USE_I2C */

+  /**

+   * @brief   LSM6DS0 accelerometer subsystem configuration structure

+   */

+  const LSM6DS0AccConfig    *acccfg;

+  /**

+   * @brief   LSM6DS0 gyroscope subsystem configuration structure

+   */

+  const LSM6DS0GyroConfig   *gyrocfg;

+  /**

+   * @brief  Accelerometer and Gyroscope Slave Address

+   */

+  lsm6ds0_sad_t             slaveaddress;

+  /**

+   * @brief   LSM6DS0 block data update

+   */

+  lsm6ds0_bdu_t             blockdataupdate;

+  /**

+   * @brief   LSM6DS0  endianness

+   */

+  lsm6ds0_end_t             endianness;

+} LSM6DS0Config;

+

+/**

+ * @brief   Structure representing a LSM6DS0 driver.

+ */

+typedef struct LSM6DS0Driver LSM6DS0Driver;

+

+/**

+ * @brief   @p LSM6DS0 accelerometer subsystem specific methods.

+ */

+#define _lsm6ds0_acc_methods                                                \

+  _base_accelerometer_methods

+  

+/**

+ * @brief   @p LSM6DS0 gyroscope subsystem specific methods.

+ */

+#define _lsm6ds0_gyro_methods                                               \

+  _base_gyroscope_methods                                                   \

+  /* Retrieve the temperature of LSM6DS0 chip.*/                            \

+  msg_t (*get_temperature)(void *instance, float* temperature);

+  

+/**

+ * @extends BaseAccelerometerVMT

+ *

+ * @brief   @p LSM6DS0 accelerometer virtual methods table.

+ */

+struct LSM6DS0ACCVMT {

+  _lsm6ds0_acc_methods

+};

+

+/**

+ * @extends BaseCompassVMT

+ *

+ * @brief   @p LSM6DS0 gyroscope virtual methods table.

+ */

+struct LSM6DS0GYROVMT {

+  _lsm6ds0_gyro_methods

+};

+

+/**

+ * @brief   @p LSM6DS0Driver specific data.

+ */

+#define _lsm6ds0_data                                                       \

+  _base_accelerometer_data                                                  \

+  _base_gyroscope_data                                                      \

+  /* Driver state.*/                                                        \

+  lsm6ds0_state_t           state;                                          \

+  /* Current configuration data.*/                                          \

+  const LSM6DS0Config       *config;                                        \

+  /* Current accelerometer sensitivity.*/                                   \

+  float                     accsensitivity[LSM6DS0_ACC_NUMBER_OF_AXES];     \

+  /* Accelerometer bias data.*/                                             \

+  int32_t                   accbias[LSM6DS0_ACC_NUMBER_OF_AXES];            \

+  /* Current gyroscope sensitivity.*/                                       \

+  float                     gyrosensitivity[LSM6DS0_GYRO_NUMBER_OF_AXES];   \

+  /* Bias data.*/                                                           \

+  int32_t                   gyrobias[LSM6DS0_GYRO_NUMBER_OF_AXES];

+  

+/**

+ * @brief   LSM6DS0 6-axis accelerometer/gyroscope class.

+ */

+struct LSM6DS0Driver {

+  /** @brief BaseSensor Virtual Methods Table. */

+  const struct BaseSensorVMT *vmt_basesensor;

+  /** @brief BaseAccelerometer Virtual Methods Table. */

+  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;

+  /** @brief BaseGyroscope Virtual Methods Table. */

+  const struct BaseGyroscopeVMT *vmt_basegyroscope;

+  /** @brief LSM6DS0 Accelerometer Virtual Methods Table. */

+  const struct LSM6DS0ACCVMT *vmt_lsm6ds0acc;

+  /** @brief LSM6DS0 Gyroscope Virtual Methods Table. */

+  const struct LSM6DS0GYROVMT *vmt_lsm6ds0gyro;

+  _lsm6ds0_data

+};

+/** @} */

+

+/*===========================================================================*/

+/* Driver macros.                                                            */

+/*===========================================================================*/

+

+/**

+ * @brief   Get current MEMS temperature.

+ * @detail  This information is very useful especially for high accuracy IMU

+ *

+ * @param[in] ip        pointer to a @p BaseGyroscope class.

+ * @param[out] temp     the MEMS temperature as single precision floating.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ * @api

+ */

+#define gyroscopeGetTemp(ip, tpp)                                           \

+        (ip)->vmt_lsm6ds0gyro->get_temperature(ip, tpp)

+	

+/*===========================================================================*/

+/* External declarations.                                                    */

+/*===========================================================================*/

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void lsm6ds0ObjectInit(LSM6DS0Driver *devp);

+  void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config);

+  void lsm6ds0Stop(LSM6DS0Driver *devp);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* _LSM6DS0_H_ */

+

+/** @} */