From 77a63206e770cb4a73abf3063ce381fa8de4705b Mon Sep 17 00:00:00 2001 From: Giovanni Di Sirio Date: Thu, 16 Jun 2016 07:44:08 +0000 Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9627 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/ex/Micron/m25q.c | 1668 ++++++++++++++++++++-------------------- os/ex/Micron/m25q.h | 546 ++++++------- os/ex/ST/l3gd20.c | 964 +++++++++++------------ os/ex/ST/l3gd20.h | 660 ++++++++-------- os/ex/ST/lis302dl.c | 858 ++++++++++----------- os/ex/ST/lis302dl.h | 604 +++++++-------- os/ex/ST/lis3dsh.c | 1228 +++++++++++++++--------------- os/ex/ST/lis3dsh.h | 652 ++++++++-------- os/ex/ST/lis3mdl.c | 966 +++++++++++------------ os/ex/ST/lis3mdl.h | 736 +++++++++--------- os/ex/ST/lsm303dlhc.c | 1808 ++++++++++++++++++++++---------------------- os/ex/ST/lsm303dlhc.h | 882 ++++++++++----------- os/ex/ST/lsm6ds0.c | 1692 ++++++++++++++++++++--------------------- os/ex/ST/lsm6ds0.h | 1208 ++++++++++++++--------------- os/ex/subsystems/mfs/mfs.c | 1366 ++++++++++++++++----------------- os/ex/subsystems/mfs/mfs.h | 678 ++++++++--------- 16 files changed, 8258 insertions(+), 8258 deletions(-) (limited to 'os/ex') 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 . -*/ - -/** - * @file m25q.c - * @brief Micron serial flash driver code. - * - * @addtogroup m25q - * @{ - */ - -#include - -#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 . +*/ + +/** + * @file m25q.c + * @brief Micron serial flash driver code. + * + * @addtogroup m25q + * @{ + */ + +#include + +#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 . -*/ - -/** - * @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 . +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . - -*/ - -/** - * @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 . + +*/ + +/** + * @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 . -*/ - -/** - * @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.
- * 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.
- * 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 . +*/ + +/** + * @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.
+ * 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.
+ * 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 . -*/ - -/** - * @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 . +*/ + +/** + * @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 */ + +/** @} */ + -- cgit v1.2.3