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

16 files changed, 8258 insertions, 8258 deletions

diff --git a/os/ex/Micron/m25q.c b/os/ex/Micron/m25q.c
index 3dbb3df62..3d6de626d 100644
--- a/os/ex/Micron/m25q.c
+++ b/os/ex/Micron/m25q.c
@@ -1,834 +1,834 @@
-/*
-    ChibiOS - Copyright (C) 2016 Giovanni Di Sirio
-
-    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    m25q.c
- * @brief   Micron serial flash driver code.
- *
- * @addtogroup m25q
- * @{
- */
-
-#include <string.h>
-
-#include "hal.h"
-#include "m25q.h"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-#define PAGE_SIZE                           256U
-#define PAGE_MASK                           (PAGE_SIZE - 1U)
-
-#if M25Q_USE_SUB_SECTORS == TRUE
-#define SECTOR_SIZE                         0x00001000U
-#define CMD_SECTOR_ERASE                    M25Q_CMD_SUBSECTOR_ERASE
-#else
-#define SECTOR_SIZE                         0x00010000U
-#define CMD_SECTOR_ERASE                    M25Q_CMD_SECTOR_ERASE
-#endif
-
-/*===========================================================================*/
-/* Driver exported variables.                                                */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local variables and types.                                         */
-/*===========================================================================*/
-
-static const flash_descriptor_t *m25q_get_descriptor(void *instance);
-static flash_error_t m25q_read(void *instance, flash_offset_t offset,
-                               size_t n, uint8_t *rp);
-static flash_error_t m25q_program(void *instance, flash_offset_t offset,
-                                  size_t n, const uint8_t *pp);
-static flash_error_t m25q_start_erase_all(void *instance);
-static flash_error_t m25q_start_erase_sector(void *instance,
-                                             flash_sector_t sector);
-static flash_error_t m25q_query_erase(void *instance, uint32_t *msec);
-static flash_error_t m25q_verify_erase(void *instance, flash_sector_t sector);
-static flash_error_t m25q_read_sfdp(void *instance, flash_offset_t offset,
-                                    size_t n, uint8_t *rp);
-
-/**
- * @brief   Virtual methods table.
- */
-static const struct M25QDriverVMT m25q_vmt = {
-  m25q_get_descriptor, m25q_read, m25q_program,
-  m25q_start_erase_all, m25q_start_erase_sector,
-  m25q_query_erase, m25q_verify_erase,
-  m25q_read_sfdp
-};
-
-/**
- * @brief   N25Q128 descriptor.
- */
-static flash_descriptor_t m25q_descriptor = {
-  .attributes       = FLASH_ATTR_ERASED_IS_ONE | FLASH_ATTR_REWRITABLE |
-                      FLASH_ATTR_SUSPEND_ERASE_CAPABLE,
-  .page_size        = 256U,
-  .sectors_count    = 0U,           /* It is overwritten.*/
-  .sectors          = NULL,
-  .sectors_size     = SECTOR_SIZE,
-  .address          = 0U
-};
-
-#if JESD216_BUS_MODE != JESD216_BUS_MODE_SPI
-/* Initial M25Q_CMD_READ_ID command.*/
-static const qspi_command_t m25q_cmd_read_id = {
-  .cfg              = QSPI_CFG_CMD(M25Q_CMD_READ_ID) |
-#if M25Q_SWITCH_WIDTH == TRUE
-                      QSPI_CFG_CMD_MODE_ONE_LINE     |
-                      QSPI_CFG_DATA_MODE_ONE_LINE,
-#else
-#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L
-                      QSPI_CFG_CMD_MODE_ONE_LINE   |
-                      QSPI_CFG_DATA_MODE_ONE_LINE,
-#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L
-                      QSPI_CFG_CMD_MODE_TWO_LINES   |
-                      QSPI_CFG_DATA_MODE_TWO_LINES,
-#else
-                      QSPI_CFG_CMD_MODE_FOUR_LINES   |
-                      QSPI_CFG_DATA_MODE_FOUR_LINES,
-#endif
-#endif
-  .addr             = 0,
-  .alt              = 0
-};
-
-/* Initial M25Q_CMD_WRITE_ENHANCED_V_CONF_REGISTER command.*/
-static const qspi_command_t m25q_cmd_write_evconf = {
-  .cfg              = QSPI_CFG_CMD(M25Q_CMD_WRITE_ENHANCED_V_CONF_REGISTER) |
-#if M25Q_SWITCH_WIDTH == TRUE
-                      QSPI_CFG_CMD_MODE_ONE_LINE     |
-                      QSPI_CFG_DATA_MODE_ONE_LINE,
-#else
-#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L
-                      QSPI_CFG_CMD_MODE_ONE_LINE   |
-                      QSPI_CFG_DATA_MODE_ONE_LINE,
-#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L
-                      QSPI_CFG_CMD_MODE_TWO_LINES   |
-                      QSPI_CFG_DATA_MODE_TWO_LINES,
-#else
-                      QSPI_CFG_CMD_MODE_FOUR_LINES   |
-                      QSPI_CFG_DATA_MODE_FOUR_LINES,
-#endif
-#endif
-  .addr             = 0,
-  .alt              = 0
-};
-
-/* Initial M25Q_CMD_WRITE_ENABLE command.*/
-static const qspi_command_t m25q_cmd_write_enable = {
-  .cfg              = QSPI_CFG_CMD(M25Q_CMD_WRITE_ENABLE) |
-#if M25Q_SWITCH_WIDTH == TRUE
-                      QSPI_CFG_CMD_MODE_ONE_LINE,
-#else
-#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L
-                      QSPI_CFG_CMD_MODE_ONE_LINE,
-#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L
-                      QSPI_CFG_CMD_MODE_TWO_LINES,
-#else
-                      QSPI_CFG_CMD_MODE_FOUR_LINES,
-#endif
-#endif
-  .addr             = 0,
-  .alt              = 0
-};
-
-/* Bus width initialization.*/
-#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L
-static const uint8_t m25q_evconf_value[1] = {0xCF};
-#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L
-static const uint8_t m25q_evconf_value[1] = {0x8F};
-#else
-static const uint8_t m25q_evconf_value[1] = {0x4F};
-#endif
-#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */
-
-static const uint8_t m25q_manufacturer_ids[] = M25Q_SUPPORTED_MANUFACTURE_IDS;
-static const uint8_t m25q_memory_type_ids[] = M25Q_SUPPORTED_MEMORY_TYPE_IDS;
-
-/*===========================================================================*/
-/* Driver local functions.                                                   */
-/*===========================================================================*/
-
-static bool m25q_find_id(const uint8_t *set, size_t size, uint8_t element) {
-  size_t i;
-
-  for (i = 0; i < size; i++) {
-    if (set[i] == element) {
-      return true;
-    }
-  }
-  return false;
-}
-
-#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI) || defined(__DOXYGEN__)
-void m25q_reset_xip(M25QDriver *devp) {
-  static const uint8_t flash_conf[1] = {
-    (M25Q_READ_DUMMY_CYCLES << 4U) | 0x0FU
-  };
-  qspi_command_t cmd;
-  uint8_t buf[1];
-
-  /* Resetting XIP mode by reading one byte without XIP confirmation bit.*/
-  cmd.alt  = 0xFF;
-  cmd.addr = 0;
-  cmd.cfg  = QSPI_CFG_CMD_MODE_NONE |
-             QSPI_CFG_ADDR_SIZE_24 |
-#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L
-             QSPI_CFG_ADDR_MODE_ONE_LINE |
-             QSPI_CFG_DATA_MODE_ONE_LINE |
-#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L
-             QSPI_CFG_ADDR_MODE_TWO_LINES |
-             QSPI_CFG_DATA_MODE_TWO_LINES |
-#else
-             QSPI_CFG_ADDR_MODE_FOUR_LINES |
-             QSPI_CFG_DATA_MODE_FOUR_LINES |
-#endif
-             QSPI_CFG_ALT_MODE_FOUR_LINES |  /* Always 4 lines, note.*/
-             QSPI_CFG_ALT_SIZE_8 |
-             QSPI_CFG_DUMMY_CYCLES(M25Q_READ_DUMMY_CYCLES - 2);
-  qspiReceive(devp->config->busp, &cmd, 1, buf);
-
-  /* Enabling write operation.*/
-  jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);
-
-  /* Rewriting volatile configuration register.*/
-  jesd216_cmd_send(devp->config->busp, M25Q_CMD_WRITE_V_CONF_REGISTER,
-                   1, flash_conf);
-}
-
-void m25q_reset_memory(M25QDriver *devp) {
-
-  /* 1x M25Q_CMD_RESET_ENABLE command.*/
-  static const qspi_command_t cmd_reset_enable_1 = {
-    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_ENABLE) |
-                        QSPI_CFG_CMD_MODE_ONE_LINE,
-    .addr             = 0,
-    .alt              = 0
-  };
-
-  /* 1x M25Q_CMD_RESET_MEMORY command.*/
-  static const qspi_command_t cmd_reset_memory_1 = {
-    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_MEMORY) |
-                        QSPI_CFG_CMD_MODE_ONE_LINE,
-    .addr             = 0,
-    .alt              = 0
-  };
-
-  /* If the device is in one bit mode then the following commands are
-     rejected because shorter than 8 bits. If the device is in multiple
-     bits mode then the commands are accepted and the device is reset to
-     one bit mode.*/
-#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI4L
-  /* 4x M25Q_CMD_RESET_ENABLE command.*/
-  static const qspi_command_t cmd_reset_enable_4 = {
-    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_ENABLE) |
-                        QSPI_CFG_CMD_MODE_FOUR_LINES,
-    .addr             = 0,
-    .alt              = 0
-  };
-
-  /* 4x M25Q_CMD_RESET_MEMORY command.*/
-  static const qspi_command_t cmd_reset_memory_4 = {
-    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_MEMORY) |
-                        QSPI_CFG_CMD_MODE_FOUR_LINES,
-    .addr             = 0,
-    .alt              = 0
-  };
-
-  qspiCommand(devp->config->busp, &cmd_reset_enable_4);
-  qspiCommand(devp->config->busp, &cmd_reset_memory_4);
-#else
-  /* 2x M25Q_CMD_RESET_ENABLE command.*/
-  static const qspi_command_t cmd_reset_enable_2 = {
-    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_ENABLE) |
-                        QSPI_CFG_CMD_MODE_TWO_LINES,
-    .addr             = 0,
-    .alt              = 0
-  };
-
-  /* 2x M25Q_CMD_RESET_MEMORY command.*/
-  static const qspi_command_t cmd_reset_memory_2 = {
-    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_MEMORY) |
-                        QSPI_CFG_CMD_MODE_TWO_LINES,
-    .addr             = 0,
-    .alt              = 0
-  };
-
-  qspiCommand(devp->config->busp, &cmd_reset_enable_2);
-  qspiCommand(devp->config->busp, &cmd_reset_memory_2);
-#endif
-
-  /* Now the device should be in one bit mode for sure and we perform a
-     device reset.*/
-  qspiCommand(devp->config->busp, &cmd_reset_enable_1);
-  qspiCommand(devp->config->busp, &cmd_reset_memory_1);
-}
-#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */
-
-static flash_error_t m25q_poll_status(M25QDriver *devp) {
-  uint8_t sts;
-
-  do {
-#if M25Q_NICE_WAITING == TRUE
-    osalThreadSleepMilliseconds(1);
-#endif
-    /* Read status command.*/
-    jesd216_cmd_receive(devp->config->busp, M25Q_CMD_READ_FLAG_STATUS_REGISTER,
-                        1, &sts);
-  } while ((sts & M25Q_FLAGS_PROGRAM_ERASE) == 0U);
-
-  /* Checking for errors.*/
-  if ((sts & M25Q_FLAGS_ALL_ERRORS) != 0U) {
-    /* Clearing status register.*/
-    jesd216_cmd(devp->config->busp, M25Q_CMD_CLEAR_FLAG_STATUS_REGISTER);
-
-    /* Program operation failed.*/
-    return FLASH_ERROR_PROGRAM;
-  }
-
-  return FLASH_NO_ERROR;
-}
-
-static const flash_descriptor_t *m25q_get_descriptor(void *instance) {
-  M25QDriver *devp = (M25QDriver *)instance;
-
-  osalDbgCheck(instance != NULL);
-  osalDbgAssert((devp->state != FLASH_UNINIT) && (devp->state != FLASH_STOP),
-                "invalid state");
-
-  return &m25q_descriptor;
-}
-
-static flash_error_t m25q_read(void *instance, flash_offset_t offset,
-                               size_t n, uint8_t *rp) {
-  M25QDriver *devp = (M25QDriver *)instance;
-
-  osalDbgCheck((instance != NULL) && (rp != NULL) && (n > 0U));
-  osalDbgCheck((size_t)offset + n <= (size_t)m25q_descriptor.sectors_count *
-                                     (size_t)m25q_descriptor.sectors_size);
-  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),
-                "invalid state");
-
-  if (devp->state == FLASH_ERASE) {
-    return FLASH_BUSY_ERASING;
-  }
-
-  /* Bus acquired.*/
-  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-  /* FLASH_READY state while the operation is performed.*/
-  devp->state = FLASH_READ;
-
-#if JESD216_BUS_MODE != JESD216_BUS_MODE_SPI
-  /* Fast read command in QSPI mode.*/
-  jesd216_cmd_addr_dummy_receive(devp->config->busp, M25Q_CMD_FAST_READ,
-                                 offset, M25Q_READ_DUMMY_CYCLES, n, rp);
-#else
-  /* Normal read command in SPI mode.*/
-  jesd216_cmd_addr_receive(devp->config->busp, M25Q_CMD_READ,
-                           offset, n, rp);
-#endif
-
-  /* Ready state again.*/
-  devp->state = FLASH_READY;
-
-  /* Bus released.*/
-  jesd216_bus_release(devp->config->busp);
-
-  return FLASH_NO_ERROR;
-}
-
-static flash_error_t m25q_program(void *instance, flash_offset_t offset,
-                                  size_t n, const uint8_t *pp) {
-  M25QDriver *devp = (M25QDriver *)instance;
-
-  osalDbgCheck((instance != NULL) && (pp != NULL) && (n > 0U));
-  osalDbgCheck((size_t)offset + n <= (size_t)m25q_descriptor.sectors_count *
-                                     (size_t)m25q_descriptor.sectors_size);
-  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),
-                "invalid state");
-
-  if (devp->state == FLASH_ERASE) {
-    return FLASH_BUSY_ERASING;
-  }
-
-  /* Bus acquired.*/
-  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-  /* FLASH_PGM state while the operation is performed.*/
-  devp->state = FLASH_PGM;
-
-  /* Data is programmed page by page.*/
-  while (n > 0U) {
-    flash_error_t err;
-
-    /* Data size that can be written in a single program page operation.*/
-    size_t chunk = (size_t)(((offset | PAGE_MASK) + 1U) - offset);
-    if (chunk > n) {
-      chunk = n;
-    }
-
-    /* Enabling write operation.*/
-    jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);
-
-    /* Page program command.*/
-    jesd216_cmd_addr_send(devp->config->busp, M25Q_CMD_PAGE_PROGRAM, offset,
-                          chunk, pp);
-
-    /* Wait for status and check errors.*/
-    err = m25q_poll_status(devp);
-    if (err != FLASH_NO_ERROR) {
-
-      /* Bus released.*/
-      jesd216_bus_release(devp->config->busp);
-
-      return err;
-    }
-
-    /* Next page.*/
-    offset += chunk;
-    pp     += chunk;
-    n      -= chunk;
-  }
-
-  /* Ready state again.*/
-  devp->state = FLASH_READY;
-
-  /* Bus released.*/
-  jesd216_bus_release(devp->config->busp);
-
-  return FLASH_NO_ERROR;
-}
-
-static flash_error_t m25q_start_erase_all(void *instance) {
-  M25QDriver *devp = (M25QDriver *)instance;
-
-  osalDbgCheck(instance != NULL);
-  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),
-                "invalid state");
-
-  if (devp->state == FLASH_ERASE) {
-    return FLASH_BUSY_ERASING;
-  }
-
-  /* Bus acquired.*/
-  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-  /* FLASH_ERASE state while the operation is performed.*/
-  devp->state = FLASH_ERASE;
-
-  /* Enabling write operation.*/
-  jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);
-
-  /* Bulk erase command.*/
-  jesd216_cmd(devp->config->busp, M25Q_CMD_BULK_ERASE);
-
-  /* Bus released.*/
-  jesd216_bus_release(devp->config->busp);
-
-  return FLASH_NO_ERROR;
-}
-
-static flash_error_t m25q_start_erase_sector(void *instance,
-                                             flash_sector_t sector) {
-  M25QDriver *devp = (M25QDriver *)instance;
-  flash_offset_t offset = (flash_offset_t)(sector * SECTOR_SIZE);
-
-  osalDbgCheck(instance != NULL);
-  osalDbgCheck(sector < m25q_descriptor.sectors_count);
-  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),
-                "invalid state");
-
-  if (devp->state == FLASH_ERASE) {
-    return FLASH_BUSY_ERASING;
-  }
-
-  /* Bus acquired.*/
-  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-  /* FLASH_ERASE state while the operation is performed.*/
-  devp->state = FLASH_ERASE;
-
-  /* Enabling write operation.*/
-  jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);
-
-  /* Sector erase command.*/
-  jesd216_cmd_addr(devp->config->busp, M25Q_CMD_SECTOR_ERASE, offset);
-
-  /* Bus released.*/
-  jesd216_bus_release(devp->config->busp);
-
-  return FLASH_NO_ERROR;
-}
-
-static flash_error_t m25q_verify_erase(void *instance,
-                                       flash_sector_t sector) {
-  M25QDriver *devp = (M25QDriver *)instance;
-  uint8_t cmpbuf[M25Q_COMPARE_BUFFER_SIZE];
-  flash_offset_t offset;
-  size_t n;
-
-  osalDbgCheck(instance != NULL);
-  osalDbgCheck(sector < m25q_descriptor.sectors_count);
-  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),
-                "invalid state");
-
-  if (devp->state == FLASH_ERASE) {
-    return FLASH_BUSY_ERASING;
-  }
-
-  /* Bus acquired.*/
-  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-  /* FLASH_READY state while the operation is performed.*/
-  devp->state = FLASH_READ;
-
-  /* Read command.*/
-  offset = (flash_offset_t)(sector * SECTOR_SIZE);
-  n = SECTOR_SIZE;
-  while (n > 0U) {
-    uint8_t *p;
-
-#if JESD216_BUS_MODE != JESD216_BUS_MODE_SPI
-   jesd216_cmd_addr_dummy_receive(devp->config->busp, M25Q_CMD_FAST_READ,
-                                  offset, M25Q_READ_DUMMY_CYCLES,
-                                  sizeof cmpbuf, cmpbuf);
-#else
-   /* Normal read command in SPI mode.*/
-   jesd216_cmd_addr_receive(devp->config->busp, M25Q_CMD_READ,
-                            offset, sizeof cmpbuf, cmpbuf);
-#endif
-
-    /* Checking for erased state of current buffer.*/
-    for (p = cmpbuf; p < &cmpbuf[M25Q_COMPARE_BUFFER_SIZE]; p++) {
-      if (*p != 0xFFU) {
-        /* Ready state again.*/
-        devp->state = FLASH_READY;
-
-        /* Bus released.*/
-        jesd216_bus_release(devp->config->busp);
-
-        return FLASH_ERROR_VERIFY;
-      }
-    }
-
-    offset += sizeof cmpbuf;
-    n -= sizeof cmpbuf;
-  }
-
-  /* Ready state again.*/
-  devp->state = FLASH_READY;
-
-  /* Bus released.*/
-  jesd216_bus_release(devp->config->busp);
-
-  return FLASH_NO_ERROR;
-}
-
-static flash_error_t m25q_query_erase(void *instance, uint32_t *msec) {
-  M25QDriver *devp = (M25QDriver *)instance;
-  uint8_t sts;
-
-  osalDbgCheck(instance != NULL);
-  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),
-                "invalid state");
-
-  /* If there is an erase in progress then the device must be checked.*/
-  if (devp->state == FLASH_ERASE) {
-
-    /* Bus acquired.*/
-    jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-    /* Read status command.*/
-    jesd216_cmd_receive(devp->config->busp, M25Q_CMD_READ_FLAG_STATUS_REGISTER,
-                        1, &sts);
-
-    /* If the P/E bit is zero (busy) or the flash in a suspended state then
-       report that the operation is still in progress.*/
-    if (((sts & M25Q_FLAGS_PROGRAM_ERASE) == 0U) ||
-        ((sts & M25Q_FLAGS_ERASE_SUSPEND) != 0U)) {
-
-      /* Bus released.*/
-      jesd216_bus_release(devp->config->busp);
-
-      /* Recommended time before polling again, this is a simplified
-         implementation.*/
-      if (msec != NULL) {
-        *msec = 1U;
-      }
-
-      return FLASH_BUSY_ERASING;
-    }
-
-    /* The device is ready to accept commands.*/
-    devp->state = FLASH_READY;
-
-    /* Checking for errors.*/
-    if ((sts & M25Q_FLAGS_ALL_ERRORS) != 0U) {
-
-      /* Clearing status register.*/
-      jesd216_cmd(devp->config->busp, M25Q_CMD_CLEAR_FLAG_STATUS_REGISTER);
-
-      /* Erase operation failed.*/
-      return FLASH_ERROR_ERASE;
-    }
-
-    /* Bus released.*/
-    jesd216_bus_release(devp->config->busp);
-  }
-
-  return FLASH_NO_ERROR;
-}
-
-static flash_error_t m25q_read_sfdp(void *instance, flash_offset_t offset,
-                                    size_t n, uint8_t *rp) {
-
-  (void)instance;
-  (void)rp;
-  (void)offset;
-  (void)n;
-
-  return FLASH_NO_ERROR;
-}
-
-/*===========================================================================*/
-/* Driver exported functions.                                                */
-/*===========================================================================*/
-
-/**
- * @brief   Initializes an instance.
- *
- * @param[out] devp     pointer to the @p M25QDriver object
- *
- * @init
- */
-void m25qObjectInit(M25QDriver *devp) {
-
-  osalDbgCheck(devp != NULL);
-
-  devp->vmt         = &m25q_vmt;
-  devp->state       = FLASH_STOP;
-  devp->config      = NULL;
-}
-
-/**
- * @brief   Configures and activates N25Q128 driver.
- *
- * @param[in] devp      pointer to the @p M25QDriver object
- * @param[in] config    pointer to the configuration
- *
- * @api
- */
-void m25qStart(M25QDriver *devp, const M25QConfig *config) {
-
-  osalDbgCheck((devp != NULL) && (config != NULL));
-  osalDbgAssert(devp->state != FLASH_UNINIT, "invalid state");
-
-  devp->config = config;
-
-  if (devp->state == FLASH_STOP) {
-
-    /* Bus acquisition.*/
-    jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-#if JESD216_BUS_MODE == JESD216_BUS_MODE_SPI
-    /* Reading device ID.*/
-    jesd216_cmd_receive(devp->config->busp, M25Q_CMD_READ_ID,
-                        sizeof devp->device_id, devp->device_id);
-
-#else /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */
-    /* Attempting a reset of the XIP mode, it could be in an unexpected state
-       because a CPU reset does not reset the memory too.*/
-    m25q_reset_xip(devp);
-
-    /* Attempting a eeset of the device, it could be in an unexpected state
-       because a CPU reset does not reset the memory too.*/
-    m25q_reset_memory(devp);
-
-    /* Reading device ID and unique ID.*/
-    qspiReceive(devp->config->busp, &m25q_cmd_read_id,
-                sizeof devp->device_id, devp->device_id);
-#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */
-
-    /* Checking if the device is white listed.*/
-    osalDbgAssert(m25q_find_id(m25q_manufacturer_ids,
-                          sizeof m25q_manufacturer_ids,
-                          devp->device_id[0]),
-                  "invalid manufacturer id");
-    osalDbgAssert(m25q_find_id(m25q_memory_type_ids,
-                          sizeof m25q_memory_type_ids,
-                          devp->device_id[1]),
-                  "invalid memory type id");
-
-#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI) && (M25Q_SWITCH_WIDTH == TRUE)
-    /* Setting up final bus width.*/
-    qspiCommand(devp->config->busp, &m25q_cmd_write_enable);
-    qspiSend(devp->config->busp, &m25q_cmd_write_evconf, 1, m25q_evconf_value);
-
-    {
-      uint8_t id[3];
-
-      /* Reading ID again for confirmation.*/
-      jesd216_cmd_receive(devp->config->busp, M25Q_CMD_MULTIPLE_IO_READ_ID,
-                          3, id);
-
-      /* Checking if the device is white listed.*/
-      osalDbgAssert(memcmp(id, devp->device_id, 3) == 0,
-                    "id confirmation failed");
-    }
-#endif
-
-    /* Setting up the device size.*/
-    m25q_descriptor.sectors_count = (1U << (size_t)devp->device_id[2]) /
-                                    SECTOR_SIZE;
-
-#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI)
-    {
-      static const uint8_t flash_conf[1] = {
-        (M25Q_READ_DUMMY_CYCLES << 4U) | 0x0FU
-      };
-
-      /* Setting up the dummy cycles to be used for fast read operations.*/
-      jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);
-      jesd216_cmd_send(devp->config->busp, M25Q_CMD_WRITE_V_CONF_REGISTER,
-                       1, flash_conf);
-    }
-#endif
-
-    /* Driver in ready state.*/
-    devp->state = FLASH_READY;
-
-    /* Bus release.*/
-    jesd216_bus_release(devp->config->busp);
-  }
-} 
-
-/**
- * @brief   Deactivates the N25Q128 driver.
- *
- * @param[in] devp      pointer to the @p M25QDriver object
- *
- * @api
- */
-void m25qStop(M25QDriver *devp) {
-
-  osalDbgCheck(devp != NULL);
-  osalDbgAssert(devp->state != FLASH_UNINIT, "invalid state");
-
-  if (devp->state != FLASH_STOP) {
-
-    /* Bus acquisition.*/
-    jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-    /* Stopping bus device.*/
-    jesd216_stop(devp->config->busp);
-
-    /* Deleting current configuration.*/
-    devp->config = NULL;
-
-    /* Driver stopped.*/
-    devp->state = FLASH_STOP;
-
-    /* Bus release.*/
-    jesd216_bus_release(devp->config->busp);
-  }
-}
-
-#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI) || defined(__DOXYGEN__)
-#if (QSPI_SUPPORTS_MEMMAP == TRUE) || defined(__DOXYGEN__)
-/**
- * @brief   Enters the memory Mapping mode.
- * @details The memory mapping mode is only available when the QSPI mode
- *          is selected and the underlying QSPI controller supports the
- *          feature.
- *
- * @param[in] devp      pointer to the @p M25QDriver object
- * @param[out] addrp    pointer to the memory start address of the mapped
- *                      flash or @p NULL
- *
- * @api
- */
-void m25qMemoryMap(M25QDriver *devp, uint8_t **addrp) {
-  static const uint8_t flash_status_xip[1] = {
-    (M25Q_READ_DUMMY_CYCLES << 4U) | 0x07U
-  };
-  qspi_command_t cmd;
-
-  /* Bus acquisition.*/
-  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-  /* Activating XIP mode in the device.*/
-  jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);
-  jesd216_cmd_send(devp->config->busp, M25Q_CMD_WRITE_V_CONF_REGISTER,
-                   1, flash_status_xip);
-
-  /* Putting the QSPI driver in memory mapped mode.*/
-  cmd.cfg = QSPI_CFG_CMD(M25Q_CMD_FAST_READ) |
-            QSPI_CFG_ADDR_SIZE_24 |
-#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L
-            QSPI_CFG_CMD_MODE_ONE_LINE |
-            QSPI_CFG_ADDR_MODE_ONE_LINE |
-            QSPI_CFG_DATA_MODE_ONE_LINE |
-#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L
-            QSPI_CFG_CMD_MODE_TWO_LINES |
-            QSPI_CFG_ADDR_MODE_TWO_LINES |
-            QSPI_CFG_DATA_MODE_TWO_LINES |
-#else
-            QSPI_CFG_CMD_MODE_FOUR_LINES |
-            QSPI_CFG_ADDR_MODE_FOUR_LINES |
-            QSPI_CFG_DATA_MODE_FOUR_LINES |
-#endif
-            QSPI_CFG_ALT_MODE_FOUR_LINES |  /* Always 4 lines, note.*/
-            QSPI_CFG_ALT_SIZE_8 |
-            QSPI_CFG_SIOO |
-            QSPI_CFG_DUMMY_CYCLES(M25Q_READ_DUMMY_CYCLES - 2);
-
-  qspiMapFlash(devp->config->busp, &cmd, addrp);
-
-  /* Bus release.*/
-  jesd216_bus_release(devp->config->busp);
-}
-
-/**
- * @brief   Leaves the memory Mapping mode.
- *
- * @param[in] devp      pointer to the @p M25QDriver object
- *
- * @api
- */
-void m25qMemoryUnmap(M25QDriver *devp) {
-
-  /* Bus acquisition.*/
-  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);
-
-  qspiUnmapFlash(devp->config->busp);
-
-  m25q_reset_xip(devp);
-
-  /* Bus release.*/
-  jesd216_bus_release(devp->config->busp);
-}
-#endif /* QSPI_SUPPORTS_MEMMAP == TRUE */
-#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */
-
-/** @} */
+/*

+    ChibiOS - Copyright (C) 2016 Giovanni Di Sirio

+

+    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    m25q.c

+ * @brief   Micron serial flash driver code.

+ *

+ * @addtogroup m25q

+ * @{

+ */

