diff options
| -rw-r--r-- | os/ex/Micron/m25q.c | 1668 | ||||
| -rw-r--r-- | os/ex/Micron/m25q.h | 546 | ||||
| -rw-r--r-- | os/ex/ST/l3gd20.c | 964 | ||||
| -rw-r--r-- | os/ex/ST/l3gd20.h | 660 | ||||
| -rw-r--r-- | os/ex/ST/lis302dl.c | 858 | ||||
| -rw-r--r-- | os/ex/ST/lis302dl.h | 604 | ||||
| -rw-r--r-- | os/ex/ST/lis3dsh.c | 1228 | ||||
| -rw-r--r-- | os/ex/ST/lis3dsh.h | 652 | ||||
| -rw-r--r-- | os/ex/ST/lis3mdl.c | 966 | ||||
| -rw-r--r-- | os/ex/ST/lis3mdl.h | 736 | ||||
| -rw-r--r-- | os/ex/ST/lsm303dlhc.c | 1808 | ||||
| -rw-r--r-- | os/ex/ST/lsm303dlhc.h | 882 | ||||
| -rw-r--r-- | os/ex/ST/lsm6ds0.c | 1692 | ||||
| -rw-r--r-- | os/ex/ST/lsm6ds0.h | 1208 | ||||
| -rw-r--r-- | os/ex/subsystems/mfs/mfs.c | 1366 | ||||
| -rw-r--r-- | os/ex/subsystems/mfs/mfs.h | 678 |
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 */
+
+/** @} */
+
|