+

+#include <string.h>

+

+#include "hal.h"

+#include "m25q.h"

+

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

+/* Driver local definitions.                                                 */

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

+

+#define PAGE_SIZE                           256U

+#define PAGE_MASK                           (PAGE_SIZE - 1U)

+

+#if M25Q_USE_SUB_SECTORS == TRUE

+#define SECTOR_SIZE                         0x00001000U

+#define CMD_SECTOR_ERASE                    M25Q_CMD_SUBSECTOR_ERASE

+#else

+#define SECTOR_SIZE                         0x00010000U

+#define CMD_SECTOR_ERASE                    M25Q_CMD_SECTOR_ERASE

+#endif

+

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

+/* Driver exported variables.                                                */

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

+

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

+/* Driver local variables and types.                                         */

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

+

+static const flash_descriptor_t *m25q_get_descriptor(void *instance);

+static flash_error_t m25q_read(void *instance, flash_offset_t offset,

+                               size_t n, uint8_t *rp);

+static flash_error_t m25q_program(void *instance, flash_offset_t offset,

+                                  size_t n, const uint8_t *pp);

+static flash_error_t m25q_start_erase_all(void *instance);

+static flash_error_t m25q_start_erase_sector(void *instance,

+                                             flash_sector_t sector);

+static flash_error_t m25q_query_erase(void *instance, uint32_t *msec);

+static flash_error_t m25q_verify_erase(void *instance, flash_sector_t sector);

+static flash_error_t m25q_read_sfdp(void *instance, flash_offset_t offset,

+                                    size_t n, uint8_t *rp);

+

+/**

+ * @brief   Virtual methods table.

+ */

+static const struct M25QDriverVMT m25q_vmt = {

+  m25q_get_descriptor, m25q_read, m25q_program,

+  m25q_start_erase_all, m25q_start_erase_sector,

+  m25q_query_erase, m25q_verify_erase,

+  m25q_read_sfdp

+};

+

+/**

+ * @brief   N25Q128 descriptor.

+ */

+static flash_descriptor_t m25q_descriptor = {

+  .attributes       = FLASH_ATTR_ERASED_IS_ONE | FLASH_ATTR_REWRITABLE |

+                      FLASH_ATTR_SUSPEND_ERASE_CAPABLE,

+  .page_size        = 256U,

+  .sectors_count    = 0U,           /* It is overwritten.*/

+  .sectors          = NULL,

+  .sectors_size     = SECTOR_SIZE,

+  .address          = 0U

+};

+

+#if JESD216_BUS_MODE != JESD216_BUS_MODE_SPI

+/* Initial M25Q_CMD_READ_ID command.*/

+static const qspi_command_t m25q_cmd_read_id = {

+  .cfg              = QSPI_CFG_CMD(M25Q_CMD_READ_ID) |

+#if M25Q_SWITCH_WIDTH == TRUE

+                      QSPI_CFG_CMD_MODE_ONE_LINE     |

+                      QSPI_CFG_DATA_MODE_ONE_LINE,

+#else

+#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L

+                      QSPI_CFG_CMD_MODE_ONE_LINE   |

+                      QSPI_CFG_DATA_MODE_ONE_LINE,

+#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L

+                      QSPI_CFG_CMD_MODE_TWO_LINES   |

+                      QSPI_CFG_DATA_MODE_TWO_LINES,

+#else

+                      QSPI_CFG_CMD_MODE_FOUR_LINES   |

+                      QSPI_CFG_DATA_MODE_FOUR_LINES,

+#endif

+#endif

+  .addr             = 0,

+  .alt              = 0

+};

+

+/* Initial M25Q_CMD_WRITE_ENHANCED_V_CONF_REGISTER command.*/

+static const qspi_command_t m25q_cmd_write_evconf = {

+  .cfg              = QSPI_CFG_CMD(M25Q_CMD_WRITE_ENHANCED_V_CONF_REGISTER) |

+#if M25Q_SWITCH_WIDTH == TRUE

+                      QSPI_CFG_CMD_MODE_ONE_LINE     |

+                      QSPI_CFG_DATA_MODE_ONE_LINE,

+#else

+#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L

+                      QSPI_CFG_CMD_MODE_ONE_LINE   |

+                      QSPI_CFG_DATA_MODE_ONE_LINE,

+#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L

+                      QSPI_CFG_CMD_MODE_TWO_LINES   |

+                      QSPI_CFG_DATA_MODE_TWO_LINES,

+#else

+                      QSPI_CFG_CMD_MODE_FOUR_LINES   |

+                      QSPI_CFG_DATA_MODE_FOUR_LINES,

+#endif

+#endif

+  .addr             = 0,

+  .alt              = 0

+};

+

+/* Initial M25Q_CMD_WRITE_ENABLE command.*/

+static const qspi_command_t m25q_cmd_write_enable = {

+  .cfg              = QSPI_CFG_CMD(M25Q_CMD_WRITE_ENABLE) |

+#if M25Q_SWITCH_WIDTH == TRUE

+                      QSPI_CFG_CMD_MODE_ONE_LINE,

+#else

+#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L

+                      QSPI_CFG_CMD_MODE_ONE_LINE,

+#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L

+                      QSPI_CFG_CMD_MODE_TWO_LINES,

+#else

+                      QSPI_CFG_CMD_MODE_FOUR_LINES,

+#endif

+#endif

+  .addr             = 0,

+  .alt              = 0

+};

+

+/* Bus width initialization.*/

+#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L

+static const uint8_t m25q_evconf_value[1] = {0xCF};

+#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L

+static const uint8_t m25q_evconf_value[1] = {0x8F};

+#else

+static const uint8_t m25q_evconf_value[1] = {0x4F};

+#endif

+#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */

+

+static const uint8_t m25q_manufacturer_ids[] = M25Q_SUPPORTED_MANUFACTURE_IDS;

+static const uint8_t m25q_memory_type_ids[] = M25Q_SUPPORTED_MEMORY_TYPE_IDS;

+

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

+/* Driver local functions.                                                   */

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

+

+static bool m25q_find_id(const uint8_t *set, size_t size, uint8_t element) {

+  size_t i;

+

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

+    if (set[i] == element) {

+      return true;

+    }

+  }

+  return false;

+}

+

+#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI) || defined(__DOXYGEN__)

+void m25q_reset_xip(M25QDriver *devp) {

+  static const uint8_t flash_conf[1] = {

+    (M25Q_READ_DUMMY_CYCLES << 4U) | 0x0FU

+  };

+  qspi_command_t cmd;

+  uint8_t buf[1];

+

+  /* Resetting XIP mode by reading one byte without XIP confirmation bit.*/

+  cmd.alt  = 0xFF;

+  cmd.addr = 0;

+  cmd.cfg  = QSPI_CFG_CMD_MODE_NONE |

+             QSPI_CFG_ADDR_SIZE_24 |

+#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L

+             QSPI_CFG_ADDR_MODE_ONE_LINE |

+             QSPI_CFG_DATA_MODE_ONE_LINE |

+#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L

+             QSPI_CFG_ADDR_MODE_TWO_LINES |

+             QSPI_CFG_DATA_MODE_TWO_LINES |

+#else

+             QSPI_CFG_ADDR_MODE_FOUR_LINES |

+             QSPI_CFG_DATA_MODE_FOUR_LINES |

+#endif

+             QSPI_CFG_ALT_MODE_FOUR_LINES |  /* Always 4 lines, note.*/

+             QSPI_CFG_ALT_SIZE_8 |

+             QSPI_CFG_DUMMY_CYCLES(M25Q_READ_DUMMY_CYCLES - 2);

+  qspiReceive(devp->config->busp, &cmd, 1, buf);

+

+  /* Enabling write operation.*/

+  jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);

+

+  /* Rewriting volatile configuration register.*/

+  jesd216_cmd_send(devp->config->busp, M25Q_CMD_WRITE_V_CONF_REGISTER,

+                   1, flash_conf);

+}

+

+void m25q_reset_memory(M25QDriver *devp) {

+

+  /* 1x M25Q_CMD_RESET_ENABLE command.*/

+  static const qspi_command_t cmd_reset_enable_1 = {

+    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_ENABLE) |

+                        QSPI_CFG_CMD_MODE_ONE_LINE,

+    .addr             = 0,

+    .alt              = 0

+  };

+

+  /* 1x M25Q_CMD_RESET_MEMORY command.*/

+  static const qspi_command_t cmd_reset_memory_1 = {

+    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_MEMORY) |

+                        QSPI_CFG_CMD_MODE_ONE_LINE,

+    .addr             = 0,

+    .alt              = 0

+  };

+

+  /* If the device is in one bit mode then the following commands are

+     rejected because shorter than 8 bits. If the device is in multiple

+     bits mode then the commands are accepted and the device is reset to

+     one bit mode.*/

+#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI4L

+  /* 4x M25Q_CMD_RESET_ENABLE command.*/

+  static const qspi_command_t cmd_reset_enable_4 = {

+    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_ENABLE) |

+                        QSPI_CFG_CMD_MODE_FOUR_LINES,

+    .addr             = 0,

+    .alt              = 0

+  };

+

+  /* 4x M25Q_CMD_RESET_MEMORY command.*/

+  static const qspi_command_t cmd_reset_memory_4 = {

+    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_MEMORY) |

+                        QSPI_CFG_CMD_MODE_FOUR_LINES,

+    .addr             = 0,

+    .alt              = 0

+  };

+

+  qspiCommand(devp->config->busp, &cmd_reset_enable_4);

+  qspiCommand(devp->config->busp, &cmd_reset_memory_4);

+#else

+  /* 2x M25Q_CMD_RESET_ENABLE command.*/

+  static const qspi_command_t cmd_reset_enable_2 = {

+    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_ENABLE) |

+                        QSPI_CFG_CMD_MODE_TWO_LINES,

+    .addr             = 0,

+    .alt              = 0

+  };

+

+  /* 2x M25Q_CMD_RESET_MEMORY command.*/

+  static const qspi_command_t cmd_reset_memory_2 = {

+    .cfg              = QSPI_CFG_CMD(M25Q_CMD_RESET_MEMORY) |

+                        QSPI_CFG_CMD_MODE_TWO_LINES,

+    .addr             = 0,

+    .alt              = 0

+  };

+

+  qspiCommand(devp->config->busp, &cmd_reset_enable_2);

+  qspiCommand(devp->config->busp, &cmd_reset_memory_2);

+#endif

+

+  /* Now the device should be in one bit mode for sure and we perform a

+     device reset.*/

+  qspiCommand(devp->config->busp, &cmd_reset_enable_1);

+  qspiCommand(devp->config->busp, &cmd_reset_memory_1);

+}

+#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */

+

+static flash_error_t m25q_poll_status(M25QDriver *devp) {

+  uint8_t sts;

+

+  do {

+#if M25Q_NICE_WAITING == TRUE

+    osalThreadSleepMilliseconds(1);

+#endif

+    /* Read status command.*/

+    jesd216_cmd_receive(devp->config->busp, M25Q_CMD_READ_FLAG_STATUS_REGISTER,

+                        1, &sts);

+  } while ((sts & M25Q_FLAGS_PROGRAM_ERASE) == 0U);

+

+  /* Checking for errors.*/

+  if ((sts & M25Q_FLAGS_ALL_ERRORS) != 0U) {

+    /* Clearing status register.*/

+    jesd216_cmd(devp->config->busp, M25Q_CMD_CLEAR_FLAG_STATUS_REGISTER);

+

+    /* Program operation failed.*/

+    return FLASH_ERROR_PROGRAM;

+  }

+

+  return FLASH_NO_ERROR;

+}

+

+static const flash_descriptor_t *m25q_get_descriptor(void *instance) {

+  M25QDriver *devp = (M25QDriver *)instance;

+

+  osalDbgCheck(instance != NULL);

+  osalDbgAssert((devp->state != FLASH_UNINIT) && (devp->state != FLASH_STOP),

+                "invalid state");

+

+  return &m25q_descriptor;

+}

+

+static flash_error_t m25q_read(void *instance, flash_offset_t offset,

+                               size_t n, uint8_t *rp) {

+  M25QDriver *devp = (M25QDriver *)instance;

+

+  osalDbgCheck((instance != NULL) && (rp != NULL) && (n > 0U));

+  osalDbgCheck((size_t)offset + n <= (size_t)m25q_descriptor.sectors_count *

+                                     (size_t)m25q_descriptor.sectors_size);

+  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),

+                "invalid state");

+

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

+    return FLASH_BUSY_ERASING;

+  }

+

+  /* Bus acquired.*/

+  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+  /* FLASH_READY state while the operation is performed.*/

+  devp->state = FLASH_READ;

+

+#if JESD216_BUS_MODE != JESD216_BUS_MODE_SPI

+  /* Fast read command in QSPI mode.*/

+  jesd216_cmd_addr_dummy_receive(devp->config->busp, M25Q_CMD_FAST_READ,

+                                 offset, M25Q_READ_DUMMY_CYCLES, n, rp);

+#else

+  /* Normal read command in SPI mode.*/

+  jesd216_cmd_addr_receive(devp->config->busp, M25Q_CMD_READ,

+                           offset, n, rp);

+#endif

+

+  /* Ready state again.*/

+  devp->state = FLASH_READY;

+

+  /* Bus released.*/

+  jesd216_bus_release(devp->config->busp);

+

+  return FLASH_NO_ERROR;

+}

+

+static flash_error_t m25q_program(void *instance, flash_offset_t offset,

+                                  size_t n, const uint8_t *pp) {

+  M25QDriver *devp = (M25QDriver *)instance;

+

+  osalDbgCheck((instance != NULL) && (pp != NULL) && (n > 0U));

+  osalDbgCheck((size_t)offset + n <= (size_t)m25q_descriptor.sectors_count *

+                                     (size_t)m25q_descriptor.sectors_size);

+  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),

+                "invalid state");

+

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

+    return FLASH_BUSY_ERASING;

+  }

+

+  /* Bus acquired.*/

+  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+  /* FLASH_PGM state while the operation is performed.*/

+  devp->state = FLASH_PGM;

+

+  /* Data is programmed page by page.*/

+  while (n > 0U) {

+    flash_error_t err;

+

+    /* Data size that can be written in a single program page operation.*/

+    size_t chunk = (size_t)(((offset | PAGE_MASK) + 1U) - offset);

+    if (chunk > n) {

+      chunk = n;

+    }

+

+    /* Enabling write operation.*/

+    jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);

+

+    /* Page program command.*/

+    jesd216_cmd_addr_send(devp->config->busp, M25Q_CMD_PAGE_PROGRAM, offset,

+                          chunk, pp);

+

+    /* Wait for status and check errors.*/

+    err = m25q_poll_status(devp);

+    if (err != FLASH_NO_ERROR) {

+

+      /* Bus released.*/

+      jesd216_bus_release(devp->config->busp);

+

+      return err;

+    }

+

+    /* Next page.*/

+    offset += chunk;

+    pp     += chunk;

+    n      -= chunk;

+  }

+

+  /* Ready state again.*/

+  devp->state = FLASH_READY;

+

+  /* Bus released.*/

+  jesd216_bus_release(devp->config->busp);

+

+  return FLASH_NO_ERROR;

+}

+

+static flash_error_t m25q_start_erase_all(void *instance) {

+  M25QDriver *devp = (M25QDriver *)instance;

+

+  osalDbgCheck(instance != NULL);

+  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),

+                "invalid state");

+

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

+    return FLASH_BUSY_ERASING;

+  }

+

+  /* Bus acquired.*/

+  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+  /* FLASH_ERASE state while the operation is performed.*/

+  devp->state = FLASH_ERASE;

+

+  /* Enabling write operation.*/

+  jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);

+

+  /* Bulk erase command.*/

+  jesd216_cmd(devp->config->busp, M25Q_CMD_BULK_ERASE);

+

+  /* Bus released.*/

+  jesd216_bus_release(devp->config->busp);

+

+  return FLASH_NO_ERROR;

+}

+

+static flash_error_t m25q_start_erase_sector(void *instance,

+                                             flash_sector_t sector) {

+  M25QDriver *devp = (M25QDriver *)instance;

+  flash_offset_t offset = (flash_offset_t)(sector * SECTOR_SIZE);

+

+  osalDbgCheck(instance != NULL);

+  osalDbgCheck(sector < m25q_descriptor.sectors_count);

+  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),

+                "invalid state");

+

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

+    return FLASH_BUSY_ERASING;

+  }

+

+  /* Bus acquired.*/

+  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+  /* FLASH_ERASE state while the operation is performed.*/

+  devp->state = FLASH_ERASE;

+

+  /* Enabling write operation.*/

+  jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);

+

+  /* Sector erase command.*/

+  jesd216_cmd_addr(devp->config->busp, M25Q_CMD_SECTOR_ERASE, offset);

+

+  /* Bus released.*/

+  jesd216_bus_release(devp->config->busp);

+

+  return FLASH_NO_ERROR;

+}

+

+static flash_error_t m25q_verify_erase(void *instance,

+                                       flash_sector_t sector) {

+  M25QDriver *devp = (M25QDriver *)instance;

+  uint8_t cmpbuf[M25Q_COMPARE_BUFFER_SIZE];

+  flash_offset_t offset;

+  size_t n;

+

+  osalDbgCheck(instance != NULL);

+  osalDbgCheck(sector < m25q_descriptor.sectors_count);

+  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),

+                "invalid state");

+

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

+    return FLASH_BUSY_ERASING;

+  }

+

+  /* Bus acquired.*/

+  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+  /* FLASH_READY state while the operation is performed.*/

+  devp->state = FLASH_READ;

+

+  /* Read command.*/

+  offset = (flash_offset_t)(sector * SECTOR_SIZE);

+  n = SECTOR_SIZE;

+  while (n > 0U) {

+    uint8_t *p;

+

+#if JESD216_BUS_MODE != JESD216_BUS_MODE_SPI

+   jesd216_cmd_addr_dummy_receive(devp->config->busp, M25Q_CMD_FAST_READ,

+                                  offset, M25Q_READ_DUMMY_CYCLES,

+                                  sizeof cmpbuf, cmpbuf);

+#else

+   /* Normal read command in SPI mode.*/

+   jesd216_cmd_addr_receive(devp->config->busp, M25Q_CMD_READ,

+                            offset, sizeof cmpbuf, cmpbuf);

+#endif

+

+    /* Checking for erased state of current buffer.*/

+    for (p = cmpbuf; p < &cmpbuf[M25Q_COMPARE_BUFFER_SIZE]; p++) {

+      if (*p != 0xFFU) {

+        /* Ready state again.*/

+        devp->state = FLASH_READY;

+

+        /* Bus released.*/

+        jesd216_bus_release(devp->config->busp);

+

+        return FLASH_ERROR_VERIFY;

+      }

+    }

+

+    offset += sizeof cmpbuf;

+    n -= sizeof cmpbuf;

+  }

+

+  /* Ready state again.*/

+  devp->state = FLASH_READY;

+

+  /* Bus released.*/

+  jesd216_bus_release(devp->config->busp);

+

+  return FLASH_NO_ERROR;

+}

+

+static flash_error_t m25q_query_erase(void *instance, uint32_t *msec) {

+  M25QDriver *devp = (M25QDriver *)instance;

+  uint8_t sts;

+

+  osalDbgCheck(instance != NULL);

+  osalDbgAssert((devp->state == FLASH_READY) || (devp->state == FLASH_ERASE),

+                "invalid state");

+

+  /* If there is an erase in progress then the device must be checked.*/

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

+

+    /* Bus acquired.*/

+    jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+    /* Read status command.*/

+    jesd216_cmd_receive(devp->config->busp, M25Q_CMD_READ_FLAG_STATUS_REGISTER,

+                        1, &sts);

+

+    /* If the P/E bit is zero (busy) or the flash in a suspended state then

+       report that the operation is still in progress.*/

+    if (((sts & M25Q_FLAGS_PROGRAM_ERASE) == 0U) ||

+        ((sts & M25Q_FLAGS_ERASE_SUSPEND) != 0U)) {

+

+      /* Bus released.*/

+      jesd216_bus_release(devp->config->busp);

+

+      /* Recommended time before polling again, this is a simplified

+         implementation.*/

+      if (msec != NULL) {

+        *msec = 1U;

+      }

+

+      return FLASH_BUSY_ERASING;

+    }

+

+    /* The device is ready to accept commands.*/

+    devp->state = FLASH_READY;

+

+    /* Checking for errors.*/

+    if ((sts & M25Q_FLAGS_ALL_ERRORS) != 0U) {

+

+      /* Clearing status register.*/

+      jesd216_cmd(devp->config->busp, M25Q_CMD_CLEAR_FLAG_STATUS_REGISTER);

+

+      /* Erase operation failed.*/

+      return FLASH_ERROR_ERASE;

+    }

+

+    /* Bus released.*/

+    jesd216_bus_release(devp->config->busp);

+  }

+

+  return FLASH_NO_ERROR;

+}

+

+static flash_error_t m25q_read_sfdp(void *instance, flash_offset_t offset,

+                                    size_t n, uint8_t *rp) {

+

+  (void)instance;

+  (void)rp;

+  (void)offset;

+  (void)n;

+

+  return FLASH_NO_ERROR;

+}

+

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

+/* Driver exported functions.                                                */

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

+

+/**

+ * @brief   Initializes an instance.

+ *

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

+ *

+ * @init

+ */

+void m25qObjectInit(M25QDriver *devp) {

+

+  osalDbgCheck(devp != NULL);

+

+  devp->vmt         = &m25q_vmt;

+  devp->state       = FLASH_STOP;

+  devp->config      = NULL;

+}

+

+/**

+ * @brief   Configures and activates N25Q128 driver.

+ *

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

+ * @param[in] config    pointer to the configuration

+ *

+ * @api

+ */

+void m25qStart(M25QDriver *devp, const M25QConfig *config) {

+

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

+  osalDbgAssert(devp->state != FLASH_UNINIT, "invalid state");

+

+  devp->config = config;

+

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

+

+    /* Bus acquisition.*/

+    jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+#if JESD216_BUS_MODE == JESD216_BUS_MODE_SPI

+    /* Reading device ID.*/

+    jesd216_cmd_receive(devp->config->busp, M25Q_CMD_READ_ID,

+                        sizeof devp->device_id, devp->device_id);

+

+#else /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */

+    /* Attempting a reset of the XIP mode, it could be in an unexpected state

+       because a CPU reset does not reset the memory too.*/

+    m25q_reset_xip(devp);

+

+    /* Attempting a eeset of the device, it could be in an unexpected state

+       because a CPU reset does not reset the memory too.*/

+    m25q_reset_memory(devp);

+

+    /* Reading device ID and unique ID.*/

+    qspiReceive(devp->config->busp, &m25q_cmd_read_id,

+                sizeof devp->device_id, devp->device_id);

+#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */

+

+    /* Checking if the device is white listed.*/

+    osalDbgAssert(m25q_find_id(m25q_manufacturer_ids,

+                          sizeof m25q_manufacturer_ids,

+                          devp->device_id[0]),

+                  "invalid manufacturer id");

+    osalDbgAssert(m25q_find_id(m25q_memory_type_ids,

+                          sizeof m25q_memory_type_ids,

+                          devp->device_id[1]),

+                  "invalid memory type id");

+

+#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI) && (M25Q_SWITCH_WIDTH == TRUE)

+    /* Setting up final bus width.*/

+    qspiCommand(devp->config->busp, &m25q_cmd_write_enable);

+    qspiSend(devp->config->busp, &m25q_cmd_write_evconf, 1, m25q_evconf_value);

+

+    {

+      uint8_t id[3];

+

+      /* Reading ID again for confirmation.*/

+      jesd216_cmd_receive(devp->config->busp, M25Q_CMD_MULTIPLE_IO_READ_ID,

+                          3, id);

+

+      /* Checking if the device is white listed.*/

+      osalDbgAssert(memcmp(id, devp->device_id, 3) == 0,

+                    "id confirmation failed");

+    }

+#endif

+

+    /* Setting up the device size.*/

+    m25q_descriptor.sectors_count = (1U << (size_t)devp->device_id[2]) /

+                                    SECTOR_SIZE;

+

+#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI)

+    {

+      static const uint8_t flash_conf[1] = {

+        (M25Q_READ_DUMMY_CYCLES << 4U) | 0x0FU

+      };

+

+      /* Setting up the dummy cycles to be used for fast read operations.*/

+      jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);

+      jesd216_cmd_send(devp->config->busp, M25Q_CMD_WRITE_V_CONF_REGISTER,

+                       1, flash_conf);

+    }

+#endif

+

+    /* Driver in ready state.*/

+    devp->state = FLASH_READY;

+

+    /* Bus release.*/

+    jesd216_bus_release(devp->config->busp);

+  }

+} 

+

+/**

+ * @brief   Deactivates the N25Q128 driver.

+ *

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

+ *

+ * @api

+ */

+void m25qStop(M25QDriver *devp) {

+

+  osalDbgCheck(devp != NULL);

+  osalDbgAssert(devp->state != FLASH_UNINIT, "invalid state");

+

+  if (devp->state != FLASH_STOP) {

+

+    /* Bus acquisition.*/

+    jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+    /* Stopping bus device.*/

+    jesd216_stop(devp->config->busp);

+

+    /* Deleting current configuration.*/

+    devp->config = NULL;

+

+    /* Driver stopped.*/

+    devp->state = FLASH_STOP;

+

+    /* Bus release.*/

+    jesd216_bus_release(devp->config->busp);

+  }

+}

+

+#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI) || defined(__DOXYGEN__)

+#if (QSPI_SUPPORTS_MEMMAP == TRUE) || defined(__DOXYGEN__)

+/**

+ * @brief   Enters the memory Mapping mode.

+ * @details The memory mapping mode is only available when the QSPI mode

+ *          is selected and the underlying QSPI controller supports the

+ *          feature.

+ *

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

+ * @param[out] addrp    pointer to the memory start address of the mapped

+ *                      flash or @p NULL

+ *

+ * @api

+ */

+void m25qMemoryMap(M25QDriver *devp, uint8_t **addrp) {

+  static const uint8_t flash_status_xip[1] = {

+    (M25Q_READ_DUMMY_CYCLES << 4U) | 0x07U

+  };

+  qspi_command_t cmd;

+

+  /* Bus acquisition.*/

+  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+  /* Activating XIP mode in the device.*/

+  jesd216_cmd(devp->config->busp, M25Q_CMD_WRITE_ENABLE);

+  jesd216_cmd_send(devp->config->busp, M25Q_CMD_WRITE_V_CONF_REGISTER,

+                   1, flash_status_xip);

+

+  /* Putting the QSPI driver in memory mapped mode.*/

+  cmd.cfg = QSPI_CFG_CMD(M25Q_CMD_FAST_READ) |

+            QSPI_CFG_ADDR_SIZE_24 |

+#if JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI1L

+            QSPI_CFG_CMD_MODE_ONE_LINE |

+            QSPI_CFG_ADDR_MODE_ONE_LINE |

+            QSPI_CFG_DATA_MODE_ONE_LINE |

+#elif JESD216_BUS_MODE == JESD216_BUS_MODE_QSPI2L

+            QSPI_CFG_CMD_MODE_TWO_LINES |

+            QSPI_CFG_ADDR_MODE_TWO_LINES |

+            QSPI_CFG_DATA_MODE_TWO_LINES |

+#else

+            QSPI_CFG_CMD_MODE_FOUR_LINES |

+            QSPI_CFG_ADDR_MODE_FOUR_LINES |

+            QSPI_CFG_DATA_MODE_FOUR_LINES |

+#endif

+            QSPI_CFG_ALT_MODE_FOUR_LINES |  /* Always 4 lines, note.*/

+            QSPI_CFG_ALT_SIZE_8 |

+            QSPI_CFG_SIOO |

+            QSPI_CFG_DUMMY_CYCLES(M25Q_READ_DUMMY_CYCLES - 2);

+

+  qspiMapFlash(devp->config->busp, &cmd, addrp);

+

+  /* Bus release.*/

+  jesd216_bus_release(devp->config->busp);

+}

+

+/**

+ * @brief   Leaves the memory Mapping mode.

+ *

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

+ *

+ * @api

+ */

+void m25qMemoryUnmap(M25QDriver *devp) {

+

+  /* Bus acquisition.*/

+  jesd216_bus_acquire(devp->config->busp, devp->config->buscfg);

+

+  qspiUnmapFlash(devp->config->busp);

+

+  m25q_reset_xip(devp);

+

+  /* Bus release.*/

+  jesd216_bus_release(devp->config->busp);

+}

+#endif /* QSPI_SUPPORTS_MEMMAP == TRUE */

+#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */

+

+/** @} */

diff --git a/os/ex/Micron/m25q.h b/os/ex/Micron/m25q.h
index 230df9814..0a5a6f31b 100644
--- a/os/ex/Micron/m25q.h
+++ b/os/ex/Micron/m25q.h
@@ -1,273 +1,273 @@
-/*
-    ChibiOS - Copyright (C) 2016 Giovanni Di Sirio
-
-    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    m25q.h
- * @brief   Micron serial flash driver header.
- *
- * @addtogroup m25q
- * @{
- */
-
-#ifndef M25Q_H
-#define M25Q_H
-
-#include "hal_jesd216_flash.h"
-
-/*===========================================================================*/
-/* Driver constants.                                                         */
-/*===========================================================================*/
-
-/**
- * @name    Version identification
- * @{
- */
-/**
- * @brief   M25Q driver version string.
- */
-#define EX_M25Q_VERSION         "1.0.0"
-
-/**
- * @brief   M25Q driver version major number.
- */
-#define EX_M25Q_MAJOR           1
-
-/**
- * @brief   M25Q driver version minor number.
- */
-#define EX_M25Q__MINOR          0
-
-/**
- * @brief   M25Q driver version patch number.
- */
-#define EX_M25Q_PATCH           0
-/** @} */
-
-/**
- * @name    Command codes
- * @{
- */
-#define M25Q_CMD_RESET_ENABLE                       0x66
-#define M25Q_CMD_RESET_MEMORY                       0x99
-#define M25Q_CMD_READ_ID                            0x9F
-#define M25Q_CMD_MULTIPLE_IO_READ_ID                0xAF
-#define M25Q_CMD_READ_DISCOVERY_PARAMETER           0x5A
-#define M25Q_CMD_READ                               0x03
-#define M25Q_CMD_FAST_READ                          0x0B
-#define M25Q_CMD_WRITE_ENABLE                       0x06
-#define M25Q_CMD_WRITE_DISABLE                      0x04
-#define M25Q_CMD_READ_STATUS_REGISTER               0x05
-#define M25Q_CMD_WRITE_STATUS_REGISTER              0x01
-#define M25Q_CMD_READ_LOCK_REGISTER                 0xE8
-#define M25Q_CMD_WRITE_LOCK_REGISTER                0xE5
-#define M25Q_CMD_READ_FLAG_STATUS_REGISTER          0x70
-#define M25Q_CMD_CLEAR_FLAG_STATUS_REGISTER         0x50
-#define M25Q_CMD_READ_NV_CONFIGURATION_REGISTER     0xB5
-#define M25Q_CMD_WRITE_NV_CONFIGURATION_REGISTER    0xB1
-#define M25Q_CMD_READ_V_CONF_REGISTER               0x85
-#define M25Q_CMD_WRITE_V_CONF_REGISTER              0x81
-#define M25Q_CMD_READ_ENHANCED_V_CONF_REGISTER      0x65
-#define M25Q_CMD_WRITE_ENHANCED_V_CONF_REGISTER     0x61
-#define M25Q_CMD_PAGE_PROGRAM                       0x02
-#define M25Q_CMD_SUBSECTOR_ERASE                    0x20
-#define M25Q_CMD_SECTOR_ERASE                       0xD8
-#define M25Q_CMD_BULK_ERASE                         0xC7
-#define M25Q_CMD_PROGRAM_ERASE_RESUME               0x7A
-#define M25Q_CMD_PROGRAM_ERASE_SUSPEND              0x75
-#define M25Q_CMD_READ_OTP_ARRAY                     0x4B
-#define M25Q_CMD_PROGRAM_OTP_ARRAY                  0x42
-/** @} */
-
-/**
- * @name    Flags status register bits
- * @{
- */
-#define M25Q_FLAGS_PROGRAM_ERASE                    0x80U
-#define M25Q_FLAGS_ERASE_SUSPEND                    0x40U
-#define M25Q_FLAGS_ERASE_ERROR                      0x20U
-#define M25Q_FLAGS_PROGRAM_ERROR                    0x10U
-#define M25Q_FLAGS_VPP_ERROR                        0x08U
-#define M25Q_FLAGS_PROGRAM_SUSPEND                  0x04U
-#define M25Q_FLAGS_PROTECTION_ERROR                 0x02U
-#define M25Q_FLAGS_RESERVED                         0x01U
-#define M25Q_FLAGS_ALL_ERRORS                   (M25Q_FLAGS_ERASE_ERROR |   \
-                                                 M25Q_FLAGS_PROGRAM_ERROR | \
-                                                 M25Q_FLAGS_VPP_ERROR |     \
-                                                 M25Q_FLAGS_PROTECTION_ERROR)
-/** @} */
-
-/*===========================================================================*/
-/* Driver pre-compile time settings.                                         */
-/*===========================================================================*/
-
-/**
- * @name    Configuration options
- * @{
- */
-/**
- * @brief   Number of dummy cycles for fast read (1..15).
- * @details This is the number of dummy cycles to be used for fast read
- *          operations.
- */
-#if !defined(M25Q_READ_DUMMY_CYCLES) || defined(__DOXYGEN__)
-#define M25Q_READ_DUMMY_CYCLES              8
-#endif
-
-/**
- * @brief   Switch QSPI bus width on initialization.
- * @details A bus width initialization is performed by writing the
- *          Enhanced Volatile Configuration Register. If the flash
- *          device is configured using the Non Volatile Configuration
- *          Register then this option is not required.
- * @note    This option is only valid in QSPI bus modes.
- */
-#if !defined(M25Q_SWITCH_WIDTH) || defined(__DOXYGEN__)
-#define M25Q_SWITCH_WIDTH                   TRUE
-#endif
-
-/**
- * @brief   Delays insertions.
- * @details If enabled this options inserts delays into the flash waiting
- *          routines releasing some extra CPU time for threads with lower
- *          priority, this may slow down the driver a bit however.
- */
-#if !defined(M25Q_NICE_WAITING) || defined(__DOXYGEN__)
-#define M25Q_NICE_WAITING                   TRUE
-#endif
-
-/**
- * @brief   Uses 4kB sub-sectors rather than 64kB sectors.
- */
-#if !defined(M25Q_USE_SUB_SECTORS) || defined(__DOXYGEN__)
-#define M25Q_USE_SUB_SECTORS                FALSE
-#endif
-
-/**
- * @brief   Supported JEDEC manufacturer identifiers.
- */
-#if !defined(M25Q_SUPPORTED_MANUFACTURE_IDS) || defined(__DOXYGEN__)
-#define M25Q_SUPPORTED_MANUFACTURE_IDS      {0x20}
-#endif
-
-/**
- * @brief   Supported memory type identifiers.
- */
-#if !defined(M25Q_SUPPORTED_MEMORY_TYPE_IDS) || defined(__DOXYGEN__)
-#define M25Q_SUPPORTED_MEMORY_TYPE_IDS      {0xBA, 0xBB}
-#endif
-
-/**
- * @brief   Size of the compare buffer.
- * @details This buffer is allocated in the stack frame of the function
- *          @p flashVerifyErase() and its size must be a power of two.
- *          Larger buffers lead to better verify performance but increase
- *          stack usage for that function.
- */
-#if !defined(M25Q_COMPARE_BUFFER_SIZE) || defined(__DOXYGEN__)
-#define M25Q_COMPARE_BUFFER_SIZE            32
-#endif
-/** @} */
-
-/*===========================================================================*/
-/* Derived constants and error checks.                                       */
-/*===========================================================================*/
-
-#if (M25Q_READ_DUMMY_CYCLES < 1) || (M25Q_READ_DUMMY_CYCLES > 15)
-#error "invalid M25Q_READ_DUMMY_CYCLES value (1..15)"
-#endif
-
-#if (M25Q_COMPARE_BUFFER_SIZE & (M25Q_COMPARE_BUFFER_SIZE - 1)) != 0
-#error "invalid M25Q_COMPARE_BUFFER_SIZE value"
-#endif
-
-/*===========================================================================*/
-/* Driver data structures and types.                                         */
-/*===========================================================================*/
-
-/**
- * @brief   Type of a M25Q configuration structure.
- */
-typedef struct {
-  _jesd216_config
-} M25QConfig;
-
-/**
- * @brief   @p M25Q specific methods.
- */
-#define _m25q_methods                                                       \
-  _jesd216_flash_methods
-
-/**
- * @extends JESD216FlashVMT
- *
- * @brief   @p M25Q virtual methods table.
- */
-struct M25QDriverVMT {
-  _m25q_methods
-};
-  
-/**
- * @extends JESD216Flash
- *
- * @brief   Type of M25Q flash class.
- */
-typedef struct {
-  /**
-   * @brief   M25QDriver Virtual Methods Table.
-   */
-  const struct M25QDriverVMT    *vmt;
-  _jesd216_flash_data
-  /**
-   * @brief   Current configuration data.
-   */
-  const M25QConfig              *config;
-  /**
-   * @brief   Device ID and unique ID.
-   */
-  uint8_t                       device_id[20];
-} M25QDriver;
-
-/*===========================================================================*/
-/* Driver macros.                                                            */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* External declarations.                                                    */
-/*===========================================================================*/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-  void m25qObjectInit(M25QDriver *devp);
-  void m25qStart(M25QDriver *devp, const M25QConfig *config);
-  void m25qStop(M25QDriver *devp);
-#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI) || defined(__DOXYGEN__)
-#if (QSPI_SUPPORTS_MEMMAP == TRUE) || defined(__DOXYGEN__)
-  void m25qMemoryMap(M25QDriver *devp, uint8_t ** addrp);
-  void m25qMemoryUnmap(M25QDriver *devp);
-#endif /* QSPI_SUPPORTS_MEMMAP == TRUE */
-#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* M25Q_H */
-
-/** @} */
-
+/*

+    ChibiOS - Copyright (C) 2016 Giovanni Di Sirio

+

+    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    m25q.h

+ * @brief   Micron serial flash driver header.

+ *

+ * @addtogroup m25q

+ * @{

+ */

+

+#ifndef M25Q_H

+#define M25Q_H

+

+#include "hal_jesd216_flash.h"

+

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

+/* Driver constants.                                                         */

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

+

+/**

+ * @name    Version identification

+ * @{

+ */

+/**

+ * @brief   M25Q driver version string.

+ */

+#define EX_M25Q_VERSION         "1.0.0"

+

+/**

+ * @brief   M25Q driver version major number.

+ */

+#define EX_M25Q_MAJOR           1

+

+/**

+ * @brief   M25Q driver version minor number.

+ */

+#define EX_M25Q__MINOR          0

+

+/**

+ * @brief   M25Q driver version patch number.

+ */

+#define EX_M25Q_PATCH           0

+/** @} */

+

+/**

+ * @name    Command codes

+ * @{

+ */

+#define M25Q_CMD_RESET_ENABLE                       0x66

+#define M25Q_CMD_RESET_MEMORY                       0x99

+#define M25Q_CMD_READ_ID                            0x9F

+#define M25Q_CMD_MULTIPLE_IO_READ_ID                0xAF

+#define M25Q_CMD_READ_DISCOVERY_PARAMETER           0x5A

+#define M25Q_CMD_READ                               0x03

+#define M25Q_CMD_FAST_READ                          0x0B

+#define M25Q_CMD_WRITE_ENABLE                       0x06

+#define M25Q_CMD_WRITE_DISABLE                      0x04

+#define M25Q_CMD_READ_STATUS_REGISTER               0x05

+#define M25Q_CMD_WRITE_STATUS_REGISTER              0x01

+#define M25Q_CMD_READ_LOCK_REGISTER                 0xE8

+#define M25Q_CMD_WRITE_LOCK_REGISTER                0xE5

+#define M25Q_CMD_READ_FLAG_STATUS_REGISTER          0x70

+#define M25Q_CMD_CLEAR_FLAG_STATUS_REGISTER         0x50

+#define M25Q_CMD_READ_NV_CONFIGURATION_REGISTER     0xB5

+#define M25Q_CMD_WRITE_NV_CONFIGURATION_REGISTER    0xB1

+#define M25Q_CMD_READ_V_CONF_REGISTER               0x85

+#define M25Q_CMD_WRITE_V_CONF_REGISTER              0x81

+#define M25Q_CMD_READ_ENHANCED_V_CONF_REGISTER      0x65

+#define M25Q_CMD_WRITE_ENHANCED_V_CONF_REGISTER     0x61

+#define M25Q_CMD_PAGE_PROGRAM                       0x02

+#define M25Q_CMD_SUBSECTOR_ERASE                    0x20

+#define M25Q_CMD_SECTOR_ERASE                       0xD8

+#define M25Q_CMD_BULK_ERASE                         0xC7

+#define M25Q_CMD_PROGRAM_ERASE_RESUME               0x7A

+#define M25Q_CMD_PROGRAM_ERASE_SUSPEND              0x75

+#define M25Q_CMD_READ_OTP_ARRAY                     0x4B

+#define M25Q_CMD_PROGRAM_OTP_ARRAY                  0x42

+/** @} */

+

+/**

+ * @name    Flags status register bits

+ * @{

+ */

+#define M25Q_FLAGS_PROGRAM_ERASE                    0x80U

+#define M25Q_FLAGS_ERASE_SUSPEND                    0x40U

+#define M25Q_FLAGS_ERASE_ERROR                      0x20U

+#define M25Q_FLAGS_PROGRAM_ERROR                    0x10U

+#define M25Q_FLAGS_VPP_ERROR                        0x08U

+#define M25Q_FLAGS_PROGRAM_SUSPEND                  0x04U

+#define M25Q_FLAGS_PROTECTION_ERROR                 0x02U

+#define M25Q_FLAGS_RESERVED                         0x01U

+#define M25Q_FLAGS_ALL_ERRORS                   (M25Q_FLAGS_ERASE_ERROR |   \

+                                                 M25Q_FLAGS_PROGRAM_ERROR | \

+                                                 M25Q_FLAGS_VPP_ERROR |     \

+                                                 M25Q_FLAGS_PROTECTION_ERROR)

+/** @} */

+

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

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

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

+

+/**

+ * @name    Configuration options

+ * @{

+ */

+/**

+ * @brief   Number of dummy cycles for fast read (1..15).

+ * @details This is the number of dummy cycles to be used for fast read

+ *          operations.

+ */

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

+#define M25Q_READ_DUMMY_CYCLES              8

+#endif

+

+/**

+ * @brief   Switch QSPI bus width on initialization.

+ * @details A bus width initialization is performed by writing the

+ *          Enhanced Volatile Configuration Register. If the flash

+ *          device is configured using the Non Volatile Configuration

+ *          Register then this option is not required.

+ * @note    This option is only valid in QSPI bus modes.

+ */

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

+#define M25Q_SWITCH_WIDTH                   TRUE

+#endif

+

+/**

+ * @brief   Delays insertions.

+ * @details If enabled this options inserts delays into the flash waiting

+ *          routines releasing some extra CPU time for threads with lower

+ *          priority, this may slow down the driver a bit however.

+ */

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

+#define M25Q_NICE_WAITING                   TRUE

+#endif

+

+/**

+ * @brief   Uses 4kB sub-sectors rather than 64kB sectors.

+ */

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

+#define M25Q_USE_SUB_SECTORS                FALSE

+#endif

+

+/**

+ * @brief   Supported JEDEC manufacturer identifiers.

+ */

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

+#define M25Q_SUPPORTED_MANUFACTURE_IDS      {0x20}

+#endif

+

+/**

+ * @brief   Supported memory type identifiers.

+ */

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

+#define M25Q_SUPPORTED_MEMORY_TYPE_IDS      {0xBA, 0xBB}

+#endif

+

+/**

+ * @brief   Size of the compare buffer.

+ * @details This buffer is allocated in the stack frame of the function

+ *          @p flashVerifyErase() and its size must be a power of two.

+ *          Larger buffers lead to better verify performance but increase

+ *          stack usage for that function.

+ */

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

+#define M25Q_COMPARE_BUFFER_SIZE            32

+#endif

+/** @} */

+

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

+/* Derived constants and error checks.                                       */

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

+

+#if (M25Q_READ_DUMMY_CYCLES < 1) || (M25Q_READ_DUMMY_CYCLES > 15)

+#error "invalid M25Q_READ_DUMMY_CYCLES value (1..15)"

+#endif

+

+#if (M25Q_COMPARE_BUFFER_SIZE & (M25Q_COMPARE_BUFFER_SIZE - 1)) != 0

+#error "invalid M25Q_COMPARE_BUFFER_SIZE value"

+#endif

+

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

+/* Driver data structures and types.                                         */

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

+

+/**

+ * @brief   Type of a M25Q configuration structure.

+ */

+typedef struct {

+  _jesd216_config

+} M25QConfig;

+

+/**

+ * @brief   @p M25Q specific methods.

+ */

+#define _m25q_methods                                                       \

+  _jesd216_flash_methods

+

+/**

+ * @extends JESD216FlashVMT

+ *

+ * @brief   @p M25Q virtual methods table.

+ */

+struct M25QDriverVMT {

+  _m25q_methods

+};

+  

+/**

+ * @extends JESD216Flash

+ *

+ * @brief   Type of M25Q flash class.

+ */

+typedef struct {

+  /**

+   * @brief   M25QDriver Virtual Methods Table.

+   */

+  const struct M25QDriverVMT    *vmt;

+  _jesd216_flash_data

+  /**

+   * @brief   Current configuration data.

+   */

+  const M25QConfig              *config;

+  /**

+   * @brief   Device ID and unique ID.

+   */

+  uint8_t                       device_id[20];

+} M25QDriver;

+

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

+/* Driver macros.                                                            */

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

+

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

+/* External declarations.                                                    */

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

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void m25qObjectInit(M25QDriver *devp);

+  void m25qStart(M25QDriver *devp, const M25QConfig *config);

+  void m25qStop(M25QDriver *devp);

+#if (JESD216_BUS_MODE != JESD216_BUS_MODE_SPI) || defined(__DOXYGEN__)

+#if (QSPI_SUPPORTS_MEMMAP == TRUE) || defined(__DOXYGEN__)

+  void m25qMemoryMap(M25QDriver *devp, uint8_t ** addrp);

+  void m25qMemoryUnmap(M25QDriver *devp);

+#endif /* QSPI_SUPPORTS_MEMMAP == TRUE */

+#endif /* JESD216_BUS_MODE != JESD216_BUS_MODE_SPI */

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* M25Q_H */

+

+/** @} */

+

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_ */

+

+/** @} */

diff --git a/os/ex/subsystems/mfs/mfs.c b/os/ex/subsystems/mfs/mfs.c
index d232b664d..99a3c0c1b 100644
--- a/os/ex/subsystems/mfs/mfs.c
+++ b/os/ex/subsystems/mfs/mfs.c
@@ -1,683 +1,683 @@
-/*
-    Managed Flash Storage - Copyright (C) 2016 Giovanni Di Sirio
-
-    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    mfs.c
- * @brief   Managed Flash Storage module code.
- * @details This module manages a flash partition as a generic storage where
- *          arbitrary data records can be created, updated, deleted and
- *          retrieved.<br>
- *          A managed partition is composed of two banks of equal size, a
- *          bank is composed of one or more erasable sectors, a sector is
- *          divided in writable pages.<br>
- *          The module handles flash wear leveling and recovery of damaged
- *          banks (where possible) caused by power loss during operations.
- *          Both operations are transparent to the user.
- *
- * @addtogroup mfs
- * @{
- */
-
-#include "hal.h"
-
-#include "mfs.h"
-
-/*===========================================================================*/
-/* Driver local definitions.                                                 */
-/*===========================================================================*/
-
-#define PAIR(a, b) (((unsigned)(a) << 2U) | (unsigned)(b))
-
-/**
- * @brief   Error check helper.
- */
-#define RET_ON_ERROR(err) do {                                              \
-  mfs_error_t e = (err);                                                    \
-  if (e != MFS_NO_ERROR) {                                                  \
-    return e;                                                               \
-  }                                                                         \
-} while (false)
-
-/*===========================================================================*/
-/* Driver exported variables.                                                */
-/*===========================================================================*/
-
-/*===========================================================================*/
-/* Driver local variables and types.                                         */
-/*===========================================================================*/
-
-const uint16_t crc16_table[256] = {
-  0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
-  0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
-  0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
-  0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
-  0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
-  0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
-  0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
-  0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
-  0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
-  0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
-  0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
-  0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
-  0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
-  0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
-  0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
-  0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
-  0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
-  0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
-  0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
-  0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
-  0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
-  0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
-  0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
-  0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
-  0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
-  0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
-  0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
-  0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
-  0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
-  0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
-  0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
-  0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
-};
-
-/*===========================================================================*/
-/* Driver local functions.                                                   */
-/*===========================================================================*/
-
-uint16_t crc16(uint16_t crc, const uint8_t *data, size_t n) {
-
-  while (n > 0U) {
-    crc = (crc << 8U) ^ crc16_table[(crc >> 8U) ^ (uint16_t)*data];
-    data++;
-    n--;
-  }
-
-  return crc;
-}
-
-#if (MFS_CFG_ID_CACHE_SIZE > 0) || defined(__DOXYGEN__)
-void mfs_cache_init(MFSDriver *devp) {
-
-  (void)devp;
-}
-
-mfs_cached_id_t *mfs_cache_find_id(MFSDriver *devp, uint32_t id) {
-
-  (void)devp;
-  (void)id;
-
-  return NULL;
-}
-
-void mfs_cache_update_id(MFSDriver *devp, uint32_t id) {
-
-  (void)devp;
-  (void)id;
-}
-
-void mfs_cache_erase_id(MFSDriver *devp, uint32_t id) {
-
-  (void)devp;
-  (void)id;
-}
-#endif /* MFS_CFG_ID_CACHE_SIZE > 0 */
-
-/**
- * @brief   Flash write.
- * @note    If the option @p MFS_CFG_WRITE_VERIFY is enabled then the flash
- *          is also read back for verification.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] offset    flash offset
- * @param[in] n         number of bytes to be read
- * @param[out] rp       pointer to the data buffer
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW
- *                      failures.
- *
- * @notapi
- */
-static mfs_error_t mfs_flash_write(MFSDriver *devp,
-                                   flash_offset_t offset,
-                                   size_t n, const
-                                   uint8_t *p) {
-  flash_error_t ferr;
-
-  ferr = flashProgram(devp->config->flashp, offset, n, p);
-  if (ferr != FLASH_NO_ERROR) {
-    return MFS_FLASH_FAILURE;
-  }
-
-  return MFS_NO_ERROR;
-}
-
-/**
- * @brief   Erases and verifies all sectors belonging to a bank.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] bank      bank to be erased
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW
- *                      failures.
- *
- * @notapi
- */
-static mfs_error_t mfs_bank_erase(MFSDriver *devp, mfs_bank_t bank) {
-  flash_sector_t sector, end;
-
-  if (bank == MFS_BANK_0) {
-    sector = devp->config->bank0_start;
-    end    = devp->config->bank0_start + devp->config->bank0_sectors;
-  }
-  else {
-    sector = devp->config->bank1_start;
-    end    = devp->config->bank1_start + devp->config->bank1_sectors;
-  }
-
-  while (sector < end) {
-    flash_error_t ferr;
-
-    ferr = flashStartEraseSector(devp->config->flashp, sector);
-    if (ferr != FLASH_NO_ERROR) {
-      return MFS_FLASH_FAILURE;
-    }
-    ferr = flashWaitErase(devp->config->flashp);
-    if (ferr != FLASH_NO_ERROR) {
-      return MFS_FLASH_FAILURE;
-    }
-    ferr = flashVerifyErase(devp->config->flashp, sector);
-    if (ferr != FLASH_NO_ERROR) {
-      return MFS_FLASH_FAILURE;
-    }
-
-    sector++;
-  }
-
-  return MFS_NO_ERROR;
-}
-
-/**
- * @brief   Writes the validation header in a bank.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] bank      bank to be validated
- * @param[in] cnt       value for the flash usage counter
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW
- *                      failures.
- *
- * @notapi
- */
-static mfs_error_t mfs_bank_set_header(MFSDriver *devp,
-                                       mfs_bank_t bank,
-                                       uint32_t cnt) {
-  flash_sector_t sector;
-  mfs_bank_header_t header;
-
-  if (bank == MFS_BANK_0) {
-    sector = devp->config->bank0_start;
-  }
-  else {
-    sector = devp->config->bank1_start;
-  }
-
-  header.magic1  = MFS_BANK_MAGIC_1;
-  header.magic1  = MFS_BANK_MAGIC_1;
-  header.counter = cnt;
-  header.next    = sizeof (mfs_bank_header_t);
-  header.crc     = crc16(0xFFFFU,
-                         (const uint8_t *)&header,
-                         sizeof (flash_sector_t) - sizeof (uint16_t));
-
-  return mfs_flash_write(devp,
-                         flashGetSectorOffset(devp->config->flashp, sector),
-                         sizeof (mfs_bank_header_t),
-                         (const uint8_t *)&header);
-}
-
-/**
- * @brief   Copies all records from a bank to another.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] sbank     source bank
- * @param[in] dbank     destination bank
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW
- *                      failures.
- *
- * @notapi
- */
-static mfs_error_t mfs_copy_bank(MFSDriver *devp,
-                                 mfs_bank_t sbank,
-                                 mfs_bank_t dbank) {
-
-  (void)devp;
-  (void)sbank;
-  (void)dbank;
-
-  return MFS_NO_ERROR;
-}
-
-/**
- * @brief   Selects a bank as current.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] bank      bank to be erased
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- *
- * @notapi
- */
-static mfs_error_t mfs_mount(MFSDriver *devp, mfs_bank_t bank) {
-
-  (void)devp;
-  (void)bank;
-
-  return MFS_NO_ERROR;
-}
-
-/**
- * @brief   Determines the state of a flash bank.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] bank          the bank identifier
- * @param[out] cntp         bank counter value, only valid if the bank is not
- *                          in the @p MFS_BANK_GARBAGE or @p MFS_BANK_ERASED
- *                          states.
- *
- * @return                  The bank state.
- * @retval MFS_BANK_ERASED  if the bank is fully erased.
- * @retval MFS_BANK_OK      if the bank contains valid data.
- * @retval MFS_BANK_PARTIAL if the bank contains errors but the data is still
- *                          readable.
- * @retval MFS_BANK_GARBAGE if the bank contains unreadable garbage.
- */
-static mfs_bank_state_t mfs_get_bank_state(MFSDriver *devp,
-                                           mfs_bank_t bank,
-                                           uint32_t *cntp) {
-
-  (void)devp;
-  (void)bank;
-  (void)cntp;
-
-  return MFS_BANK_OK;
-}
-
-/**
- * @brief   Performs a flash partition mount attempt.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_REPAIR_WARNING if the operation has been completed but a
- *                      repair has been performed.
- * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW
- *                      failures.
- *
- * @api
- */
-static mfs_error_t mfs_try_mount(MFSDriver *devp) {
-  mfs_bank_state_t sts0, sts1;
-  uint32_t cnt0 = 0, cnt1 = 0;
-
-  /* Assessing the state of the two banks.*/
-  sts0 = mfs_get_bank_state(devp, MFS_BANK_0, &cnt0);
-  sts1 = mfs_get_bank_state(devp, MFS_BANK_1, &cnt1);
-
-  /* Handling all possible scenarios, each one requires its own recovery
-     strategy.*/
-  switch (PAIR(sts0, sts1)) {
-
-  case PAIR(MFS_BANK_ERASED, MFS_BANK_ERASED):
-    /* Both banks erased, first initialization.*/
-    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, 1));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    return MFS_NO_ERROR;
-
-  case PAIR(MFS_BANK_ERASED, MFS_BANK_OK):
-    /* Normal situation, bank one is used.*/
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_1));
-    return MFS_NO_ERROR;
-
-  case PAIR(MFS_BANK_ERASED, MFS_BANK_PARTIAL):
-    /* Bank zero is erased, bank one has problems.*/
-    RET_ON_ERROR(mfs_copy_bank(devp, MFS_BANK_1, MFS_BANK_0));
-    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, cnt1 + 1));
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_ERASED, MFS_BANK_GARBAGE):
-    /* Bank zero is erased, bank one is not readable.*/
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, 1));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_OK, MFS_BANK_ERASED):
-    /* Normal situation, bank zero is used.*/
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    return MFS_NO_ERROR;
-
-  case PAIR(MFS_BANK_OK, MFS_BANK_OK):
-    /* Both banks appear to be valid but one must be newer, erasing the
-       older one.*/
-    if (cnt0 > cnt1) {
-      /* Bank 0 is newer.*/
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-      RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    }
-    else {
-      /* Bank 1 is newer.*/
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-      RET_ON_ERROR(mfs_mount(devp, MFS_BANK_1));
-    }
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_OK, MFS_BANK_PARTIAL):
-    /* Bank zero is normal, bank one has problems.*/
-    if (cnt0 > cnt1) {
-      /* Normal bank zero is more recent than the partial bank one, the
-         partial bank needs to be erased.*/
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-      RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    }
-    else {
-      /* Partial bank one is more recent than the normal bank zero.*/
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-      RET_ON_ERROR(mfs_copy_bank(devp, MFS_BANK_1, MFS_BANK_0));
-      RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, cnt1 + 1));
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-      RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    }
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_OK, MFS_BANK_GARBAGE):
-    /* Bank zero is normal, bank one is unreadable.*/
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_PARTIAL, MFS_BANK_ERASED):
-    /* Bank zero has problems, bank one is erased.*/
-    RET_ON_ERROR(mfs_copy_bank(devp, MFS_BANK_0, MFS_BANK_1));
-    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_1, cnt0 + 1));
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_1));
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_PARTIAL, MFS_BANK_OK):
-    /* Bank zero has problems, bank one is normal.*/
-    if (cnt1 > cnt0) {
-      /* Normal bank one is more recent than the partial bank zero, the
-         partial bank has to be erased.*/
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-      RET_ON_ERROR(mfs_mount(devp, MFS_BANK_1));
-    }
-    else {
-      /* Partial bank zero is more recent than the normal bank one.*/
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-      RET_ON_ERROR(mfs_copy_bank(devp, MFS_BANK_0, MFS_BANK_1));
-      RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_1, cnt0 + 1));
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-      RET_ON_ERROR(mfs_mount(devp, MFS_BANK_1));
-    }
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_PARTIAL, MFS_BANK_PARTIAL):
-    /* Both banks have problems.*/
-    if (cnt0 > cnt1) {
-      /* Bank zero is newer, copying in bank one and using it.*/
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-      RET_ON_ERROR(mfs_copy_bank(devp, MFS_BANK_0, MFS_BANK_1));
-      RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_1, cnt0 + 1));
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-      RET_ON_ERROR(mfs_mount(devp, MFS_BANK_1));
-    }
-    else {
-      /* Bank one is newer, copying in bank zero and using it.*/
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-      RET_ON_ERROR(mfs_copy_bank(devp, MFS_BANK_1, MFS_BANK_0));
-      RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, cnt1 + 1));
-      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-      RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    }
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_PARTIAL, MFS_BANK_GARBAGE):
-    /* Bank zero has problems, bank one is unreadable.*/
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-    RET_ON_ERROR(mfs_copy_bank(devp, MFS_BANK_0, MFS_BANK_1));
-    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_1, cnt0 + 1));
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_1));
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_GARBAGE, MFS_BANK_ERASED):
-    /* Bank zero is unreadable, bank one is erased.*/
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, 1));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_GARBAGE, MFS_BANK_OK):
-    /* Bank zero is unreadable, bank one is normal.*/
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_1));
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_GARBAGE, MFS_BANK_PARTIAL):
-    /* Bank zero is unreadable, bank one has problems.*/
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-    RET_ON_ERROR(mfs_copy_bank(devp, MFS_BANK_1, MFS_BANK_0));
-    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, cnt0 + 1));
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    return MFS_REPAIR_WARNING;
-
-  case PAIR(MFS_BANK_GARBAGE, MFS_BANK_GARBAGE):
-    /* Both banks are unreadable, reinitializing.*/
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));
-    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));
-    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, 1));
-    RET_ON_ERROR(mfs_mount(devp, MFS_BANK_0));
-    return MFS_REPAIR_WARNING;
-
-  default:
-    osalSysHalt("internal error");
-  }
-
-  /* Never reached.*/
-  return MFS_INTERNAL_ERROR;
-}
-
-/*===========================================================================*/
-/* Driver exported functions.                                                */
-/*===========================================================================*/
-
-/**
- * @brief   Initializes an instance.
- *
- * @param[out] devp     pointer to the @p MFSDriver object
- *
- * @init
- */
-void mfsObjectInit(MFSDriver *devp) {
-
-  osalDbgCheck(devp != NULL);
-
-  devp->state = MFS_STOP;
-  devp->config = NULL;
-}
-
-/**
- * @brief   Configures and activates a MFS driver.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] config    pointer to the configuration
- *
- * @api
- */
-void mfsStart(MFSDriver *devp, const MFSConfig *config) {
-
-  osalDbgCheck((devp != NULL) && (config != NULL));
-  osalDbgAssert(devp->state != MFS_UNINIT, "invalid state");
-
-  if (devp->state == MFS_STOP) {
-
-    devp->config = config;
-    devp->state  = MFS_READY;
-  }
-} 
-
-/**
- * @brief   Deactivates a MFS driver.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- *
- * @api
- */
-void mfsStop(MFSDriver *devp) {
-
-  osalDbgCheck(devp != NULL);
-  osalDbgAssert(devp->state != MFS_UNINIT, "invalid state");
-
-  if (devp->state != MFS_STOP) {
-
-    devp->state = MFS_STOP;
-  }
-}
-
-/**
- * @brief   Mounts a managed flash storage.
- * @details This functions checks the storage internal state and eventually
- *          performs the required initialization or repair operations.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_REPAIR_WARNING if the operation has been completed but a
- *                      repair has been performed.
- * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW
- *                      failures.
- *
- * @api
- */
-mfs_error_t mfsMount(MFSDriver *devp) {
-  unsigned i;
-
-  /* Attempting to mount the managed partition.*/
-  for (i = 0; i < MFS_CFG_MAX_REPAIR_ATTEMPTS; i++) {
-    mfs_error_t err;
-
-    err = mfs_try_mount(devp);
-    if (!MFS_IS_ERROR(err))
-      return err;
-  }
-
-  return MFS_FLASH_FAILURE;
-}
-
-/**
- * @brief   Unmounts a manage flash storage.
- */
-mfs_error_t mfsUnmount(MFSDriver *devp) {
-
-  (void)devp;
-
-  return MFS_NO_ERROR;
-}
-
-/**
- * @brief   Retrieves and reads a data record.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] id        record numeric identifier
- * @param[in,out] np    on input is the maximum buffer size, on return it is
- *                      the size of the data copied into the buffer
- * @param[in] buffer    pointer to a buffer for record data
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_ID_NOT_FOUND if the specified id does not exists.
- * @retval MFS_CRC_ERROR if retrieved data has a CRC error.
- *
- * @api
- */
-mfs_error_t mfsReadRecord(MFSDriver *devp, uint32_t id,
-                          uint32_t *np, uint8_t *buffer) {
-
-  (void)devp;
-  (void)id;
-  (void)np;
-  (void)buffer;
-
-  return MFS_NO_ERROR;
-}
-
-/**
- * @brief   Creates or updates a data record.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] id        record numeric identifier
- * @param[in] n         size of data to be written, it cannot be zero
- * @param[in] buffer    pointer to a buffer for record data
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW
- *                      failures.
- *
- * @api
- */
-mfs_error_t mfsUpdateRecord(MFSDriver *devp, uint32_t id,
-                            uint32_t n, const uint8_t *buffer) {
-
-  (void)devp;
-  (void)id;
-  (void)n;
-  (void)buffer;
-
-  return MFS_NO_ERROR;
-}
-
-/**
- * @brief   Erases a data record.
- *
- * @param[in] devp      pointer to the @p MFSDriver object
- * @param[in] id        record numeric identifier
- * @return              The operation status.
- * @retval MFS_NO_ERROR if the operation has been successfully completed.
- * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW
- *                      failures.
- *
- * @api
- */
-mfs_error_t mfsEraseRecord(MFSDriver *devp, uint32_t id) {
-
-  (void)devp;
-  (void)id;
-
-  return MFS_NO_ERROR;
-}
-
-/** @} */
+/*

+    Managed Flash Storage - Copyright (C) 2016 Giovanni Di Sirio

+

+    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    mfs.c

+ * @brief   Managed Flash Storage module code.

+ * @details This module manages a flash partition as a generic storage where

+ *          arbitrary data records can be created, updated, deleted and

+ *          retrieved.<br>

+ *          A managed partition is composed of two banks of equal size, a

+ *          bank is composed of one or more erasable sectors, a sector is

+ *          divided in writable pages.<br>

+ *          The module handles flash wear leveling and recovery of damaged

+ *          banks (where possible) caused by power loss during operations.

+ *          Both operations are transparent to the user.

+ *

+ * @addtogroup mfs

+ * @{

+ */

+

+#include "hal.h"

+

+#include "mfs.h"

+

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

+/* Driver local definitions.                                                 */

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

+

+#define PAIR(a, b) (((unsigned)(a) << 2U) | (unsigned)(b))

+

+/**

+ * @brief   Error check helper.

+ */

+#define RET_ON_ERROR(err) do {                                              \

+  mfs_error_t e = (err);                                                    \

+  if (e != MFS_NO_ERROR) {                                                  \

+    return e;                                                               \

+  }                                                                         \

+} while (false)

+

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

+/* Driver exported variables.                                                */

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

+

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

+/* Driver local variables and types.                                         */

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

+

+const uint16_t crc16_table[256] = {

+  0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,

+  0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,

+  0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,

+  0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,

+  0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,

+  0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,

+  0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,

+  0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,

+  0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,

+  0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,

+  0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,

+  0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,

+  0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,

+  0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,

+  0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,

+  0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,

+  0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,

+  0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,

+  0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,

+  0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,

+  0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,

+  0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,

+  0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,

+  0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,

+  0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,

+  0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,

+  0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,

+  0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,

+  0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,

+  0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,

+  0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,

+  0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0

+};

+

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

+/* Driver local functions.                                                   */

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

+

+uint16_t crc16(uint16_t crc, const uint8_t *data, size_t n) {

+

+  while (n > 0U) {

+    crc = (crc << 8U) ^ crc16_table[(crc >> 8U) ^ (uint16_t)*data];

+    data++;

+    n--;

+  }

+

+  return crc;

+}

+

+#if (MFS_CFG_ID_CACHE_SIZE > 0) || defined(__DOXYGEN__)

+void mfs_cache_init(MFSDriver *devp) {

+

+  (void)devp;

+}

+

+mfs_cached_id_t *mfs_cache_find_id(MFSDriver *devp, uint32_t id) {

+

+  (void)devp;

+  (void)id;

+

+  return NULL;

+}

+

+void mfs_cache_update_id(MFSDriver *devp, uint32_t id) {

+

+  (void)devp;

+  (void)id;

+}

+

+void mfs_cache_erase_id(MFSDriver *devp, uint32_t id) {

+

+  (void)devp;

+  (void)id;

+}

+#endif /* MFS_CFG_ID_CACHE_SIZE > 0 */

+

+/**

+ * @brief   Flash write.

+ * @note    If the option @p MFS_CFG_WRITE_VERIFY is enabled then the flash

+ *          is also read back for verification.

+ *

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

+ * @param[in] offset    flash offset

+ * @param[in] n         number of bytes to be read

+ * @param[out] rp       pointer to the data buffer

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW

+ *                      failures.

+ *

+ * @notapi

+ */

+static mfs_error_t mfs_flash_write(MFSDriver *devp,

+                                   flash_offset_t offset,

+                                   size_t n, const

+                                   uint8_t *p) {

+  flash_error_t ferr;

+

+  ferr = flashProgram(devp->config->flashp, offset, n, p);

+  if (ferr != FLASH_NO_ERROR) {

+    return MFS_FLASH_FAILURE;

+  }

+

+  return MFS_NO_ERROR;

+}

+

+/**

+ * @brief   Erases and verifies all sectors belonging to a bank.

+ *

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

+ * @param[in] bank      bank to be erased

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW

+ *                      failures.

+ *

+ * @notapi

+ */

+static mfs_error_t mfs_bank_erase(MFSDriver *devp, mfs_bank_t bank) {

+  flash_sector_t sector, end;

+

+  if (bank == MFS_BANK_0) {

+    sector = devp->config->bank0_start;

+    end    = devp->config->bank0_start + devp->config->bank0_sectors;

+  }

+  else {

+    sector = devp->config->bank1_start;

+    end    = devp->config->bank1_start + devp->config->bank1_sectors;

+  }

+

+  while (sector < end) {

+    flash_error_t ferr;

+

+    ferr = flashStartEraseSector(devp->config->flashp, sector);

+    if (ferr != FLASH_NO_ERROR) {

+      return MFS_FLASH_FAILURE;

+    }

+    ferr = flashWaitErase(devp->config->flashp);

+    if (ferr != FLASH_NO_ERROR) {

+      return MFS_FLASH_FAILURE;

+    }

+    ferr = flashVerifyErase(devp->config->flashp, sector);

+    if (ferr != FLASH_NO_ERROR) {

+      return MFS_FLASH_FAILURE;

+    }

+

+    sector++;

+  }

+

+  return MFS_NO_ERROR;

+}

+

+/**

+ * @brief   Writes the validation header in a bank.

+ *

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

+ * @param[in] bank      bank to be validated

+ * @param[in] cnt       value for the flash usage counter

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW

+ *                      failures.

+ *

+ * @notapi

+ */

+static mfs_error_t mfs_bank_set_header(MFSDriver *devp,

+                                       mfs_bank_t bank,

+                                       uint32_t cnt) {

+  flash_sector_t sector;

+  mfs_bank_header_t header;

+

+  if (bank == MFS_BANK_0) {

+    sector = devp->config->bank0_start;

+  }

+  else {

+    sector = devp->config->bank1_start;

+  }

+

+  header.magic1  = MFS_BANK_MAGIC_1;

+  header.magic1  = MFS_BANK_MAGIC_1;

+  header.counter = cnt;

+  header.next    = sizeof (mfs_bank_header_t);

+  header.crc     = crc16(0xFFFFU,

+                         (const uint8_t *)&header,

+                         sizeof (flash_sector_t) - sizeof (uint16_t));

+

+  return mfs_flash_write(devp,

+                         flashGetSectorOffset(devp->config->flashp, sector),

+                         sizeof (mfs_bank_header_t),

+                         (const uint8_t *)&header);

+}

+

+/**

+ * @brief   Copies all records from a bank to another.

+ *

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

+ * @param[in] sbank     source bank

+ * @param[in] dbank     destination bank

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW

+ *                      failures.

+ *

+ * @notapi

+ */

+static mfs_error_t mfs_bank_copy(MFSDriver *devp,

+                                 mfs_bank_t sbank,

+                                 mfs_bank_t dbank) {

+

+  (void)devp;

+  (void)sbank;

+  (void)dbank;

+

+  return MFS_NO_ERROR;

+}

+

+/**

+ * @brief   Selects a bank as current.

+ *

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

+ * @param[in] bank      bank to be erased

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ *

+ * @notapi

+ */

+static mfs_error_t mfs_bank_mount(MFSDriver *devp, mfs_bank_t bank) {

+

+  (void)devp;

+  (void)bank;

+

+  return MFS_NO_ERROR;

+}

+

+/**

+ * @brief   Determines the state of a flash bank.

+ *

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

+ * @param[in] bank          the bank identifier

+ * @param[out] cntp         bank counter value, only valid if the bank is not

+ *                          in the @p MFS_BANK_GARBAGE or @p MFS_BANK_ERASED

+ *                          states.

+ *

+ * @return                  The bank state.

+ * @retval MFS_BANK_ERASED  if the bank is fully erased.

+ * @retval MFS_BANK_OK      if the bank contains valid data.

+ * @retval MFS_BANK_PARTIAL if the bank contains errors but the data is still

+ *                          readable.

+ * @retval MFS_BANK_GARBAGE if the bank contains unreadable garbage.

+ */

+static mfs_bank_state_t mfs_get_bank_state(MFSDriver *devp,

+                                           mfs_bank_t bank,

+                                           uint32_t *cntp) {

+

+  (void)devp;

+  (void)bank;

+  (void)cntp;

+

+  return MFS_BANK_OK;

+}

+

+/**

+ * @brief   Performs a flash partition mount attempt.

+ *

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

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_REPAIR_WARNING if the operation has been completed but a

+ *                      repair has been performed.

+ * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW

+ *                      failures.

+ *

+ * @api

+ */

+static mfs_error_t mfs_try_mount(MFSDriver *devp) {

+  mfs_bank_state_t sts0, sts1;

+  uint32_t cnt0 = 0, cnt1 = 0;

+

+  /* Assessing the state of the two banks.*/

+  sts0 = mfs_get_bank_state(devp, MFS_BANK_0, &cnt0);

+  sts1 = mfs_get_bank_state(devp, MFS_BANK_1, &cnt1);

+

+  /* Handling all possible scenarios, each one requires its own recovery

+     strategy.*/

+  switch (PAIR(sts0, sts1)) {

+

+  case PAIR(MFS_BANK_ERASED, MFS_BANK_ERASED):

+    /* Both banks erased, first initialization.*/

+    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, 1));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    return MFS_NO_ERROR;

+

+  case PAIR(MFS_BANK_ERASED, MFS_BANK_OK):

+    /* Normal situation, bank one is used.*/

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_1));

+    return MFS_NO_ERROR;

+

+  case PAIR(MFS_BANK_ERASED, MFS_BANK_PARTIAL):

+    /* Bank zero is erased, bank one has problems.*/

+    RET_ON_ERROR(mfs_bank_copy(devp, MFS_BANK_1, MFS_BANK_0));

+    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, cnt1 + 1));

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_ERASED, MFS_BANK_GARBAGE):

+    /* Bank zero is erased, bank one is not readable.*/

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, 1));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_OK, MFS_BANK_ERASED):

+    /* Normal situation, bank zero is used.*/

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    return MFS_NO_ERROR;

+

+  case PAIR(MFS_BANK_OK, MFS_BANK_OK):

+    /* Both banks appear to be valid but one must be newer, erasing the

+       older one.*/

+    if (cnt0 > cnt1) {

+      /* Bank 0 is newer.*/

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+      RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    }

+    else {

+      /* Bank 1 is newer.*/

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+      RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_1));

+    }

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_OK, MFS_BANK_PARTIAL):

+    /* Bank zero is normal, bank one has problems.*/

+    if (cnt0 > cnt1) {

+      /* Normal bank zero is more recent than the partial bank one, the

+         partial bank needs to be erased.*/

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+      RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    }

+    else {

+      /* Partial bank one is more recent than the normal bank zero.*/

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+      RET_ON_ERROR(mfs_bank_copy(devp, MFS_BANK_1, MFS_BANK_0));

+      RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, cnt1 + 1));

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+      RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    }

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_OK, MFS_BANK_GARBAGE):

+    /* Bank zero is normal, bank one is unreadable.*/

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_PARTIAL, MFS_BANK_ERASED):

+    /* Bank zero has problems, bank one is erased.*/

+    RET_ON_ERROR(mfs_bank_copy(devp, MFS_BANK_0, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_1, cnt0 + 1));

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_1));

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_PARTIAL, MFS_BANK_OK):

+    /* Bank zero has problems, bank one is normal.*/

+    if (cnt1 > cnt0) {

+      /* Normal bank one is more recent than the partial bank zero, the

+         partial bank has to be erased.*/

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+      RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_1));

+    }

+    else {

+      /* Partial bank zero is more recent than the normal bank one.*/

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+      RET_ON_ERROR(mfs_bank_copy(devp, MFS_BANK_0, MFS_BANK_1));

+      RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_1, cnt0 + 1));

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+      RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_1));

+    }

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_PARTIAL, MFS_BANK_PARTIAL):

+    /* Both banks have problems.*/

+    if (cnt0 > cnt1) {

+      /* Bank zero is newer, copying in bank one and using it.*/

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+      RET_ON_ERROR(mfs_bank_copy(devp, MFS_BANK_0, MFS_BANK_1));

+      RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_1, cnt0 + 1));

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+      RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_1));

+    }

+    else {

+      /* Bank one is newer, copying in bank zero and using it.*/

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+      RET_ON_ERROR(mfs_bank_copy(devp, MFS_BANK_1, MFS_BANK_0));

+      RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, cnt1 + 1));

+      RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+      RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    }

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_PARTIAL, MFS_BANK_GARBAGE):

+    /* Bank zero has problems, bank one is unreadable.*/

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_copy(devp, MFS_BANK_0, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_1, cnt0 + 1));

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_1));

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_GARBAGE, MFS_BANK_ERASED):

+    /* Bank zero is unreadable, bank one is erased.*/

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, 1));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_GARBAGE, MFS_BANK_OK):

+    /* Bank zero is unreadable, bank one is normal.*/

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_1));

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_GARBAGE, MFS_BANK_PARTIAL):

+    /* Bank zero is unreadable, bank one has problems.*/

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+    RET_ON_ERROR(mfs_bank_copy(devp, MFS_BANK_1, MFS_BANK_0));

+    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, cnt0 + 1));

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    return MFS_REPAIR_WARNING;

+

+  case PAIR(MFS_BANK_GARBAGE, MFS_BANK_GARBAGE):

+    /* Both banks are unreadable, reinitializing.*/

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_0));

+    RET_ON_ERROR(mfs_bank_erase(devp, MFS_BANK_1));

+    RET_ON_ERROR(mfs_bank_set_header(devp, MFS_BANK_0, 1));

+    RET_ON_ERROR(mfs_bank_mount(devp, MFS_BANK_0));

+    return MFS_REPAIR_WARNING;

+

+  default:

+    osalSysHalt("internal error");

+  }

+

+  /* Never reached.*/

+  return MFS_INTERNAL_ERROR;

+}

+

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

+/* Driver exported functions.                                                */

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

+

+/**

+ * @brief   Initializes an instance.

+ *

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

+ *

+ * @init

+ */

+void mfsObjectInit(MFSDriver *devp) {

+

+  osalDbgCheck(devp != NULL);

+

+  devp->state = MFS_STOP;

+  devp->config = NULL;

+}

+

+/**

+ * @brief   Configures and activates a MFS driver.

+ *

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

+ * @param[in] config    pointer to the configuration

+ *

+ * @api

+ */

+void mfsStart(MFSDriver *devp, const MFSConfig *config) {

+

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

+  osalDbgAssert(devp->state != MFS_UNINIT, "invalid state");

+

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

+

+    devp->config = config;

+    devp->state  = MFS_READY;

+  }

+} 

+

+/**

+ * @brief   Deactivates a MFS driver.

+ *

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

+ *

+ * @api

+ */

+void mfsStop(MFSDriver *devp) {

+

+  osalDbgCheck(devp != NULL);

+  osalDbgAssert(devp->state != MFS_UNINIT, "invalid state");

+

+  if (devp->state != MFS_STOP) {

+

+    devp->state = MFS_STOP;

+  }

+}

+

+/**

+ * @brief   Mounts a managed flash storage.

+ * @details This functions checks the storage internal state and eventually

+ *          performs the required initialization or repair operations.

+ *

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

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_REPAIR_WARNING if the operation has been completed but a

+ *                      repair has been performed.

+ * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW

+ *                      failures.

+ *

+ * @api

+ */

+mfs_error_t mfsMount(MFSDriver *devp) {

+  unsigned i;

+

+  /* Attempting to mount the managed partition.*/

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

+    mfs_error_t err;

+

+    err = mfs_try_mount(devp);

+    if (!MFS_IS_ERROR(err))

+      return err;

+  }

+

+  return MFS_FLASH_FAILURE;

+}

+

+/**

+ * @brief   Unmounts a manage flash storage.

+ */

+mfs_error_t mfsUnmount(MFSDriver *devp) {

+

+  (void)devp;

+

+  return MFS_NO_ERROR;

+}

+

+/**

+ * @brief   Retrieves and reads a data record.

+ *

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

+ * @param[in] id        record numeric identifier

+ * @param[in,out] np    on input is the maximum buffer size, on return it is

+ *                      the size of the data copied into the buffer

+ * @param[in] buffer    pointer to a buffer for record data

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_ID_NOT_FOUND if the specified id does not exists.

+ * @retval MFS_CRC_ERROR if retrieved data has a CRC error.

+ *

+ * @api

+ */

+mfs_error_t mfsReadRecord(MFSDriver *devp, uint32_t id,

+                          uint32_t *np, uint8_t *buffer) {

+

+  (void)devp;

+  (void)id;

+  (void)np;

+  (void)buffer;

+

+  return MFS_NO_ERROR;

+}

+

+/**

+ * @brief   Creates or updates a data record.

+ *

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

+ * @param[in] id        record numeric identifier

+ * @param[in] n         size of data to be written, it cannot be zero

+ * @param[in] buffer    pointer to a buffer for record data

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW

+ *                      failures.

+ *

+ * @api

+ */

+mfs_error_t mfsUpdateRecord(MFSDriver *devp, uint32_t id,

+                            uint32_t n, const uint8_t *buffer) {

+

+  (void)devp;

+  (void)id;

+  (void)n;

+  (void)buffer;

+

+  return MFS_NO_ERROR;

+}

+

+/**

+ * @brief   Erases a data record.

+ *

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

+ * @param[in] id        record numeric identifier

+ * @return              The operation status.

+ * @retval MFS_NO_ERROR if the operation has been successfully completed.

+ * @retval MFS_FLASH_FAILURE if the flash memory is unusable because HW

+ *                      failures.

+ *

+ * @api

+ */

+mfs_error_t mfsEraseRecord(MFSDriver *devp, uint32_t id) {

+

+  (void)devp;

+  (void)id;

+

+  return MFS_NO_ERROR;

+}

+

+/** @} */

diff --git a/os/ex/subsystems/mfs/mfs.h b/os/ex/subsystems/mfs/mfs.h
index ec5719738..89d6b9958 100644
--- a/os/ex/subsystems/mfs/mfs.h
+++ b/os/ex/subsystems/mfs/mfs.h
@@ -1,339 +1,339 @@
-/*
-    Managed Flash Storage - Copyright (C) 2016 Giovanni Di Sirio
-
-    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    mfs.h
- * @brief   Managed Flash Storage module header.
- *
- * @{
- */
-
-#ifndef MFS_H
-#define MFS_H
-
-#include "hal_flash.h"
-
-/*===========================================================================*/
-/* Driver constants.                                                         */
-/*===========================================================================*/
-
-#define MFS_BANK_MAGIC_1                    0xEC705ADEU
-#define MFS_BANK_MAGIC_2                    0xF0339CC5U
-#define MFS_HEADER_MAGIC                    0x5FAEU
-
-/*===========================================================================*/
-/* Driver pre-compile time settings.                                         */
-/*===========================================================================*/
-
-/**
- * @name    Configuration options
- * @{
- */
-/**
- * @brief   Record identifiers cache size.
- * @details Cache trades RAM for a faster access to stored records. If zero
- *          then the cache is disabled.
- */
-#if !defined(MFS_CFG_ID_CACHE_SIZE) || defined(__DOXIGEN__)
-#define MFS_CFG_ID_CACHE_SIZE               16
-#endif
-
-/**
- * @brief   Maximum number of repair attempts on partition mount.
- */
-#if !defined(MFS_CFG_MAX_REPAIR_ATTEMPTS) || defined(__DOXIGEN__)
-#define MFS_CFG_MAX_REPAIR_ATTEMPTS         3
-#endif
-
-/**
- * @brief   Verify written data.
- */
-#if !defined(MFS_CFG_WRITE_VERIFY) || defined(__DOXIGEN__)
-#define MFS_CFG_WRITE_VERIFY                TRUE
-#endif
-/** @} */
-
-/*===========================================================================*/
-/* Derived constants and error checks.                                       */
-/*===========================================================================*/
-
-#if MFS_CFG_ID_CACHE_SIZE < 0
-#error "invalid MFS_CFG_ID_CACHE_SIZE value"
-#endif
-
-#if (MFS_CFG_MAX_REPAIR_ATTEMPTS < 1) || (MFS_CFG_MAX_REPAIR_ATTEMPTS > 10)
-#error "invalid MFS_MAX_REPAIR_ATTEMPTS value"
-#endif
-
-/*===========================================================================*/
-/* Driver data structures and types.                                         */
-/*===========================================================================*/
-
-/**
- * @brief   Type of a flash bank.
- */
-typedef enum {
-  MFS_BANK_0 = 0,
-  MFS_BANK_1 = 1
-} mfs_bank_t;
-
-/**
- * @brief   Type of driver state machine states.
- */
-typedef enum {
-  MFS_UNINIT = 0,
-  MFS_STOP = 1,
-  MFS_READY = 2,
-  MFS_MOUNTED = 3,
-  MFS_ACTIVE = 4
-} mfs_state_t;
-
-/**
- * @brief   Type of an MFS error code.
- * @note    Errors are negative integers, informative warnings are positive
- *          integers.
- */
-typedef enum {
-  MFS_NO_ERROR = 0,
-  MFS_REPAIR_WARNING = 1,
-  MFS_GC_WARNING = 2,
-  MFS_ID_NOT_FOUND = -1,
-  MFS_CRC_ERROR = -2,
-  MFS_FLASH_FAILURE = -3,
-  MFS_INTERNAL_ERROR = -4
-} mfs_error_t;
-
-/**
- * @brief   Type of a bank state assessment.
- */
-typedef enum {
-  MFS_BANK_ERASED = 0,
-  MFS_BANK_OK = 1,
-  MFS_BANK_PARTIAL = 2,
-  MFS_BANK_GARBAGE = 3
-} mfs_bank_state_t;
-
-/**
- * @brief   Type of a bank header.
- * @note    The header resides in the first 16 bytes of a bank extending
- *          to the next page boundary.
- */
-typedef struct {
-  /**
-   * @brief   Bank magic 1.
-   */
-  uint32_t                  magic1;
-  /**
-   * @brief   Bank magic 2.
-   */
-  uint32_t                  magic2;
-  /**
-   * @brief   Usage counter of the bank.
-   * @details This value is increased each time a bank swap is performed. It
-   *          indicates how much wearing the flash has already endured.
-   */
-  uint32_t                  counter;
-  /**
-   * @brief   First data element.
-   */
-  flash_offset_t            next;
-  /**
-   * @brief   Header CRC.
-   */
-  uint16_t                  crc;
-} mfs_bank_header_t;
-
-/**
- * @brief   Type of a data block header.
- * @details This structure is placed before each written data block.
- */
-typedef struct {
-  /**
-   * @brief   Data header magic.
-   */
-  uint16_t                  magic;
-  /**
-   * @brief   Data CRC.
-   */
-  uint16_t                  crc;
-  /**
-   * @brief   Data identifier.
-   */
-  uint32_t                  id;
-  /**
-   * @brief   Data size for forward scan.
-   */
-  uint32_t                  size;
-  /**
-   * @brief   Address of the previous header or zero if none.
-   */
-  flash_offset_t            prev_header;
-} mfs_data_header_t;
-
-#if (MFS_CFG_ID_CACHE_SIZE > 0) || defined(__DOXYGEN__)
-/**
- * @brief   Type of an element of the record identifiers cache.
- */
-typedef struct mfs_cached_id {
-  /**
-   * @brief   Pointer to the next element in the list.
-   */
-  struct mfs_cached_id      *lru_next;
-  /**
-   * @brief   Pointer to the previous element in the list.
-   */
-  struct mfs_cached_id      *lru_prev;
-  /**
-   * @brief   Identifier of the cached element.
-   */
-  uint32_t                  id;
-  /**
-   * @brief   Data address of the cached element.
-   */
-  flash_offset_t            offset;
-  /**
-   * @brief   Data size of the cached element.
-   */
-  uint32_t                  size;
-} mfs_cached_id_t;
-
-/**
- * @brief   Type of an element of the record identifiers cache.
- */
-typedef struct mfs_cache_header {
-  /**
-   * @brief   Pointer to the first element in the list.
-   */
-  struct mfs_cached_id      *lru_next;
-  /**
-   * @brief   Pointer to the last element in the list.
-   */
-  struct mfs_cached_id      *lru_prev;
-} mfs_cache_header_t;
-#endif /* MFS_CFG_ID_CACHE_SIZE > 0 */
-
-/**
- * @brief   Type of a MFS configuration structure.
- */
-typedef struct {
-  /**
-   * @brief   Flash driver associated to this MFS instance.
-   */
-  BaseFlash                 *flashp;
-  /**
-   * @brief   Base sector index for bank 0.
-   */
-  flash_sector_t            bank0_start;
-  /**
-   * #brief   Number of sectors for bank 0.
-   */
-  flash_sector_t            bank0_sectors;
-  /**
-   * @brief   Base sector index for bank 1.
-   */
-  flash_sector_t            bank1_start;
-  /**
-   * #brief   Number of sectors for bank 1.
-   */
-  flash_sector_t            bank1_sectors;
-} MFSConfig;
-
-/**
- * @extends BaseFlash
- *
- * @brief   Type of an MFS instance.
- */
-typedef struct {
-  /**
-   * @brief   Driver state.
-   */
-  mfs_state_t               state;
-  /**
-   * @brief   Current configuration data.
-   */
-  const MFSConfig           *config;
-  /**
-   * @brief   Bank currently in use.
-   */
-  mfs_bank_t                current_bank;
-  /**
-   * @brief   Size in bytes of banks.
-   */
-  uint32_t                  banks_size;
-  /**
-   * @brief   Pointer to the next free position in the current bank.
-   */
-  flash_offset_t            next_offset;
-  /**
-   * @brief   Pointer to the last header in the list or zero.
-   */
-  flash_offset_t            last_offset;
-  /**
-   * @brief   Used space in the current bank without considering erased records.
-   */
-  uint32_t                  used_space;
-#if (MFS_CFG_ID_CACHE_SIZE > 0) || defined(__DOXYGEN__)
-  /**
-   * @brief   Header of the cache LRU list.
-   */
-  mfs_cache_header_t        cache_header;
-  /**
-   * @brief   Array of the cached identifiers.
-   */
-  mfs_cached_id_t           cache_buffer[MFS_CFG_ID_CACHE_SIZE];
-#endif /* MFS_CFG_ID_CACHE_SIZE > 0 */
-} MFSDriver;
-
-/*===========================================================================*/
-/* Driver macros.                                                            */
-/*===========================================================================*/
-
-/**
- * @name   Error codes handling macros
- * @{
- */
-#define MFS_IS_ERROR(err) ((err) < MFS_NO_ERROR)
-#define MFS_IS_WARNING(err) ((err) > MFS_NO_ERROR)
-/** @} */
-
-/*===========================================================================*/
-/* External declarations.                                                    */
-/*===========================================================================*/
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-  void mfsObjectInit(MFSDriver *devp);
-  void mfsStart(MFSDriver *devp, const MFSConfig *config);
-  void mfsStop(MFSDriver *devp);
-  mfs_error_t mfsMount(MFSDriver *devp);
-  mfs_error_t mfsUnmount(MFSDriver *devp);
-  mfs_error_t mfsReadRecord(MFSDriver *devp, uint32_t id,
-                            uint32_t *np, uint8_t *buffer);
-  mfs_error_t mfsUpdateRecord(MFSDriver *devp, uint32_t id,
-                              uint32_t n, const uint8_t *buffer);
-  mfs_error_t mfsEraseRecord(MFSDriver *devp, uint32_t id);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* MFS_H */
-
-/** @} */
-
+/*

+    Managed Flash Storage - Copyright (C) 2016 Giovanni Di Sirio

+

+    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    mfs.h

+ * @brief   Managed Flash Storage module header.

+ *

+ * @{

+ */

+

+#ifndef MFS_H

+#define MFS_H

+

+#include "hal_flash.h"

+

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

+/* Driver constants.                                                         */

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

+

+#define MFS_BANK_MAGIC_1                    0xEC705ADEU

+#define MFS_BANK_MAGIC_2                    0xF0339CC5U

+#define MFS_HEADER_MAGIC                    0x5FAEU

+

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

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

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

+

+/**

+ * @name    Configuration options

+ * @{

+ */

+/**

+ * @brief   Record identifiers cache size.

+ * @details Cache trades RAM for a faster access to stored records. If zero

+ *          then the cache is disabled.

+ */

+#if !defined(MFS_CFG_ID_CACHE_SIZE) || defined(__DOXIGEN__)

+#define MFS_CFG_ID_CACHE_SIZE               16

+#endif

+

+/**

+ * @brief   Maximum number of repair attempts on partition mount.

+ */

+#if !defined(MFS_CFG_MAX_REPAIR_ATTEMPTS) || defined(__DOXIGEN__)

+#define MFS_CFG_MAX_REPAIR_ATTEMPTS         3

+#endif

+

+/**

+ * @brief   Verify written data.

+ */

+#if !defined(MFS_CFG_WRITE_VERIFY) || defined(__DOXIGEN__)

+#define MFS_CFG_WRITE_VERIFY                TRUE

+#endif

+/** @} */

+

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

+/* Derived constants and error checks.                                       */

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

+

+#if MFS_CFG_ID_CACHE_SIZE < 0

+#error "invalid MFS_CFG_ID_CACHE_SIZE value"

+#endif

+

+#if (MFS_CFG_MAX_REPAIR_ATTEMPTS < 1) || (MFS_CFG_MAX_REPAIR_ATTEMPTS > 10)

+#error "invalid MFS_MAX_REPAIR_ATTEMPTS value"

+#endif

+

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

+/* Driver data structures and types.                                         */

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

+

+/**

+ * @brief   Type of a flash bank.

+ */

+typedef enum {

+  MFS_BANK_0 = 0,

+  MFS_BANK_1 = 1

+} mfs_bank_t;

+

+/**

+ * @brief   Type of driver state machine states.

+ */

+typedef enum {

+  MFS_UNINIT = 0,

+  MFS_STOP = 1,

+  MFS_READY = 2,

+  MFS_MOUNTED = 3,

+  MFS_ACTIVE = 4

+} mfs_state_t;

+

+/**

+ * @brief   Type of an MFS error code.

+ * @note    Errors are negative integers, informative warnings are positive

+ *          integers.

+ */

+typedef enum {

+  MFS_NO_ERROR = 0,

+  MFS_REPAIR_WARNING = 1,

+  MFS_GC_WARNING = 2,

+  MFS_ID_NOT_FOUND = -1,

+  MFS_CRC_ERROR = -2,

+  MFS_FLASH_FAILURE = -3,

+  MFS_INTERNAL_ERROR = -4

+} mfs_error_t;

+

+/**

+ * @brief   Type of a bank state assessment.

+ */

+typedef enum {

+  MFS_BANK_ERASED = 0,

+  MFS_BANK_OK = 1,

+  MFS_BANK_PARTIAL = 2,

+  MFS_BANK_GARBAGE = 3

+} mfs_bank_state_t;

+

+/**

+ * @brief   Type of a bank header.

+ * @note    The header resides in the first 16 bytes of a bank extending

+ *          to the next page boundary.

+ */

+typedef struct {

+  /**

+   * @brief   Bank magic 1.

+   */

+  uint32_t                  magic1;

+  /**

+   * @brief   Bank magic 2.

+   */

+  uint32_t                  magic2;

+  /**

+   * @brief   Usage counter of the bank.

+   * @details This value is increased each time a bank swap is performed. It

+   *          indicates how much wearing the flash has already endured.

+   */

+  uint32_t                  counter;

+  /**

+   * @brief   First data element.

+   */

+  flash_offset_t            next;

+  /**

+   * @brief   Header CRC.

+   */

+  uint16_t                  crc;

+} mfs_bank_header_t;

+

+/**

+ * @brief   Type of a data block header.

+ * @details This structure is placed before each written data block.

+ */

+typedef struct {

+  /**

+   * @brief   Data header magic.

+   */

+  uint16_t                  magic;

+  /**

+   * @brief   Data CRC.

+   */

+  uint16_t                  crc;

+  /**

+   * @brief   Data identifier.

+   */

+  uint32_t                  id;

+  /**

+   * @brief   Data size for forward scan.

+   */

+  uint32_t                  size;

+  /**

+   * @brief   Address of the previous header or zero if none.

+   */

+  flash_offset_t            prev_header;

+} mfs_data_header_t;

+

+#if (MFS_CFG_ID_CACHE_SIZE > 0) || defined(__DOXYGEN__)

+/**

+ * @brief   Type of an element of the record identifiers cache.

+ */

+typedef struct mfs_cached_id {

+  /**

+   * @brief   Pointer to the next element in the list.

+   */

+  struct mfs_cached_id      *lru_next;

+  /**

+   * @brief   Pointer to the previous element in the list.

+   */

+  struct mfs_cached_id      *lru_prev;

+  /**

+   * @brief   Identifier of the cached element.

+   */

+  uint32_t                  id;

+  /**

+   * @brief   Data address of the cached element.

+   */

+  flash_offset_t            offset;

+  /**

+   * @brief   Data size of the cached element.

+   */

+  uint32_t                  size;

+} mfs_cached_id_t;

+

+/**

+ * @brief   Type of an element of the record identifiers cache.

+ */

+typedef struct mfs_cache_header {

+  /**

+   * @brief   Pointer to the first element in the list.

+   */

+  struct mfs_cached_id      *lru_next;

+  /**

+   * @brief   Pointer to the last element in the list.

+   */

+  struct mfs_cached_id      *lru_prev;

+} mfs_cache_header_t;

+#endif /* MFS_CFG_ID_CACHE_SIZE > 0 */

+

+/**

+ * @brief   Type of a MFS configuration structure.

+ */

+typedef struct {

+  /**

+   * @brief   Flash driver associated to this MFS instance.

+   */

+  BaseFlash                 *flashp;

+  /**

+   * @brief   Base sector index for bank 0.

+   */

+  flash_sector_t            bank0_start;

+  /**

+   * #brief   Number of sectors for bank 0.

+   */

+  flash_sector_t            bank0_sectors;

+  /**

+   * @brief   Base sector index for bank 1.

+   */

+  flash_sector_t            bank1_start;

+  /**

+   * #brief   Number of sectors for bank 1.

+   */

+  flash_sector_t            bank1_sectors;

+} MFSConfig;

+

+/**

+ * @extends BaseFlash

+ *

+ * @brief   Type of an MFS instance.

+ */

+typedef struct {

+  /**

+   * @brief   Driver state.

+   */

+  mfs_state_t               state;

+  /**

+   * @brief   Current configuration data.

+   */

+  const MFSConfig           *config;

+  /**

+   * @brief   Bank currently in use.

+   */

+  mfs_bank_t                current_bank;

+  /**

+   * @brief   Size in bytes of banks.

+   */

+  uint32_t                  banks_size;

+  /**

+   * @brief   Pointer to the next free position in the current bank.

+   */

+  flash_offset_t            next_offset;

+  /**

+   * @brief   Pointer to the last header in the list or zero.

+   */

+  flash_offset_t            last_offset;

+  /**

+   * @brief   Used space in the current bank without considering erased records.

+   */

+  uint32_t                  used_space;

+#if (MFS_CFG_ID_CACHE_SIZE > 0) || defined(__DOXYGEN__)

+  /**

+   * @brief   Header of the cache LRU list.

+   */

+  mfs_cache_header_t        cache_header;

+  /**

+   * @brief   Array of the cached identifiers.

+   */

+  mfs_cached_id_t           cache_buffer[MFS_CFG_ID_CACHE_SIZE];

+#endif /* MFS_CFG_ID_CACHE_SIZE > 0 */

+} MFSDriver;

+

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

+/* Driver macros.                                                            */

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

+

+/**

+ * @name   Error codes handling macros

+ * @{

+ */

+#define MFS_IS_ERROR(err) ((err) < MFS_NO_ERROR)

+#define MFS_IS_WARNING(err) ((err) > MFS_NO_ERROR)

+/** @} */

+

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

+/* External declarations.                                                    */

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

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+  void mfsObjectInit(MFSDriver *devp);

+  void mfsStart(MFSDriver *devp, const MFSConfig *config);

+  void mfsStop(MFSDriver *devp);

+  mfs_error_t mfsMount(MFSDriver *devp);

+  mfs_error_t mfsUnmount(MFSDriver *devp);

+  mfs_error_t mfsReadRecord(MFSDriver *devp, uint32_t id,

+                            uint32_t *np, uint8_t *buffer);

+  mfs_error_t mfsUpdateRecord(MFSDriver *devp, uint32_t id,

+                              uint32_t n, const uint8_t *buffer);

+  mfs_error_t mfsEraseRecord(MFSDriver *devp, uint32_t id);

+#ifdef __cplusplus

+}

+#endif

+

+#endif /* MFS_H */

+

+/** @} */

+