diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..335ec95 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +*.tar.gz diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..40eb22d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.13) + +project(motion.emd.icm42670p.driver) + +function(configure_executable _name) + message(STATUS "Configuring ${_name}") + add_executable(${_name} + ${CMAKE_CURRENT_SOURCE_DIR}/examples/${_name}/${_name}.c + ${_sources}) + + target_include_directories(${_name} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/ + ${CMAKE_CURRENT_SOURCE_DIR}/examples/) +endfunction() + +set(_sources + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu.h + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_apex.c + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_apex.h + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_defs.h + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_driver.c + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_driver.h + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_extfunc.h + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_regmap_rev_a.h + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_selftest.c + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_selftest.h + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_transport.c + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_transport.h + ${CMAKE_CURRENT_SOURCE_DIR}/icm42670p/imu/inv_imu_version.h + ${CMAKE_CURRENT_SOURCE_DIR}/examples/system_interface.h + ${CMAKE_CURRENT_SOURCE_DIR}/examples/system_interface.c) + +configure_executable(eis) +configure_executable(freefall) +configure_executable(pedometer) +configure_executable(raw) +configure_executable(selftest) +configure_executable(smd) +configure_executable(tilt) +configure_executable(wom) + diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..79f88f7 --- /dev/null +++ b/LICENSE.txt @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2020, Invensense, inc. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..dd7b044 --- /dev/null +++ b/README.md @@ -0,0 +1,26 @@ +# eMD software for ICM-42670-P + +_eMD_ stands for _embedded Motion Driver_. + +## Summary + +The eMD software for ICM-42670-P consists in a driver and a set of examples to showcase our IMU's capabilities. It targets microcontroller-based application and is coded in C language. + +## Content + +The `icm42670p/` folder contains the drivers for the IMU. + +The `examples/` folder contains: +* A set of examples showcasing the IMU capabilities: `examples/*/`. Each sub-folder contains a README describing the application. +* The hardware abstraction layer definition: `system_interface.h`. + +## Building the code + +A `CMakeLists.txt` file is provided as an example on how to build. + +Open a terminal and go to the project root folder, then execute the following commands: + +``` +cmake -S . -B build/ +cmake --build build/ +``` diff --git a/examples/Invn/InvError.h b/examples/Invn/InvError.h new file mode 100644 index 0000000..280498e --- /dev/null +++ b/examples/Invn/InvError.h @@ -0,0 +1,51 @@ +/* + * + * Copyright (c) [2015] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/** @defgroup InvError Error code + * @brief Common error code + * + * @ingroup EmbUtils + * @{ + */ + +#ifndef _INV_ERROR_H_ +#define _INV_ERROR_H_ + +/** @brief Common error code definition + */ +enum inv_error { + INV_ERROR_SUCCESS = 0, /**< no error */ + INV_ERROR = -1, /**< unspecified error */ + INV_ERROR_NIMPL = -2, /**< function not implemented for given arguments */ + INV_ERROR_TRANSPORT = -3, /**< error occurred at transport level */ + INV_ERROR_TIMEOUT = -4, /**< action did not complete in the expected time window */ + INV_ERROR_SIZE = -5, /**< argument's size is not suitable to complete requested action */ + INV_ERROR_OS = -6, /**< error related to OS */ + INV_ERROR_IO = -7, /**< error related to IO operation */ + INV_ERROR_MEM = -9, /**< not enough memory to complete requested action */ + INV_ERROR_HW = -10, /**< error at HW level */ + INV_ERROR_BAD_ARG = -11, /**< provided arguments are not good to perform requested action */ + INV_ERROR_UNEXPECTED = -12, /**< something unexpected happened */ + INV_ERROR_FILE = -13, /**< cannot access file or unexpected format */ + INV_ERROR_PATH = -14, /**< invalid file path */ + INV_ERROR_IMAGE_TYPE = -15, /**< error when image type is not managed */ + INV_ERROR_WATCHDOG = -16, /**< error when device doesn't respond to ping */ +}; + +#endif /* _INV_ERROR_H_ */ + +/** @} */ diff --git a/examples/eis/README.md b/examples/eis/README.md new file mode 100644 index 0000000..c42cd1b --- /dev/null +++ b/examples/eis/README.md @@ -0,0 +1,64 @@ +# eis + +This application demonstrates how to enable FSYNC feature required for EIS application. The FSYNC feature consists of an external signal provided to the IMU as input, which is used to tag sensor data closest to the rising edge of the signal, as well as the delay between the rising edge and the sensor data. + +For this application, the FSYNC signal will be emulated by the MCU with a 30 Hz square signal. + +## Command interface + +This application allows the following command to be sent through UART: +* `f`: to toggle FIFO usage (enabled/disabled, enabled by default). Data will be read from sensor registers if FIFO is disabled. +* `c`: to print current configuration. +* `h`: to print helps screen. + +## Terminal output + +### Data format + +Data are printed on the terminal as follow: +``` + us Gyro: dps [FSYNC occurred us ago] +``` + +With: +* ``: Time in microsecond read from MCU clock when latest INT1 was fired. +* ``: Raw gyroscope value converted in dps. +* ``: Only for sample tagged with FSYNC. Time in us between the FSYNC rising edge and the current sampling time. + +### Example of output + +``` +[I] ### +[I] ### Example EIS +[I] ### +[I] 8190 us Gyro: -0.24 -0.06 0.00 dps +[I] 13189 us Gyro: -0.37 -0.18 0.12 dps +[I] 18189 us Gyro: -0.31 -0.18 0.00 dps +[I] 23188 us Gyro: -0.31 -0.18 0.12 dps +[I] 28187 us Gyro: -0.37 -0.12 0.00 dps +[I] 33187 us Gyro: -0.31 -0.12 0.00 dps +[I] 38186 us Gyro: -0.43 -0.12 0.06 dps FSYNC occurred 3684 us ago +[I] 43185 us Gyro: -0.37 -0.06 0.00 dps +[I] 48185 us Gyro: -0.31 -0.12 -0.06 dps +[I] 53184 us Gyro: -0.37 -0.24 0.06 dps +[I] 58183 us Gyro: -0.31 -0.12 -0.12 dps +[I] 63183 us Gyro: -0.31 -0.12 0.00 dps +[I] 68182 us Gyro: -0.31 -0.18 0.06 dps FSYNC occurred 347 us ago +[I] 73181 us Gyro: -0.37 -0.06 0.00 dps +[I] 78180 us Gyro: -0.37 -0.18 -0.06 dps +[I] 83180 us Gyro: -0.31 -0.18 0.06 dps +[I] 88179 us Gyro: -0.37 -0.06 0.06 dps +[I] 93178 us Gyro: -0.31 -0.12 0.06 dps +[I] 98178 us Gyro: -0.37 -0.18 0.06 dps +[I] 103177 us Gyro: -0.43 -0.06 0.00 dps FSYNC occurred 2011 us ago +[I] 108176 us Gyro: -0.37 -0.06 0.12 dps +[I] 113176 us Gyro: -0.43 -0.12 0.12 dps + + +``` + +We can verify the FSYNC signal frequency (expected to be 30 Hz) based on the last two FSYNC event detected above: + +- fysnc_delta_time = (t1 - delay1) - (t0 - delay0) +- fsync_delta_time = (103177 - 2011) - (68182 - 347) +- fysnc_delta_time = 33331 us = 30.002 Hz diff --git a/examples/eis/eis.c b/examples/eis/eis.c new file mode 100644 index 0000000..9e8fd85 --- /dev/null +++ b/examples/eis/eis.c @@ -0,0 +1,357 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/* Driver */ +#include "imu/inv_imu_driver.h" + +/* Board drivers */ +#include "system_interface.h" + +/* std */ +#include + +/* + * Select communication link between SmartMotion and IMU. + * SPI4: `UI_SPI4` + * I2C: `UI_I2C` + */ +#define SERIF_TYPE UI_SPI4 + +/* + * FSYNC toggle frequency emulating a camera module + */ +#define FSYNC_FREQUENCY_HZ 30 + +/* Static variables */ +static inv_imu_device_t imu_dev; /* Driver structure */ +static volatile int int1_flag; /* Flag set when INT1 is asserted */ +static volatile uint64_t int1_timestamp; /* Timestamp when INT1 is asserted */ + +/* Static variables for command interface */ +static uint8_t fifo_en; /* Indicates if data are read from FIFO (1) or registers (0) */ + +/* Static functions definition */ +static int setup_mcu(); +static int setup_imu(); +static int configure_fifo(); +static void int_cb(void *context, unsigned int int_num); +static void fsync_cb(void *context); +static void sensor_event_cb(inv_imu_sensor_event_t *event); +static int get_uart_command(); +static int print_help(); +static int print_current_config(); + +/* Main function implementation */ +int main(void) +{ + int rc = 0; + + rc |= setup_mcu(); + SI_CHECK_RC(rc); + + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + INV_MSG(INV_MSG_LEVEL_INFO, "### Example EIS"); + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + + /* Reset commands interface states */ + fifo_en = 1; + + rc |= setup_imu(); + SI_CHECK_RC(rc); + + /* Reset timestamp and interrupt flag */ + int1_flag = 0; + int1_timestamp = 0; + + do { + /* Poll device for data */ + if (int1_flag) { + si_disable_irq(); + /* Clear interrupt flag */ + int1_flag = 0; + si_enable_irq(); + + if (fifo_en) + rc |= inv_imu_get_data_from_fifo(&imu_dev); + else + rc |= inv_imu_get_data_from_registers(&imu_dev); + + SI_CHECK_RC(rc); + rc = 0; /* reset `rc` (contains the number of packet read if above check is passing) */ + } + + rc |= get_uart_command(); + } while (rc == 0); + + return rc; +} + +/* Initializes MCU peripherals. */ +static int setup_mcu() +{ + int rc = 0; + + rc |= si_board_init(); + + /* Configure UART for log */ + rc |= si_config_uart_for_print(SI_UART_ID_FTDI, INV_MSG_LEVEL_DEBUG); + + /* Configure GPIO to call `int_cb` when INT1 fires. */ + rc |= si_init_gpio_int(SI_GPIO_INT1, int_cb); + + /* Init timer peripheral for sleep and get_time */ + rc |= si_init_timers(); + + /* Initialize serial interface between MCU and IMU */ + rc |= si_io_imu_init(SERIF_TYPE); + + /* Send FSYNC signal to IMU to emulate camera module at 30Hz */ + rc |= si_start_gpio_fsync(2 * FSYNC_FREQUENCY_HZ /* FSYNC with rising edge @ 30Hz */, fsync_cb); + + return rc; +} + +static int setup_imu() +{ + int rc = 0; + inv_imu_serif_t imu_serif; + uint8_t whoami; + inv_imu_int1_pin_config_t int1_pin_config; + + /* Initialize serial interface between MCU and IMU */ + imu_serif.context = 0; /* no need */ + imu_serif.read_reg = si_io_imu_read_reg; + imu_serif.write_reg = si_io_imu_write_reg; + imu_serif.max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */ + imu_serif.max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */ + imu_serif.serif_type = SERIF_TYPE; + + /* Init device */ + rc |= inv_imu_init(&imu_dev, &imu_serif, sensor_event_cb); + SI_CHECK_RC(rc); + +#if SERIF_TYPE == UI_SPI4 + /* Configure slew-rate to 19 ns (required when using EVB) */ + rc |= inv_imu_set_spi_slew_rate(&imu_dev, DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS); + SI_CHECK_RC(rc); +#endif + + /* Check WHOAMI */ + rc |= inv_imu_get_who_am_i(&imu_dev, &whoami); + SI_CHECK_RC(rc); + if (whoami != INV_IMU_WHOAMI) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Erroneous WHOAMI value."); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Read 0x%02x", whoami); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Expected 0x%02x", INV_IMU_WHOAMI); + return INV_ERROR; + } + + /* + * Configure interrupts pins + * - Polarity High + * - Pulse mode + * - Push-Pull drive + */ + int1_pin_config.int_polarity = INT_CONFIG_INT1_POLARITY_HIGH; + int1_pin_config.int_mode = INT_CONFIG_INT1_MODE_PULSED; + int1_pin_config.int_drive = INT_CONFIG_INT1_DRIVE_CIRCUIT_PP; + rc |= inv_imu_set_pin_config_int1(&imu_dev, &int1_pin_config); + + rc |= inv_imu_set_gyro_fsr(&imu_dev, GYRO_CONFIG0_FS_SEL_2000dps); + rc |= inv_imu_set_gyro_frequency(&imu_dev, GYRO_CONFIG0_ODR_200_HZ); + rc |= configure_fifo(&imu_dev); + rc |= inv_imu_enable_fsync(&imu_dev); + rc |= inv_imu_enable_gyro_low_noise_mode(&imu_dev); + + return rc; +} + +static int configure_fifo() +{ + int rc = 0; + uint8_t data = 0; + inv_imu_interrupt_parameter_t int1_config = { (inv_imu_interrupt_value)0 }; + + rc |= inv_imu_configure_fifo(&imu_dev, fifo_en ? INV_IMU_FIFO_ENABLED : INV_IMU_FIFO_DISABLED); + + if (fifo_en) { + int1_config.INV_FIFO_THS = INV_IMU_ENABLE; + } else { + int1_config.INV_UI_DRDY = INV_IMU_ENABLE; + + /* Configure IMU to tag FSYNC in TEMP register LSB */ + rc |= inv_imu_read_reg(&imu_dev, FSYNC_CONFIG_MREG1, 1, &data); + data &= ~FSYNC_CONFIG_FSYNC_UI_SEL_MASK; + data |= (uint8_t)FSYNC_CONFIG_UI_SEL_TEMP; + rc |= inv_imu_write_reg(&imu_dev, FSYNC_CONFIG_MREG1, 1, &data); + } + rc |= inv_imu_set_config_int1(&imu_dev, &int1_config); + + return rc; +} + +/* IMU interrupt handler. */ +static void int_cb(void *context, unsigned int int_num) +{ + (void)context; + + if (int_num == SI_GPIO_INT1) { + int1_timestamp = si_get_time_us(); + int1_flag = 1; + } +} + +/* FSYNC callback. */ +static void fsync_cb(void *context) +{ + (void)context; + si_toggle_gpio_fsync(); +} + +static void sensor_event_cb(inv_imu_sensor_event_t *event) +{ + uint64_t int_timestamp = 0; + int fsync_detected = 0; + uint16_t fsync_delay = 0; + + /* Get timestamp */ + si_disable_irq(); + int_timestamp = int1_timestamp; + si_enable_irq(); + + /* Check FSYNC tag */ + if (fifo_en ? (event->sensor_mask & (1 << INV_SENSOR_FSYNC_EVENT)) : (event->temperature & 1)) { + /* + * When data are read from FIFO, FSYNC is tagged in FIFO's header and FSYNC delay replaces + * timestamp in the `timestamp_fsync` field. + * + * When data are read from registers, FSYNC is tagged in sensor data LSB (configured in this + * example to TEMP register LSB). + * FSYNC delay is read from TMST_FSYNC registers. + */ + if (!fifo_en) { + uint8_t fsync_count[2]; + + inv_imu_read_reg(&imu_dev, TMST_FSYNCH, 2, fsync_count); + format_u16_data(imu_dev.endianness_data, &fsync_count[0], &event->timestamp_fsync); + } + + fsync_detected = 1; + fsync_delay = event->timestamp_fsync; + } + + /* Check gyro data */ + if (fifo_en ? ((event->sensor_mask & (1 << INV_SENSOR_GYRO)) && + (event->sensor_mask & (1 << INV_SENSOR_TEMPERATURE))) : + (event->gyro[0] != INVALID_VALUE_FIFO)) { + /* + * Gyro data are coded as 16-bits signed (max_lsb = 2^(16-1) = 32768) + * with the configured FSR (2000 dps, see `setup_imu()` function). + */ + int max_lsb = 32768; /* 16-bits signed (max_lsb = 2^(16-1) = 32768) */ + int gyro_fsr_dps = 2000; /* 2000 dps, see `setup_imu()` function */ + float gyro_dps[3]; + char string_fsync[32] = ""; + + /* Convert raw data from FIFO data to SI units */ + gyro_dps[0] = (float)(event->gyro[0] * gyro_fsr_dps) / max_lsb; + gyro_dps[1] = (float)(event->gyro[1] * gyro_fsr_dps) / max_lsb; + gyro_dps[2] = (float)(event->gyro[2] * gyro_fsr_dps) / max_lsb; + + /* FSYNC delay has a 1 us resolution, regardless of `TMST_RES` field. */ + if (fsync_detected) + snprintf(string_fsync, sizeof(string_fsync), "FSYNC occurred %hu us ago", fsync_delay); + + INV_MSG(INV_MSG_LEVEL_INFO, "%10llu us Gyro:% 8.2f % 8.2f % 8.2f dps %s", int_timestamp, + gyro_dps[0], gyro_dps[1], gyro_dps[2], string_fsync); + } +} + +/* Get command from user through UART */ +static int get_uart_command() +{ + int rc = 0; + char cmd = 0; + + rc |= si_get_uart_command(SI_UART_ID_FTDI, &cmd); + SI_CHECK_RC(rc); + + switch (cmd) { + case 'f': /* Use FIFO or sensor register */ + fifo_en = !fifo_en; + INV_MSG(INV_MSG_LEVEL_INFO, "%s FIFO.", fifo_en ? "Enabling" : "Disabling"); + rc |= configure_fifo(); + break; + case 'c': + rc |= print_current_config(); + break; + case 'h': + case 'H': + rc |= print_help(); + break; + case 0: + break; /* No command received */ + default: + INV_MSG(INV_MSG_LEVEL_INFO, "Unknown command : %c", cmd); + rc |= print_help(); + break; + } + + return rc; +} + +/* Help for UART command interface */ +static int print_help() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Help"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'f' : Toggle FIFO usage"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'c' : Print current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'h' : Print this helper"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Print current sample configuration */ +static int print_current_config() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# FIFO: %s", fifo_en ? "Enabled" : "Disabled"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Get time implementation for IMU driver */ +uint64_t inv_imu_get_time_us(void) +{ + return si_get_time_us(); +} + +/* Sleep implementation for IMU driver */ +void inv_imu_sleep_us(uint32_t us) +{ + si_sleep_us(us); +} diff --git a/examples/freefall/README.md b/examples/freefall/README.md new file mode 100644 index 0000000..5d9392c --- /dev/null +++ b/examples/freefall/README.md @@ -0,0 +1,97 @@ +# freefall + +This application demonstrates how to configure and use FreeFall feature. Two types of events are detected by the algorithm: +* LOW-G: aims at detecting when the device is free-falling (before the end of the fall). +* FREEFALL: aims at detecting when the device felt and hit the ground. + +## Terminal output + +### Data format + +Data are printed on the terminal as follow: + +``` + us LOW_G + us FREEFALL length: cm ( ms) +``` + +With: +* ``: Time in microsecond read from MCU clock when latest INT1 was fired. +* ``: Length of the FreeFall in centimeters, computed based on the number of samples. +* ``: Duration of the FreeFall in milliseconds, computed based on the number of samples. + +### Example of output + +``` +[I] ### +[I] ### Example FreeFall +[I] ### +[I] 2030564 us LOW_G +[I] 2033061 us LOW_G +[I] 2035565 us LOW_G +[I] 2038070 us LOW_G +[I] 2040573 us LOW_G +[I] 2043077 us LOW_G +[I] 2045581 us LOW_G +[I] 2048084 us LOW_G +[I] 2050588 us LOW_G +[I] 2053092 us LOW_G +[I] 2055596 us LOW_G +[I] 2058099 us LOW_G +[I] 2060603 us LOW_G +[I] 2063106 us LOW_G +[I] 2065610 us LOW_G +[I] 2068115 us LOW_G +[I] 2070618 us LOW_G +[I] 2073122 us LOW_G +[I] 2075626 us LOW_G +[I] 2078131 us LOW_G +[I] 2080633 us LOW_G +[I] 2083137 us LOW_G +[I] 2085641 us LOW_G +[I] 2088145 us LOW_G +[I] 2090649 us LOW_G +[I] 2093153 us LOW_G +[I] 2095657 us LOW_G +[I] 2098162 us LOW_G +[I] 2100665 us LOW_G +[I] 2103168 us LOW_G +[I] 2105673 us LOW_G +[I] 2108176 us LOW_G +[I] 2110681 us LOW_G +[I] 2113184 us LOW_G +[I] 2115688 us LOW_G +[I] 2118191 us LOW_G +[I] 2120696 us LOW_G +[I] 2123200 us LOW_G +[I] 2125704 us LOW_G +[I] 2128208 us LOW_G +[I] 2130712 us LOW_G +[I] 2133216 us LOW_G +[I] 2135720 us LOW_G +[I] 2138224 us LOW_G +[I] 2140727 us LOW_G +[I] 2143231 us LOW_G +[I] 2145735 us LOW_G +[I] 2148239 us LOW_G +[I] 2150742 us LOW_G +[I] 2153247 us LOW_G +[I] 2155750 us LOW_G +[I] 2158254 us LOW_G +[I] 2160760 us LOW_G +[I] 2163262 us LOW_G +[I] 2170775 us LOW_G +[I] 2173278 us LOW_G +[I] 2175783 us LOW_G +[I] 2178287 us LOW_G +[I] 2180791 us LOW_G +[I] 2183295 us LOW_G +[I] 2185800 us LOW_G +[I] 2188303 us LOW_G +[I] 2190807 us LOW_G +[I] 2193311 us LOW_G +[I] 2195815 us LOW_G +[I] 2198320 us LOW_G +[I] 2215815 us FREEFALL length: 17.15 cm (75 ms) +``` + diff --git a/examples/freefall/freefall.c b/examples/freefall/freefall.c new file mode 100644 index 0000000..1f1ac48 --- /dev/null +++ b/examples/freefall/freefall.c @@ -0,0 +1,240 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/* Driver */ +#include "imu/inv_imu_driver.h" +#include "imu/inv_imu_apex.h" + +/* Board drivers */ +#include "system_interface.h" + +#include /* NULL */ + +/* + * This example showcases how to configure and use FreeFall. + */ + +/* + * Select communication link between SmartMotion and IMU. + * SPI4: `UI_SPI4` + * I2C: `UI_I2C` + */ +#define SERIF_TYPE UI_SPI4 + +/* Static variables */ +static inv_imu_device_t imu_dev; /* Driver structure */ +static volatile int int1_flag; /* Flag set when INT1 is received */ +static volatile uint64_t int1_timestamp; /* Store timestamp when int from IMU fires */ + +/* Static functions definition */ +static int setup_mcu(); +static int setup_imu(); +static void int_cb(void *context, unsigned int int_num); +static float convert_ff_duration_sample_to_cm(uint16_t ff_duration_samples); + +/* Main function implementation */ +int main(void) +{ + int rc = 0; + + rc |= setup_mcu(); + SI_CHECK_RC(rc); + + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + INV_MSG(INV_MSG_LEVEL_INFO, "### Example FreeFall"); + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + + rc |= setup_imu(); + SI_CHECK_RC(rc); + + /* Reset timestamp and interrupt flag */ + int1_flag = 0; + int1_timestamp = 0; + + do { + /* Poll device for data */ + if (int1_flag) { + uint8_t int_status3; + uint64_t timestamp; + + si_disable_irq(); + /* Clear interrupt flag */ + int1_flag = 0; + /* Retrieve timestamp */ + timestamp = int1_timestamp; + si_enable_irq(); + + /* Read FreeFall interrupt status */ + rc |= inv_imu_read_reg(&imu_dev, INT_STATUS3, 1, &int_status3); + SI_CHECK_RC(rc); + + if (int_status3 & (INT_STATUS3_LOWG_DET_INT_MASK)) + INV_MSG(INV_MSG_LEVEL_INFO, " %10llu us LOW_G", timestamp); + + if (int_status3 & (INT_STATUS3_FF_DET_INT_MASK)) { + uint8_t data[2]; + uint16_t ff_duration_samples; + float ff_distance_cm; + + rc |= inv_imu_read_reg(&imu_dev, APEX_DATA4, 2, &data[0]); + ff_duration_samples = (uint16_t)((data[1] << 8) | data[0]); + + ff_distance_cm = convert_ff_duration_sample_to_cm(ff_duration_samples); + + INV_MSG(INV_MSG_LEVEL_INFO, " %10llu us FREEFALL length: %.2f cm (%d ms)", + timestamp, ff_distance_cm, ff_duration_samples); + } + } + } while (rc == 0); + + return rc; +} + +/* Initializes MCU peripherals. */ +static int setup_mcu() +{ + int rc = 0; + + rc |= si_board_init(); + + /* Configure UART for log */ + rc |= si_config_uart_for_print(SI_UART_ID_FTDI, INV_MSG_LEVEL_DEBUG); + + /* Configure GPIO to call `int_cb` when INT1 fires. */ + rc |= si_init_gpio_int(SI_GPIO_INT1, int_cb); + + /* Init timer peripheral for sleep and get_time */ + rc |= si_init_timers(); + + /* Initialize serial interface between MCU and IMU */ + rc |= si_io_imu_init(SERIF_TYPE); + + return rc; +} + +static int setup_imu() +{ + int rc = 0; + inv_imu_serif_t imu_serif; + uint8_t whoami; + inv_imu_apex_parameters_t apex_inputs; + inv_imu_int1_pin_config_t int1_pin_config; + inv_imu_interrupt_parameter_t int1_config = { (inv_imu_interrupt_value)0 }; + + /* Initialize serial interface between MCU and IMU */ + imu_serif.context = 0; /* no need */ + imu_serif.read_reg = si_io_imu_read_reg; + imu_serif.write_reg = si_io_imu_write_reg; + imu_serif.max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */ + imu_serif.max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */ + imu_serif.serif_type = SERIF_TYPE; + + /* Init device */ + rc |= inv_imu_init(&imu_dev, &imu_serif, NULL); + SI_CHECK_RC(rc); + +#if SERIF_TYPE == UI_SPI4 + /* Configure slew-rate to 19 ns (required when using EVB) */ + rc |= inv_imu_set_spi_slew_rate(&imu_dev, DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS); + SI_CHECK_RC(rc); +#endif + + /* Check WHOAMI */ + rc |= inv_imu_get_who_am_i(&imu_dev, &whoami); + SI_CHECK_RC(rc); + if (whoami != INV_IMU_WHOAMI) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Erroneous WHOAMI value."); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Read 0x%02x", whoami); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Expected 0x%02x", INV_IMU_WHOAMI); + return INV_ERROR; + } + + /* + * Configure interrupts pins + * - Polarity High + * - Pulse mode + * - Push-Pull drive + */ + int1_pin_config.int_polarity = INT_CONFIG_INT1_POLARITY_HIGH; + int1_pin_config.int_mode = INT_CONFIG_INT1_MODE_PULSED; + int1_pin_config.int_drive = INT_CONFIG_INT1_DRIVE_CIRCUIT_PP; + rc |= inv_imu_set_pin_config_int1(&imu_dev, &int1_pin_config); + + /* Configure interrupts sources */ + int1_config.INV_FF = INV_IMU_ENABLE; + int1_config.INV_LOWG = INV_IMU_ENABLE; + rc |= inv_imu_set_config_int1(&imu_dev, &int1_config); + + /* Disabling FIFO usage to optimize power consumption */ + rc |= inv_imu_configure_fifo(&imu_dev, INV_IMU_FIFO_DISABLED); + + /* Configure accel frequency */ + rc |= inv_imu_set_accel_frequency(&imu_dev, ACCEL_CONFIG0_ODR_400_HZ); + rc |= inv_imu_apex_set_frequency(&imu_dev, APEX_CONFIG1_DMP_ODR_400Hz); + + /* Set 2X averaging to minimize power consumption */ + rc |= inv_imu_set_accel_lp_avg(&imu_dev, ACCEL_CONFIG1_ACCEL_FILT_AVG_2); + + /* Enable accel in LP mode */ + rc |= inv_imu_enable_accel_low_power_mode(&imu_dev); + + /* Set APEX parameters */ + rc |= inv_imu_apex_init_parameters_struct(&imu_dev, &apex_inputs); + /* Parameters can be modified here if needed */ + apex_inputs.power_save = APEX_CONFIG0_DMP_POWER_SAVE_DIS; /* Disable power save mode */ + rc |= inv_imu_apex_configure_parameters(&imu_dev, &apex_inputs); + + /* Enable FreeFall */ + rc |= inv_imu_apex_enable_ff(&imu_dev); + + return rc; +} + +/* IMU interrupt handler. */ +static void int_cb(void *context, unsigned int int_num) +{ + (void)context; + + if (int_num == SI_GPIO_INT1) { + int1_timestamp = si_get_time_us(); + int1_flag = 1; + } +} + +static float convert_ff_duration_sample_to_cm(uint16_t ff_duration_samples) +{ + uint16_t ff_duration_ms; + float ff_distance_cm; + + ff_duration_ms = (uint16_t)(((uint32_t)ff_duration_samples * 2500) / 1000); + /* dist = speed_average * time = (9.81 * time ^ 2) / 2) */ + ff_distance_cm = (9.81f * ff_duration_ms * ff_duration_ms) / 2 / 10000 /* to cm/s^2 */; + + return ff_distance_cm; +} + +/* Get time implementation for IMU driver */ +uint64_t inv_imu_get_time_us(void) +{ + return si_get_time_us(); +} + +/* Sleep implementation for IMU driver */ +void inv_imu_sleep_us(uint32_t us) +{ + si_sleep_us(us); +} diff --git a/examples/pedometer/README.md b/examples/pedometer/README.md new file mode 100644 index 0000000..50419e3 --- /dev/null +++ b/examples/pedometer/README.md @@ -0,0 +1,51 @@ +# pedometer + +This application demonstrates how to configure and use Pedometer feature. The algorithm will detect steps and provide details such as the cadence and the estimated activity type (Walk, Run, Unknown). + +## Command interface + +This application allows the following command to be sent through UART: +* `a`: to toggle Pedometer configuration (Normal/Low-Power, Normal by default) +* `p`: to toggle Power-Save mode usage (enable/disable, disabled by default) +* `c`: to print current configuration. +* `h`: to print helps screen. + +## Terminal output + +### Data format + +Data are printed on the terminal as follow: + +``` + us STEP_DET count: steps cadence: steps/s activity: +``` + +With: +* ``: Time in microsecond read from MCU clock when latest INT1 was fired. +* `step_count`: Number of steps as reported by eDMP +* `cadence`: Number of steps per second +* `activity_class`: Type of activity detected amongst (Run, Walk, Unknown) + +**Note:** Step count and cadency are valid only once the step count starts incrementing. `` will remain _Unknown_ meanwhile. + +### Example of output + +``` +[I] ### +[I] ### Example Pedometer +[I] ### +[I] 2818306 us STEP_DET count: 0 steps cadence: 2.0 steps/s activity: Unknown +[I] 3177550 us STEP_DET count: 0 steps cadence: 2.0 steps/s activity: Unknown +[I] 3556702 us STEP_DET count: 6 steps cadence: 2.0 steps/s activity: Unknown +[I] 3915950 us STEP_DET count: 7 steps cadence: 2.1 steps/s activity: Walk +[I] 4255250 us STEP_DET count: 8 steps cadence: 2.2 steps/s activity: Walk +[I] 4594566 us STEP_DET count: 9 steps cadence: 2.3 steps/s activity: Walk +[I] 4933861 us STEP_DET count: 10 steps cadence: 2.4 steps/s activity: Walk +[I] 5273155 us STEP_DET count: 11 steps cadence: 2.5 steps/s activity: Run +[I] 5632388 us STEP_DET count: 12 steps cadence: 2.5 steps/s activity: Run +[I] 5991618 us STEP_DET count: 13 steps cadence: 2.6 steps/s activity: Run +[I] 6350879 us STEP_DET count: 14 steps cadence: 2.6 steps/s activity: Run +[I] 6710180 us STEP_DET count: 15 steps cadence: 2.6 steps/s activity: Run +[I] 7069450 us STEP_DET count: 16 steps cadence: 2.7 steps/s activity: Run +``` + diff --git a/examples/pedometer/pedometer.c b/examples/pedometer/pedometer.c new file mode 100644 index 0000000..b97fd61 --- /dev/null +++ b/examples/pedometer/pedometer.c @@ -0,0 +1,364 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/* Driver */ +#include "imu/inv_imu_driver.h" +#include "imu/inv_imu_apex.h" + +/* Board drivers */ +#include "system_interface.h" + +#include /* NULL */ + +/* + * This example showcases how to configure and use Pedometer. + */ + +/* + * Select communication link between SmartMotion and IMU. + * SPI4: `UI_SPI4` + * I2C: `UI_I2C` + */ +#define SERIF_TYPE UI_SPI4 + +/* WOM threshold */ +#if INV_IMU_HFSR_SUPPORTED +#define WOM_THRESHOLD 7 /* ~50 mg */ +#else +#define WOM_THRESHOLD 13 /* ~50 mg */ +#endif + +/* Static variables */ +static inv_imu_device_t imu_dev; /* Driver structure */ +static volatile int int1_flag; /* Flag set when INT1 is received */ +static volatile uint64_t int1_timestamp; /* Store timestamp when int from IMU fires */ +static uint8_t dmp_odr_hz; /* DMP ODR */ + +/* Static variables for command interface */ +static uint8_t use_lp_config; /* Indicates if pedometer is configured in Low Power mode */ +static uint8_t use_power_save; /* Indicates if power save mode is being used */ + +/* Static functions definition */ +static int setup_mcu(); +static int setup_imu(); +static int configure_pedometer(); +static void int_cb(void *context, unsigned int int_num); +static int get_uart_command(); +static int print_help(); +static int print_current_config(); + +/* Main function implementation */ +int main(void) +{ + int rc = 0; + + rc |= setup_mcu(); + SI_CHECK_RC(rc); + + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + INV_MSG(INV_MSG_LEVEL_INFO, "### Example Pedometer"); + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + + /* Reset commands interface states */ + use_lp_config = 0; + use_power_save = 0; + + rc |= setup_imu(); + SI_CHECK_RC(rc); + + /* Reset timestamp and interrupt flag */ + int1_flag = 0; + int1_timestamp = 0; + + do { + /* Poll device for data */ + if (int1_flag) { + uint8_t int_status3; + uint64_t timestamp; + static uint8_t step_cnt_ovflw = 0; + static uint64_t step_cnt = 0; + + si_disable_irq(); + /* Clear interrupt flag */ + int1_flag = 0; + /* Retrieve timestamp */ + timestamp = int1_timestamp; + si_enable_irq(); + + /* Read Pedometer interrupt status */ + rc |= inv_imu_read_reg(&imu_dev, INT_STATUS3, 1, &int_status3); + SI_CHECK_RC(rc); + + if (int_status3 & INT_STATUS3_STEP_CNT_OVF_INT_MASK) { + step_cnt_ovflw++; + INV_MSG(INV_MSG_LEVEL_INFO, " %10llu us STEP_CNT_OVFL", timestamp); + } + + if (int_status3 & INT_STATUS3_STEP_DET_INT_MASK) { + inv_imu_apex_step_activity_t ped_data; + float nb_samples = 0; + float cadence_steps_per_sec = 0; + + rc |= inv_imu_apex_get_data_activity(&imu_dev, &ped_data); + SI_CHECK_RC(rc); + + /* Decode `step_candence` into number of samples */ + nb_samples = (float)ped_data.step_cadence / 4; + + /* Compute cadence in steps per sec */ + cadence_steps_per_sec = (float)dmp_odr_hz / nb_samples; + + /* Compute step count */ + step_cnt = ped_data.step_cnt + step_cnt_ovflw * UINT16_MAX; + + INV_MSG( + INV_MSG_LEVEL_INFO, + " %10llu us STEP_DET count: %5llu steps cadence: %.1f steps/s activity: %s", + timestamp, step_cnt, cadence_steps_per_sec, + ped_data.activity_class == APEX_DATA3_ACTIVITY_CLASS_WALK ? "Walk" : + ped_data.activity_class == APEX_DATA3_ACTIVITY_CLASS_RUN ? "Run" : + "Unknown"); + } + } + + rc |= get_uart_command(); + } while (rc == 0); + + return rc; +} + +/* Initializes MCU peripherals. */ +static int setup_mcu() +{ + int rc = 0; + + rc |= si_board_init(); + + /* Configure UART for log */ + rc |= si_config_uart_for_print(SI_UART_ID_FTDI, INV_MSG_LEVEL_DEBUG); + + /* Configure GPIO to call `int_cb` when INT1 fires. */ + rc |= si_init_gpio_int(SI_GPIO_INT1, int_cb); + + /* Init timer peripheral for sleep and get_time */ + rc |= si_init_timers(); + + /* Initialize serial interface between MCU and IMU */ + rc |= si_io_imu_init(SERIF_TYPE); + + return rc; +} + +static int setup_imu() +{ + int rc = 0; + inv_imu_serif_t imu_serif; + uint8_t whoami; + inv_imu_int1_pin_config_t int1_pin_config; + inv_imu_interrupt_parameter_t int1_config = { (inv_imu_interrupt_value)0 }; + + /* Initialize serial interface between MCU and IMU */ + imu_serif.context = 0; /* no need */ + imu_serif.read_reg = si_io_imu_read_reg; + imu_serif.write_reg = si_io_imu_write_reg; + imu_serif.max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */ + imu_serif.max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */ + imu_serif.serif_type = SERIF_TYPE; + + /* Init device */ + rc |= inv_imu_init(&imu_dev, &imu_serif, NULL); + SI_CHECK_RC(rc); + +#if SERIF_TYPE == UI_SPI4 + /* Configure slew-rate to 19 ns (required when using EVB) */ + rc |= inv_imu_set_spi_slew_rate(&imu_dev, DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS); + SI_CHECK_RC(rc); +#endif + + /* Check WHOAMI */ + rc |= inv_imu_get_who_am_i(&imu_dev, &whoami); + SI_CHECK_RC(rc); + if (whoami != INV_IMU_WHOAMI) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Erroneous WHOAMI value."); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Read 0x%02x", whoami); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Expected 0x%02x", INV_IMU_WHOAMI); + return INV_ERROR; + } + + /* + * Configure interrupts pins + * - Polarity High + * - Pulse mode + * - Push-Pull drive + */ + int1_pin_config.int_polarity = INT_CONFIG_INT1_POLARITY_HIGH; + int1_pin_config.int_mode = INT_CONFIG_INT1_MODE_PULSED; + int1_pin_config.int_drive = INT_CONFIG_INT1_DRIVE_CIRCUIT_PP; + rc |= inv_imu_set_pin_config_int1(&imu_dev, &int1_pin_config); + + /* Configure interrupts sources */ + int1_config.INV_STEP_DET = INV_IMU_ENABLE; + int1_config.INV_STEP_CNT_OVFL = INV_IMU_ENABLE; + rc |= inv_imu_set_config_int1(&imu_dev, &int1_config); + + /* Disabling FIFO usage to optimize power consumption */ + rc |= inv_imu_configure_fifo(&imu_dev, INV_IMU_FIFO_DISABLED); + + /* Set 2X averaging to minimize power consumption */ + rc |= inv_imu_set_accel_lp_avg(&imu_dev, ACCEL_CONFIG1_ACCEL_FILT_AVG_2); + + /* Enable accel in LP mode */ + rc |= inv_imu_enable_accel_low_power_mode(&imu_dev); + + rc |= configure_pedometer(); + + return rc; +} + +static int configure_pedometer() +{ + int rc = 0; + inv_imu_apex_parameters_t apex_inputs; + + /* Disable Pedometer before configuring it */ + rc |= inv_imu_apex_disable_pedometer(&imu_dev); + + if (use_lp_config) { + rc |= inv_imu_set_accel_frequency(&imu_dev, ACCEL_CONFIG0_ODR_25_HZ); + rc |= inv_imu_apex_set_frequency(&imu_dev, APEX_CONFIG1_DMP_ODR_25Hz); + dmp_odr_hz = 25; + } else { + rc |= inv_imu_set_accel_frequency(&imu_dev, ACCEL_CONFIG0_ODR_50_HZ); + rc |= inv_imu_apex_set_frequency(&imu_dev, APEX_CONFIG1_DMP_ODR_50Hz); + dmp_odr_hz = 50; + } + + /* Set APEX parameters */ + rc |= inv_imu_apex_init_parameters_struct(&imu_dev, &apex_inputs); + /* Parameters can be modified here if needed */ + apex_inputs.power_save = + use_power_save ? APEX_CONFIG0_DMP_POWER_SAVE_EN : APEX_CONFIG0_DMP_POWER_SAVE_DIS; + rc |= inv_imu_apex_configure_parameters(&imu_dev, &apex_inputs); + + /* If POWER_SAVE mode is enabled, WOM has to be enabled */ + if (use_power_save) { + /* Configure and enable WOM to wake-up the DMP once it goes in power save mode */ + rc |= inv_imu_configure_wom(&imu_dev, WOM_THRESHOLD, WOM_THRESHOLD, WOM_THRESHOLD, + WOM_CONFIG_WOM_INT_MODE_ANDED, WOM_CONFIG_WOM_INT_DUR_1_SMPL); + rc |= inv_imu_enable_wom(&imu_dev); + } + + /* Enable Pedometer */ + rc |= inv_imu_apex_enable_pedometer(&imu_dev); + + return rc; +} + +/* IMU interrupt handler. */ +static void int_cb(void *context, unsigned int int_num) +{ + (void)context; + + if (int_num == SI_GPIO_INT1) { + int1_timestamp = si_get_time_us(); + int1_flag = 1; + } +} + +/* Get command from user through UART */ +static int get_uart_command() +{ + int rc = 0; + char cmd = 0; + + rc |= si_get_uart_command(SI_UART_ID_FTDI, &cmd); + SI_CHECK_RC(rc); + + switch (cmd) { + case 'a': /* Use Low Power or Normal Pedometer configuration */ + use_lp_config = !use_lp_config; + INV_MSG(INV_MSG_LEVEL_INFO, "Pedometer configured in %s mode.", + use_lp_config ? "Low Power" : "Normal"); + rc |= configure_pedometer(); + break; + case 'p': /* Toggle power-save mode usage */ + use_power_save = !use_power_save; + INV_MSG(INV_MSG_LEVEL_INFO, "Power-Save mode: %s", use_power_save ? "Enabled" : "Disabled"); + rc |= configure_pedometer(); + break; + case 'c': + rc |= print_current_config(); + break; + case 'h': + case 'H': + rc |= print_help(); + break; + case 0: + break; /* No command received */ + default: + INV_MSG(INV_MSG_LEVEL_INFO, "Unknown command : %c", cmd); + rc |= print_help(); + break; + } + + return rc; +} + +/* Help for UART command interface */ +static int print_help() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Help"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'a' : Toggle Pedometer config (Normal and Low Power)"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'p' : Toggle Power-Save mode usage"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'c' : Print current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'h' : Print this helper"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Print current sample configuration */ +static int print_current_config() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Pedometer configuration: %s mode", + use_lp_config ? "Low Power" : "Normal"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Power-Save: %s", use_power_save ? "Enabled" : "Disabled"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Get time implementation for IMU driver */ +uint64_t inv_imu_get_time_us(void) +{ + return si_get_time_us(); +} + +/* Sleep implementation for IMU driver */ +void inv_imu_sleep_us(uint32_t us) +{ + si_sleep_us(us); +} diff --git a/examples/raw/README.md b/examples/raw/README.md new file mode 100644 index 0000000..34bbdf5 --- /dev/null +++ b/examples/raw/README.md @@ -0,0 +1,72 @@ +# raw + +This application demonstrates how to retrieve accel and gyro data. ODR is configured at 50 Hz. + +## Command interface + +This application allows the following command to be sent through UART: +* `s`: to toggle print data in SI unit (enabled/disabled, enabled by default). +* `l`: to toggle print data in LSB (enabled/disabled, disabled by default). +* `f`: to toggle FIFO usage (enabled/disabled, enabled by default). Data will be read from sensor registers if FIFO is disabled. +* `i`: to toggle FIFO highres mode usage (enabled/disabled, enabled by default). Only apply if FIFO is enabled. +* `p`: to toggle selected power-mode (low-noise/low-power, low-noise by default). +* `c`: to print current configuration. +* `h`: to print helps screen. + +## Terminal output + +### Data format + +Data are printed on the terminal as follow: + +* When print in SI unit is enabled: +``` +SI us Accel: g Gyro: dps Temp: degC FIFO time: us +``` +* When print in LSB is enabled: +``` +LSB us Accel: Gyro: Temp: FIFO time: us +``` + +With: +* `timestamp`: Time in microsecond read from MCU clock when latest INT1 was fired +* `raw_acc_x|y|z`: Raw accel value +* `acc_x|y|z`: Accel value converted in g +* `raw_gyr_x|y|z`: Raw gyro value +* `gyr_x|y|z`: Gyro value converted in dps +* `raw_temp`: Raw temperature value +* `temp`: Temperature value converted in °C +* `fifo_time`: 16-bit timestamp field from FIFO. + +### Example of output + +``` +[I] ### +[I] ### Example Raw +[I] ### +[I] SI 9363 us Accel: - - - Gyro: - - - Temp: 21.63 degC FIFO Time: 3232 us +[I] SI 29425 us Accel: 0.07 -1.00 0.03 g Gyro: - - - Temp: 21.59 degC FIFO Time: 23232 us +[I] SI 49423 us Accel: 0.07 -1.00 0.03 g Gyro: - - - Temp: 21.72 degC FIFO Time: 43232 us +[I] SI 69420 us Accel: 0.07 -1.00 0.03 g Gyro: - - - Temp: 21.74 degC FIFO Time: 63232 us +[I] SI 89417 us Accel: 0.07 -1.00 0.03 g Gyro: -0.32 -0.06 -0.03 dps Temp: 21.74 degC FIFO Time: 83232 us +[I] SI 109414 us Accel: 0.07 -1.00 0.03 g Gyro: -0.34 -0.09 0.02 dps Temp: 21.72 degC FIFO Time: 103232 us +[I] SI 129412 us Accel: 0.07 -1.00 0.03 g Gyro: -0.31 -0.05 0.02 dps Temp: 21.74 degC FIFO Time: 123232 us +[I] SI 149409 us Accel: 0.07 -1.00 0.03 g Gyro: -0.34 -0.05 -0.02 dps Temp: 21.76 degC FIFO Time: 143232 us +[I] SI 169406 us Accel: 0.07 -1.00 0.03 g Gyro: -0.34 -0.05 -0.05 dps Temp: 21.73 degC FIFO Time: 163232 us +[I] SI 189403 us Accel: 0.07 -1.00 0.03 g Gyro: -0.34 -0.02 0.03 dps Temp: 21.74 degC FIFO Time: 183232 us +[I] SI 209401 us Accel: 0.07 -1.00 0.03 g Gyro: -0.31 -0.05 0.02 dps Temp: 21.75 degC FIFO Time: 203232 us +[I] SI 229398 us Accel: 0.07 -1.00 0.03 g Gyro: -0.34 -0.05 0.05 dps Temp: 21.74 degC FIFO Time: 223232 us +[I] SI 249395 us Accel: 0.07 -1.00 0.02 g Gyro: -0.35 -0.05 0.05 dps Temp: 21.72 degC FIFO Time: 243232 us +[I] SI 269392 us Accel: 0.07 -1.00 0.03 g Gyro: -0.35 -0.05 0.09 dps Temp: 21.73 degC FIFO Time: 263232 us +[I] SI 289390 us Accel: 0.07 -1.00 0.02 g Gyro: -0.31 -0.03 0.05 dps Temp: 21.71 degC FIFO Time: 283232 us +[I] SI 309387 us Accel: 0.07 -1.00 0.03 g Gyro: -0.34 -0.02 0.05 dps Temp: 21.76 degC FIFO Time: 303232 us +[I] SI 329384 us Accel: 0.07 -1.00 0.03 g Gyro: -0.34 -0.06 0.02 dps Temp: 21.76 degC FIFO Time: 323232 us +[I] SI 349381 us Accel: 0.07 -1.00 0.03 g Gyro: -0.32 -0.06 0.02 dps Temp: 21.73 degC FIFO Time: 343232 us +[I] SI 369379 us Accel: 0.07 -1.00 0.03 g Gyro: -0.37 -0.09 0.02 dps Temp: 21.73 degC FIFO Time: 363232 us +[I] SI 389376 us Accel: 0.07 -1.00 0.03 g Gyro: -0.35 -0.03 0.02 dps Temp: 21.78 degC FIFO Time: 383232 us +[I] SI 409373 us Accel: 0.07 -1.00 0.03 g Gyro: -0.35 0.00 0.05 dps Temp: 21.75 degC FIFO Time: 403232 us +[I] SI 429370 us Accel: 0.07 -1.00 0.03 g Gyro: -0.35 0.00 0.00 dps Temp: 21.74 degC FIFO Time: 423232 us +[I] SI 449367 us Accel: 0.07 -1.00 0.03 g Gyro: -0.32 -0.03 0.06 dps Temp: 21.78 degC FIFO Time: 443232 us +[I] SI 469365 us Accel: 0.07 -1.00 0.03 g Gyro: -0.35 -0.03 0.06 dps Temp: 21.75 degC FIFO Time: 463232 us +``` + diff --git a/examples/raw/raw.c b/examples/raw/raw.c new file mode 100644 index 0000000..a004f4d --- /dev/null +++ b/examples/raw/raw.c @@ -0,0 +1,484 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/* Driver */ +#include "imu/inv_imu_driver.h" + +/* Board drivers */ +#include "system_interface.h" + +/* std */ +#include + +/* + * This example showcases how to configure IMU to stream accel and gyro data. + */ + +/* + * Select communication link between SmartMotion and IMU. + * SPI4: `UI_SPI4` + * I2C: `UI_I2C` + */ +#define SERIF_TYPE UI_SPI4 + +/* Static variables */ +static inv_imu_device_t imu_dev; /* Driver structure */ +static volatile int int1_flag; /* Flag set when INT1 is asserted */ +static volatile uint64_t int1_timestamp; /* Timestamp when INT1 is asserted */ + +/* Static variables for command interface */ +static uint8_t print_si; /* Indicates if data should be printed in SI */ +static uint8_t print_lsb; /* Indicates if data should be printed in LSB */ +static uint8_t fifo_en; /* Indicates if data are read from FIFO (1) or registers (0) */ +static uint8_t hires_en; /* Indicates if highres is enabled */ +static uint8_t use_ln; /* Indicates if power mode is low noise (1) or low power (0) */ + +/* Static functions definition */ +static int setup_mcu(); +static int setup_imu(); +static int configure_fifo(); +static int configure_hires(); +static int configure_power_mode(); +static void int_cb(void *context, unsigned int int_num); +static void sensor_event_cb(inv_imu_sensor_event_t *event); +static int get_uart_command(); +static int print_help(); +static int print_current_config(); + +/* Main function implementation */ +int main(void) +{ + int rc = 0; + + rc |= setup_mcu(); + SI_CHECK_RC(rc); + + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + INV_MSG(INV_MSG_LEVEL_INFO, "### Example Raw"); + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + + /* Reset commands interface states */ + print_si = 1; + print_lsb = 0; + fifo_en = 1; + hires_en = 1; + use_ln = 1; + + rc |= setup_imu(); + SI_CHECK_RC(rc); + + /* Reset timestamp and interrupt flag */ + int1_flag = 0; + int1_timestamp = 0; + + do { + /* Poll device for data */ + if (int1_flag) { + si_disable_irq(); + /* Clear interrupt flag */ + int1_flag = 0; + si_enable_irq(); + + if (fifo_en) + rc |= inv_imu_get_data_from_fifo(&imu_dev); + else + rc |= inv_imu_get_data_from_registers(&imu_dev); + + SI_CHECK_RC(rc); + rc = 0; /* reset `rc` (contains the number of packet read if above check is passing) */ + } + + rc |= get_uart_command(); + } while (rc == 0); + + return rc; +} + +/* Initializes MCU peripherals. */ +static int setup_mcu() +{ + int rc = 0; + + rc |= si_board_init(); + + /* Configure UART for log */ + rc |= si_config_uart_for_print(SI_UART_ID_FTDI, INV_MSG_LEVEL_DEBUG); + + /* Configure GPIO to call `int_cb` when INT1 fires. */ + rc |= si_init_gpio_int(SI_GPIO_INT1, int_cb); + + /* Init timer peripheral for sleep and get_time */ + rc |= si_init_timers(); + + /* Initialize serial interface between MCU and IMU */ + rc |= si_io_imu_init(SERIF_TYPE); + + return rc; +} + +static int setup_imu() +{ + int rc = 0; + inv_imu_serif_t imu_serif; + uint8_t whoami; + inv_imu_int1_pin_config_t int1_pin_config; + + /* Initialize serial interface between MCU and IMU */ + imu_serif.context = 0; /* no need */ + imu_serif.read_reg = si_io_imu_read_reg; + imu_serif.write_reg = si_io_imu_write_reg; + imu_serif.max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */ + imu_serif.max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */ + imu_serif.serif_type = SERIF_TYPE; + + /* Init device */ + rc |= inv_imu_init(&imu_dev, &imu_serif, sensor_event_cb); + SI_CHECK_RC(rc); + +#if SERIF_TYPE == UI_SPI4 + /* Configure slew-rate to 19 ns (required when using EVB) */ + rc |= inv_imu_set_spi_slew_rate(&imu_dev, DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS); + SI_CHECK_RC(rc); +#endif + + /* Check WHOAMI */ + rc |= inv_imu_get_who_am_i(&imu_dev, &whoami); + SI_CHECK_RC(rc); + if (whoami != INV_IMU_WHOAMI) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Erroneous WHOAMI value."); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Read 0x%02x", whoami); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Expected 0x%02x", INV_IMU_WHOAMI); + return INV_ERROR; + } + + /* + * Configure interrupts pins + * - Polarity High + * - Pulse mode + * - Push-Pull drive + */ + int1_pin_config.int_polarity = INT_CONFIG_INT1_POLARITY_HIGH; + int1_pin_config.int_mode = INT_CONFIG_INT1_MODE_PULSED; + int1_pin_config.int_drive = INT_CONFIG_INT1_DRIVE_CIRCUIT_PP; + rc |= inv_imu_set_pin_config_int1(&imu_dev, &int1_pin_config); + + /* Configure FSR (doesn't apply if FIFO is used in highres mode) */ + rc |= inv_imu_set_accel_fsr(&imu_dev, ACCEL_CONFIG0_FS_SEL_4g); + rc |= inv_imu_set_gyro_fsr(&imu_dev, GYRO_CONFIG0_FS_SEL_2000dps); + + /* Configure ODR */ + rc |= inv_imu_set_accel_frequency(&imu_dev, ACCEL_CONFIG0_ODR_50_HZ); + rc |= inv_imu_set_gyro_frequency(&imu_dev, GYRO_CONFIG0_ODR_50_HZ); + + /* Variable configuration */ + rc |= configure_fifo(); + rc |= configure_hires(); + rc |= configure_power_mode(); + + return rc; +} + +static int configure_fifo() +{ + int rc = 0; + inv_imu_interrupt_parameter_t int1_config = { (inv_imu_interrupt_value)0 }; + + rc |= inv_imu_configure_fifo(&imu_dev, fifo_en ? INV_IMU_FIFO_ENABLED : INV_IMU_FIFO_DISABLED); + + /* Configure interrupts sources */ + if (fifo_en) + int1_config.INV_FIFO_THS = INV_IMU_ENABLE; + else + int1_config.INV_UI_DRDY = INV_IMU_ENABLE; + rc |= inv_imu_set_config_int1(&imu_dev, &int1_config); + + return rc; +} + +static int configure_hires() +{ + int rc = 0; + + if (hires_en) + rc |= inv_imu_enable_high_resolution_fifo(&imu_dev); + else + rc |= inv_imu_disable_high_resolution_fifo(&imu_dev); + + return rc; +} + +static int configure_power_mode() +{ + int rc = 0; + + if (use_ln) + rc |= inv_imu_enable_accel_low_noise_mode(&imu_dev); + else + rc |= inv_imu_enable_accel_low_power_mode(&imu_dev); + + rc |= inv_imu_enable_gyro_low_noise_mode(&imu_dev); + + return rc; +} + +/* IMU interrupt handler. */ +static void int_cb(void *context, unsigned int int_num) +{ + (void)context; + + if (int_num == SI_GPIO_INT1) { + int1_timestamp = si_get_time_us(); + int1_flag = 1; + } +} + +static void sensor_event_cb(inv_imu_sensor_event_t *event) +{ + uint64_t int_timestamp = 0; + int accel_raw[3]; + int gyro_raw[3]; + char accel_str[40]; + char gyro_str[40]; + char temp_str[20]; + char fifo_time_str[30]; + + si_disable_irq(); + int_timestamp = int1_timestamp; + si_enable_irq(); + + if (fifo_en) { + uint64_t fifo_timestamp; + static uint64_t last_fifo_timestamp = 0; + static uint32_t rollover_num = 0; + + /* Handle rollover */ + if (last_fifo_timestamp > event->timestamp_fsync) + rollover_num++; + last_fifo_timestamp = event->timestamp_fsync; + + /* Compute timestamp in us */ + if (last_fifo_timestamp == 0 && rollover_num == 0) { + fifo_timestamp = int_timestamp; + } else { + fifo_timestamp = event->timestamp_fsync + rollover_num * UINT16_MAX; + fifo_timestamp *= inv_imu_get_timestamp_resolution_us(&imu_dev); + } + + snprintf(fifo_time_str, 30, "FIFO Time: %5llu us", fifo_timestamp); + + if (hires_en) { + accel_raw[0] = ((int32_t)event->accel[0] << 4) | event->accel_high_res[0]; + accel_raw[1] = ((int32_t)event->accel[1] << 4) | event->accel_high_res[1]; + accel_raw[2] = ((int32_t)event->accel[2] << 4) | event->accel_high_res[2]; + gyro_raw[0] = ((int32_t)event->gyro[0] << 4) | event->gyro_high_res[0]; + gyro_raw[1] = ((int32_t)event->gyro[1] << 4) | event->gyro_high_res[1]; + gyro_raw[2] = ((int32_t)event->gyro[2] << 4) | event->gyro_high_res[2]; + } else { + accel_raw[0] = event->accel[0]; + accel_raw[1] = event->accel[1]; + accel_raw[2] = event->accel[2]; + gyro_raw[0] = event->gyro[0]; + gyro_raw[1] = event->gyro[1]; + gyro_raw[2] = event->gyro[2]; + } + } else { + /* No timestamp info if not using FIFO */ + snprintf(fifo_time_str, 30, " "); + + accel_raw[0] = event->accel[0]; + accel_raw[1] = event->accel[1]; + accel_raw[2] = event->accel[2]; + + gyro_raw[0] = event->gyro[0]; + gyro_raw[1] = event->gyro[1]; + gyro_raw[2] = event->gyro[2]; + + /* Force sensor_mask so it gets displayed below */ + event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE); + event->sensor_mask |= (1 << INV_SENSOR_ACCEL); + event->sensor_mask |= (1 << INV_SENSOR_GYRO); + } + + if (print_si) { + float accel_g[3]; + float gyro_dps[3]; + float temp_degc; +#if INV_IMU_HFSR_SUPPORTED + uint16_t accel_fsr_g = fifo_en && hires_en ? 32 : 4; + uint16_t gyro_fsr_dps = fifo_en && hires_en ? 4000 : 2000; +#else + uint16_t accel_fsr_g = fifo_en && hires_en ? 16 : 4; + uint16_t gyro_fsr_dps = 2000; +#endif + int max_lsb = fifo_en && hires_en ? 524287 : 32768; + + /* Convert raw data into scaled data in g and dps */ + accel_g[0] = (float)(accel_raw[0] * accel_fsr_g) / (float)max_lsb; + accel_g[1] = (float)(accel_raw[1] * accel_fsr_g) / (float)max_lsb; + accel_g[2] = (float)(accel_raw[2] * accel_fsr_g) / (float)max_lsb; + gyro_dps[0] = (float)(gyro_raw[0] * gyro_fsr_dps) / (float)max_lsb; + gyro_dps[1] = (float)(gyro_raw[1] * gyro_fsr_dps) / (float)max_lsb; + gyro_dps[2] = (float)(gyro_raw[2] * gyro_fsr_dps) / (float)max_lsb; + if (hires_en || !fifo_en) + temp_degc = 25 + ((float)event->temperature / 128); + else + temp_degc = 25 + ((float)event->temperature / 2); + + /* Generate strings and print for SI */ + if (event->sensor_mask & (1 << INV_SENSOR_ACCEL)) + snprintf(accel_str, 40, "Accel:% 8.2f % 8.2f % 8.2f g", accel_g[0], accel_g[1], + accel_g[2]); + else + snprintf(accel_str, 40, "Accel: - - - "); + + if (event->sensor_mask & (1 << INV_SENSOR_GYRO)) + snprintf(gyro_str, 40, "Gyro:% 8.2f % 8.2f % 8.2f dps", gyro_dps[0], gyro_dps[1], + gyro_dps[2]); + else + snprintf(gyro_str, 40, "Gyro: - - - "); + + snprintf(temp_str, 20, "Temp: % 4.2f degC", temp_degc); + + INV_MSG(INV_MSG_LEVEL_INFO, "SI %10llu us %s %s %s %s", int_timestamp, accel_str, + gyro_str, temp_str, fifo_time_str); + } + + if (print_lsb) { + /* Generate strings and print for LSB */ + if (event->sensor_mask & (1 << INV_SENSOR_ACCEL)) + snprintf(accel_str, 40, "Accel:% 8d % 8d % 8d", accel_raw[0], accel_raw[1], + accel_raw[2]); + else + snprintf(accel_str, 40, "Accel: - - -"); + + if (event->sensor_mask & (1 << INV_SENSOR_GYRO)) + snprintf(gyro_str, 40, "Gyro:% 8d % 8d % 8d", gyro_raw[0], gyro_raw[1], gyro_raw[2]); + else + snprintf(gyro_str, 40, "Gyro: - - -"); + + snprintf(temp_str, 20, "Temp: % 6d", event->temperature); + + INV_MSG(INV_MSG_LEVEL_INFO, "LSB %10llu us %s %s %s %s", int_timestamp, + accel_str, gyro_str, temp_str, fifo_time_str); + } +} + +/* Get command from user through UART */ +static int get_uart_command() +{ + int rc = 0; + char cmd = 0; + + rc |= si_get_uart_command(SI_UART_ID_FTDI, &cmd); + SI_CHECK_RC(rc); + + switch (cmd) { + case 's': /* Print SI */ + print_si = !print_si; + INV_MSG(INV_MSG_LEVEL_INFO, "%s SI print.", print_si ? "Enabling" : "Disabling"); + break; + case 'l': /* Print LSB */ + print_lsb = !print_lsb; + INV_MSG(INV_MSG_LEVEL_INFO, "%s LSB print.", print_lsb ? "Enabling" : "Disabling"); + break; + case 'f': /* Use FIFO or sensor register */ + fifo_en = !fifo_en; + INV_MSG(INV_MSG_LEVEL_INFO, "%s FIFO.", fifo_en ? "Enabling" : "Disabling"); + rc |= configure_fifo(); + rc |= configure_hires(); + break; + case 'i': + hires_en = !hires_en; + INV_MSG(INV_MSG_LEVEL_INFO, "%s highres mode.", hires_en ? "Enabling" : "Disabling"); + if (!fifo_en) + INV_MSG(INV_MSG_LEVEL_INFO, "Warning: highres mode only apply if FIFO is enabled."); + rc |= configure_hires(); + break; + case 'p': + use_ln = !use_ln; + INV_MSG(INV_MSG_LEVEL_INFO, "%s selected.", use_ln ? "Low-noise" : "Low-power"); + rc |= configure_power_mode(); + break; + case 'c': + rc |= print_current_config(); + break; + case 'h': + case 'H': + rc |= print_help(); + break; + case 0: + break; /* No command received */ + default: + INV_MSG(INV_MSG_LEVEL_INFO, "Unknown command : %c", cmd); + rc |= print_help(); + break; + } + + return rc; +} + +/* Help for UART command interface */ +static int print_help() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Help"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 's' : Toggle print data in SI"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'l' : Toggle print data in LSB"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'f' : Toggle FIFO usage"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'i' : Toggle Highres mode"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'p' : Toggle power mode (low-noise, low-power)"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'c' : Print current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'h' : Print this helper"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Print current sample configuration */ +static int print_current_config() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# FIFO: %s", fifo_en ? "Enabled" : "Disabled"); + if (fifo_en) + INV_MSG(INV_MSG_LEVEL_INFO, "# Highres mode: %s", hires_en ? "Enabled" : "Disabled"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Power mode: %s", use_ln ? "Low-noise" : "Low-power"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Data in SI are %s", print_si ? "printed" : "hidden"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Data in LSB are %s", print_lsb ? "printed" : "hidden"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Get time implementation for IMU driver */ +uint64_t inv_imu_get_time_us(void) +{ + return si_get_time_us(); +} + +/* Sleep implementation for IMU driver */ +void inv_imu_sleep_us(uint32_t us) +{ + si_sleep_us(us); +} diff --git a/examples/selftest/README.md b/examples/selftest/README.md new file mode 100644 index 0000000..76eef13 --- /dev/null +++ b/examples/selftest/README.md @@ -0,0 +1,27 @@ +# selftest + +This application demonstrates how to execute the Self-test from the DMP. + +Selftest procedure is run in a loop. It would only stop in case of failure. + +**Warning:** The board needs to be static during the execution of the Self-Test. + +## Terminal output + +``` +[I] ### +[I] ### Example Self-Test +[I] ### +[I] [0] Accel self-test OK +[I] [0] Gyro self-test OK +[I] +[I] [1] Accel self-test OK +[I] [1] Gyro self-test OK +[I] +[I] [2] Accel self-test OK +[I] [2] Gyro self-test OK +[I] +[I] [3] Accel self-test OK +[I] [3] Gyro self-test OK +``` + diff --git a/examples/selftest/selftest.c b/examples/selftest/selftest.c new file mode 100644 index 0000000..fdcdfa0 --- /dev/null +++ b/examples/selftest/selftest.c @@ -0,0 +1,173 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/* Driver */ +#include "imu/inv_imu_driver.h" +#include "imu/inv_imu_selftest.h" + +/* Board drivers */ +#include "system_interface.h" + +#include /* NULL */ + +/* + * This example showcases how to run self-test with DMP. + */ + +/* + * Select communication link between SmartMotion and IMU. + * SPI4: `UI_SPI4` + * I2C: `UI_I2C` + */ +#define SERIF_TYPE UI_SPI4 + +/* Static variables */ +static inv_imu_device_t imu_dev; /* Driver structure */ + +/* Static functions definition */ +static int setup_mcu(); +static int setup_imu(); + +/* Main function implementation */ +int main(void) +{ + int rc = 0; + + rc |= setup_mcu(); + SI_CHECK_RC(rc); + + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + INV_MSG(INV_MSG_LEVEL_INFO, "### Example Self-Test"); + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + + rc |= setup_imu(); + SI_CHECK_RC(rc); + + do { + int rc = 0; + static int iter = 0; + inv_imu_selftest_output_t out; + inv_imu_selftest_parameters_t params; + + rc |= inv_imu_init_selftest_parameters_struct(&imu_dev, ¶ms); + /* Update `params` if needed here */ + rc |= inv_imu_run_selftest(&imu_dev, params, &out); + SI_CHECK_RC(rc); + + /* Print self-test status */ + INV_MSG(INV_MSG_LEVEL_INFO, "[%u] Accel self-test %s", iter, + out.accel_status == 1 ? "OK" : "KO"); + if (out.accel_status != 1) { + INV_MSG(INV_MSG_LEVEL_VERBOSE, " - Accel X: %s", out.ax_status == 1 ? "OK" : "KO"); + INV_MSG(INV_MSG_LEVEL_VERBOSE, " - Accel Y: %s", out.ay_status == 1 ? "OK" : "KO"); + INV_MSG(INV_MSG_LEVEL_VERBOSE, " - Accel Z: %s", out.az_status == 1 ? "OK" : "KO"); + rc |= INV_ERROR; + } + +#if INV_IMU_IS_GYRO_SUPPORTED + INV_MSG(INV_MSG_LEVEL_INFO, "[%u] Gyro self-test %s", iter, + out.gyro_status == 1 ? "OK" : "KO"); + if (out.gyro_status != 1) { + INV_MSG(INV_MSG_LEVEL_VERBOSE, " - Gyro X: %s", out.gx_status == 1 ? "OK" : "KO"); + INV_MSG(INV_MSG_LEVEL_VERBOSE, " - Gyro Y: %s", out.gy_status == 1 ? "OK" : "KO"); + INV_MSG(INV_MSG_LEVEL_VERBOSE, " - Gyro Z: %s", out.gz_status == 1 ? "OK" : "KO"); + rc |= INV_ERROR; + } + + /* Check incomplete state */ + if (out.gyro_status & 0x2) { + INV_MSG(INV_MSG_LEVEL_ERROR, "[%u] Gyro self-test are incomplete.", iter); + rc |= INV_ERROR; + } +#endif + + iter++; + + /* Print empty line to ease readability */ + INV_MSG(INV_MSG_LEVEL_INFO, ""); + } while (rc == 0); + + return rc; +} + +/* Initializes MCU peripherals. */ +static int setup_mcu() +{ + int rc = 0; + + rc |= si_board_init(); + + /* Configure UART for log */ + rc |= si_config_uart_for_print(SI_UART_ID_FTDI, INV_MSG_LEVEL_DEBUG); + + /* Init timer peripheral for sleep and get_time */ + rc |= si_init_timers(); + + /* Initialize serial interface between MCU and IMU */ + rc |= si_io_imu_init(SERIF_TYPE); + + return rc; +} + +static int setup_imu() +{ + int rc = 0; + inv_imu_serif_t imu_serif; + uint8_t whoami; + + /* Initialize serial interface between MCU and IMU */ + imu_serif.context = 0; /* no need */ + imu_serif.read_reg = si_io_imu_read_reg; + imu_serif.write_reg = si_io_imu_write_reg; + imu_serif.max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */ + imu_serif.max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */ + imu_serif.serif_type = SERIF_TYPE; + + /* Init device */ + rc |= inv_imu_init(&imu_dev, &imu_serif, NULL); + SI_CHECK_RC(rc); + +#if SERIF_TYPE == UI_SPI4 + /* Configure slew-rate to 19 ns (required when using EVB) */ + rc |= inv_imu_set_spi_slew_rate(&imu_dev, DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS); + SI_CHECK_RC(rc); +#endif + + /* Check WHOAMI */ + rc |= inv_imu_get_who_am_i(&imu_dev, &whoami); + SI_CHECK_RC(rc); + if (whoami != INV_IMU_WHOAMI) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Erroneous WHOAMI value."); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Read 0x%02x", whoami); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Expected 0x%02x", INV_IMU_WHOAMI); + return INV_ERROR; + } + + return rc; +} + +/* Get time implementation for IMU driver */ +uint64_t inv_imu_get_time_us(void) +{ + return si_get_time_us(); +} + +/* Sleep implementation for IMU driver */ +void inv_imu_sleep_us(uint32_t us) +{ + si_sleep_us(us); +} diff --git a/examples/smd/README.md b/examples/smd/README.md new file mode 100644 index 0000000..1cc8820 --- /dev/null +++ b/examples/smd/README.md @@ -0,0 +1,34 @@ +# smd + +This application demonstrates how to configure and use SMD feature. The algorithm detects when the user has moved significantly. + +## Command interface + +This application allows the following command to be sent through UART: +* `p`: to toggle Power-Save mode usage (enable/disable, disabled by default) +* `c`: to print current configuration. +* `h`: to print helps screen. + +## Terminal output + +### Data format + +Data are printed on the terminal as follow: + +``` + us SMD +``` + +With: +* ``: Time in microsecond read from MCU clock when latest INT1 was fired. + +### Example of output + +``` +[I] ### +[I] ### Example SMD +[I] ### +[I] 6500645 us SMD +[I] 14515148 us SMD +``` + diff --git a/examples/smd/smd.c b/examples/smd/smd.c new file mode 100644 index 0000000..65efcfc --- /dev/null +++ b/examples/smd/smd.c @@ -0,0 +1,316 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/* Driver */ +#include "imu/inv_imu_driver.h" +#include "imu/inv_imu_apex.h" + +/* Board drivers */ +#include "system_interface.h" + +#include /* NULL */ + +/* + * This example showcases how to configure and use SMD. + */ + +/* + * Select communication link between SmartMotion and IMU. + * SPI4: `UI_SPI4` + * I2C: `UI_I2C` + */ +#define SERIF_TYPE UI_SPI4 + +/* WOM threshold */ +#if INV_IMU_HFSR_SUPPORTED +#define WOM_THRESHOLD 7 /* ~50 mg */ +#else +#define WOM_THRESHOLD 13 /* ~50 mg */ +#endif + +/* Static variables */ +static inv_imu_device_t imu_dev; /* Driver structure */ +static volatile int int1_flag; /* Flag set when INT1 is received */ +static volatile uint64_t int1_timestamp; /* Store timestamp when int from IMU fires */ + +/* Static variables for command interface */ +static uint8_t use_power_save; /* Indicates if power save mode is being used */ + +/* Static functions definition */ +static int setup_mcu(); +static int setup_imu(); +static int configure_smd(); +static void int_cb(void *context, unsigned int int_num); +static int get_uart_command(); +static int print_help(); +static int print_current_config(); + +/* Main function implementation */ +int main(void) +{ + int rc = 0; + + rc |= setup_mcu(); + SI_CHECK_RC(rc); + + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + INV_MSG(INV_MSG_LEVEL_INFO, "### Example SMD"); + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + + /* Reset commands interface states */ + use_power_save = 0; + + rc |= setup_imu(); + SI_CHECK_RC(rc); + + /* Reset timestamp and interrupt flag */ + int1_flag = 0; + int1_timestamp = 0; + + do { + /* Poll device for data */ + if (int1_flag) { + uint8_t int_status2; + uint64_t timestamp; + + si_disable_irq(); + /* Clear interrupt flag */ + int1_flag = 0; + /* Retrieve timestamp */ + timestamp = int1_timestamp; + si_enable_irq(); + + /* Read SMD interrupt status */ + rc |= inv_imu_read_reg(&imu_dev, INT_STATUS2, 1, &int_status2); + SI_CHECK_RC(rc); + + if (int_status2 & INT_STATUS2_SMD_INT_MASK) + INV_MSG(INV_MSG_LEVEL_INFO, " %10llu us SMD", timestamp); + } + + rc |= get_uart_command(); + } while (rc == 0); + + return rc; +} + +/* Initializes MCU peripherals. */ +static int setup_mcu() +{ + int rc = 0; + + rc |= si_board_init(); + + /* Configure UART for log */ + rc |= si_config_uart_for_print(SI_UART_ID_FTDI, INV_MSG_LEVEL_DEBUG); + + /* Configure GPIO to call `int_cb` when INT1 fires. */ + rc |= si_init_gpio_int(SI_GPIO_INT1, int_cb); + + /* Init timer peripheral for sleep and get_time */ + rc |= si_init_timers(); + + /* Initialize serial interface between MCU and IMU */ + rc |= si_io_imu_init(SERIF_TYPE); + + return rc; +} + +static int setup_imu() +{ + int rc = 0; + inv_imu_serif_t imu_serif; + uint8_t whoami; + inv_imu_int1_pin_config_t int1_pin_config; + inv_imu_interrupt_parameter_t int1_config = { (inv_imu_interrupt_value)0 }; + + /* Initialize serial interface between MCU and IMU */ + imu_serif.context = 0; /* no need */ + imu_serif.read_reg = si_io_imu_read_reg; + imu_serif.write_reg = si_io_imu_write_reg; + imu_serif.max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */ + imu_serif.max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */ + imu_serif.serif_type = SERIF_TYPE; + + /* Init device */ + rc |= inv_imu_init(&imu_dev, &imu_serif, NULL); + SI_CHECK_RC(rc); + +#if SERIF_TYPE == UI_SPI4 + /* Configure slew-rate to 19 ns (required when using EVB) */ + rc |= inv_imu_set_spi_slew_rate(&imu_dev, DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS); + SI_CHECK_RC(rc); +#endif + + /* Check WHOAMI */ + rc |= inv_imu_get_who_am_i(&imu_dev, &whoami); + SI_CHECK_RC(rc); + if (whoami != INV_IMU_WHOAMI) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Erroneous WHOAMI value."); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Read 0x%02x", whoami); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Expected 0x%02x", INV_IMU_WHOAMI); + return INV_ERROR; + } + + /* + * Configure interrupts pins + * - Polarity High + * - Pulse mode + * - Push-Pull drive + */ + int1_pin_config.int_polarity = INT_CONFIG_INT1_POLARITY_HIGH; + int1_pin_config.int_mode = INT_CONFIG_INT1_MODE_PULSED; + int1_pin_config.int_drive = INT_CONFIG_INT1_DRIVE_CIRCUIT_PP; + rc |= inv_imu_set_pin_config_int1(&imu_dev, &int1_pin_config); + + /* Configure interrupts sources */ + int1_config.INV_SMD = INV_IMU_ENABLE; + rc |= inv_imu_set_config_int1(&imu_dev, &int1_config); + + /* Disabling FIFO usage to optimize power consumption */ + rc |= inv_imu_configure_fifo(&imu_dev, INV_IMU_FIFO_DISABLED); + + /* Set 2X averaging to minimize power consumption */ + rc |= inv_imu_set_accel_lp_avg(&imu_dev, ACCEL_CONFIG1_ACCEL_FILT_AVG_2); + + /* Enable accel in LP mode */ + rc |= inv_imu_enable_accel_low_power_mode(&imu_dev); + + rc |= configure_smd(); + + return rc; +} + +static int configure_smd() +{ + int rc = 0; + inv_imu_apex_parameters_t apex_inputs; + + /* Disable pedometer and SMD before configuring it */ + rc |= inv_imu_apex_disable_pedometer(&imu_dev); + rc |= inv_imu_apex_disable_smd(&imu_dev); + + rc |= inv_imu_set_accel_frequency(&imu_dev, ACCEL_CONFIG0_ODR_50_HZ); + rc |= inv_imu_apex_set_frequency(&imu_dev, APEX_CONFIG1_DMP_ODR_50Hz); + + /* Set APEX parameters */ + rc |= inv_imu_apex_init_parameters_struct(&imu_dev, &apex_inputs); + /* Parameters can be modified here if needed */ + apex_inputs.power_save = + use_power_save ? APEX_CONFIG0_DMP_POWER_SAVE_EN : APEX_CONFIG0_DMP_POWER_SAVE_DIS; + rc |= inv_imu_apex_configure_parameters(&imu_dev, &apex_inputs); + + /* If POWER_SAVE mode is enabled, WOM has to be enabled */ + if (use_power_save) { + /* Configure and enable WOM to wake-up the DMP once it goes in power save mode */ + rc |= inv_imu_configure_wom(&imu_dev, WOM_THRESHOLD, WOM_THRESHOLD, WOM_THRESHOLD, + WOM_CONFIG_WOM_INT_MODE_ANDED, WOM_CONFIG_WOM_INT_DUR_1_SMPL); + rc |= inv_imu_enable_wom(&imu_dev); + } + + /* Enable SMD (and Pedometer as SMD uses it) */ + rc |= inv_imu_apex_enable_pedometer(&imu_dev); + rc |= inv_imu_apex_enable_smd(&imu_dev); + + return rc; +} + +/* IMU interrupt handler. */ +static void int_cb(void *context, unsigned int int_num) +{ + (void)context; + + if (int_num == SI_GPIO_INT1) { + int1_timestamp = si_get_time_us(); + int1_flag = 1; + } +} + +/* Get command from user through UART */ +static int get_uart_command() +{ + int rc = 0; + char cmd = 0; + + rc |= si_get_uart_command(SI_UART_ID_FTDI, &cmd); + SI_CHECK_RC(rc); + + switch (cmd) { + case 'p': /* Toggle power-save mode usage */ + use_power_save = !use_power_save; + INV_MSG(INV_MSG_LEVEL_INFO, "Power-Save mode: %s", use_power_save ? "Enabled" : "Disabled"); + rc |= configure_smd(); + break; + case 'c': + rc |= print_current_config(); + break; + case 'h': + case 'H': + rc |= print_help(); + break; + case 0: + break; /* No command received */ + default: + INV_MSG(INV_MSG_LEVEL_INFO, "Unknown command : %c", cmd); + rc |= print_help(); + break; + } + + return rc; +} + +/* Help for UART command interface */ +static int print_help() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Help"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'p' : Toggle Power-Save mode usage"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'c' : Print current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'h' : Print this helper"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Print current sample configuration */ +static int print_current_config() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Power-Save: %s", use_power_save ? "Enabled" : "Disabled"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Get time implementation for IMU driver */ +uint64_t inv_imu_get_time_us(void) +{ + return si_get_time_us(); +} + +/* Sleep implementation for IMU driver */ +void inv_imu_sleep_us(uint32_t us) +{ + si_sleep_us(us); +} diff --git a/examples/system_interface.c b/examples/system_interface.c new file mode 100644 index 0000000..867b9fe --- /dev/null +++ b/examples/system_interface.c @@ -0,0 +1,202 @@ +/* + * + * Copyright (c) [2020] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#include "system_interface.h" + +/* Standard includes */ +#include +#include +#include + +/* IMU drivers */ +#include "imu/inv_imu_defs.h" /* For error codes */ + +/* + * Board + */ +int si_board_init() +{ + return 0; +} + +/* + * UART + */ +int si_config_uart_for_print(inv_uart_num_t id, int level) +{ + (void)id; + (void)level; + return 0; +} + +int si_config_uart_for_bin(inv_uart_num_t id) +{ + (void)id; + return 0; +} + +int si_get_uart_command(inv_uart_num_t id, char *cmd) +{ + (void)id; + (void)cmd; + return 0; +} + +/* + * I/O for IMU device + */ +int si_io_imu_init(uint32_t serif_type) +{ + (void)serif_type; + return 0; +} + +int si_io_imu_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t *buf, uint32_t len) +{ + (void)serif; + (void)reg; + (void)buf; + (void)len; + return 0; +} + +int si_io_imu_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t *buf, uint32_t len) +{ + (void)serif; + (void)reg; + (void)buf; + (void)len; + return 0; +} + +/* + * Timers + */ +int si_init_timers() +{ + return 0; +} + +void si_sleep_us(uint32_t us) +{ + (void)us; +} + +uint64_t si_get_time_us() +{ + return 0; +} + +int si_start_periodic_timer(const uint32_t period_us, void callback(void *context), int *ch) +{ + (void)period_us; + (void)ch; + return 0; +} + +int si_stop_periodic_timer(int ch) +{ + (void)ch; + return 0; +} + +/* + * GPIO + */ +int si_init_gpio_int(unsigned int_num, void (*int_cb)(void *context, unsigned int_num)) +{ + (void)int_num; + (void)int_cb; + return 0; +} + +int si_start_gpio_fsync(uint32_t freq, void (*fsync_timer_cb)(void *context)) +{ + (void)freq; + (void)fsync_timer_cb; + return 0; +} + +int si_stop_gpio_fsync() +{ + return 0; +} + +void si_toggle_gpio_fsync() +{ +} + +/* + * Common + */ + +void si_disable_irq() +{ +} + +void si_enable_irq() +{ +} + +/* + * Error codes + */ +int si_print_error_if_any(int rc) +{ + if (rc < 0) { + switch (rc) { + case INV_ERROR: + INV_MSG(INV_MSG_LEVEL_ERROR, "Unspecified error (%d)", rc); + break; + case INV_ERROR_NIMPL: + INV_MSG(INV_MSG_LEVEL_ERROR, "Function not implemented for given arguments (%d)", rc); + break; + case INV_ERROR_TRANSPORT: + INV_MSG(INV_MSG_LEVEL_ERROR, "Error occurred at transport level (%d)", rc); + break; + case INV_ERROR_TIMEOUT: + INV_MSG(INV_MSG_LEVEL_ERROR, "Action did not complete in the expected time window (%d)", + rc); + break; + case INV_ERROR_SIZE: + INV_MSG(INV_MSG_LEVEL_ERROR, "Invalid argument's size provided (%d)", rc); + break; + case INV_ERROR_BAD_ARG: + INV_MSG(INV_MSG_LEVEL_ERROR, "Invalid argument provided (%d)", rc); + break; + case INV_ERROR_UNEXPECTED: + INV_MSG(INV_MSG_LEVEL_ERROR, "Something unexpected happened (%d)", rc); + break; + default: + INV_MSG(INV_MSG_LEVEL_ERROR, "Unknown error (%d)", rc); + break; + } + + return rc; + } + + return 0; +} + +/* + * Message + */ +void inv_msg(int level, const char *str, ...) +{ + (void)level; + (void)str; +} diff --git a/examples/system_interface.h b/examples/system_interface.h new file mode 100644 index 0000000..a881d3a --- /dev/null +++ b/examples/system_interface.h @@ -0,0 +1,216 @@ +/* + * + * Copyright (c) [2020] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#ifndef _SYSTEM_INTERFACE_H_ +#define _SYSTEM_INTERFACE_H_ + +#include "imu/inv_imu_transport.h" + +/* + * Board + */ + +/** @brief Initialize board. + * @return 0 on success, negative value on error. + */ +int si_board_init(); + +/* + * UART + */ + +#define SI_UART_ID_FTDI 0 +#define SI_UART_ID_EDBG 1 + +typedef uint32_t inv_uart_num_t; + +/** @brief Configure UART to transmit characters. + * Baudrate: 921600 + * Flow control: None + * @param[in] id UART to configure. + * @param[in] level Trace level (enum inv_msg_level). + * @return 0 on success, negative value on error. + */ +int si_config_uart_for_print(inv_uart_num_t id, int level); + +/** @brief Configure UART to transmit binary data. + * Baudrate: 2000000 + * Flow control: RTS/CTS + * @param[in] id UART to configure. + * @return 0 on success, negative value on error. + */ +int si_config_uart_for_bin(inv_uart_num_t id); + +/** @brief Get user command from UART. + * @param[in] id UART to read from. + * @param[out] cmd User command if one has been sent, 0 otherwise. + * @return 0 on success, negative value on error. + */ +int si_get_uart_command(inv_uart_num_t id, char *cmd); + +/* + * I/O for IMU device + */ + +/** @brief Configure I/O for IMU device. + * @param[in] serif_type Serial interface type to be used. + * @return 0 on success, negative value on error. + */ +int si_io_imu_init(uint32_t serif_type); + +/** @brief Read register(s) implementation for IMU device. + * @param[in] serif Serial interface. + * @param[in] reg Register address to be read. + * @param[out] buf Output data from the register. + * @param[in] len Number of byte to be read. + * @return 0 on success, negative value on error. + */ +int si_io_imu_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t *buf, uint32_t len); + +/** @brief Write register(s) implementation for IMU device. + * @param[in] serif Serial interface. + * @param[in] reg Register address to be written. + * @param[in] buf Input data to write. + * @param[in] len Number of byte to be written. + * @return 0 on success, negative value on error. + */ +int si_io_imu_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t *buf, uint32_t len); + +/* + * Timers + */ + +/** @brief Initialize timers IP. + * @return 0 on success, negative value on error. + */ +int si_init_timers(); + +/** @brief Sleep function implementation. + * @param[in] us Time to sleep in microseconds. + */ +void si_sleep_us(uint32_t us); + +/** @brief Get current time function implementation. + * @return The current time in us or 0 if get_time_us pointer is null. + */ +uint64_t si_get_time_us(); + +/** @brief Start a periodic timer. + * @param[in] period_us Period of the timer in us. + * @param[in] callback Function to be called when period expires. + * @param[out] ch Channel number of the timer. + * @return 0 on success, negative value on error. + */ +int si_start_periodic_timer(const uint32_t period_us, void callback(void *context), int *ch); + +/** @brief Stop a periodic timer. + * @param[in] ch Channel number of the timer. + * @return 0 on success, negative value on error. + */ +int si_stop_periodic_timer(int ch); + +/* + * GPIO + */ + +#define SI_GPIO_INT1 0 +#define SI_GPIO_INT2 1 + +/** @brief Initializes GPIO module to trigger callback when interrupt from IMU fires. + * @param[in] int_num Interrupt pin (`SI_GPIO_INT1` or `SI_GPIO_INT2`). + * @param[in] int_cb Callback to be called when interrupt fires. + * @return 0 on success, negative value on error. + */ +int si_init_gpio_int(unsigned int_num, void (*int_cb)(void *context, unsigned int_num)); + +/** @brief Initializes GPIO module to initializes FSYNC output pin and trigger callback at + * a given frequency to allow user to toggle FSYNC pin at regular interval to emulate + * camera module. + * @param[in] freq Frequency at which fsync_timer_cb should be called (0 if no timer is to be enabled) + * @param[in] fsync_timer_cb Callback to be called when FSYNC timer expires. + * @return 0 on success, negative value on error. + */ +int si_start_gpio_fsync(uint32_t freq, void (*fsync_timer_cb)(void *context)); + +/** @brief Stop FSYNC module. + * @return 0 on success, negative value on error. + */ +int si_stop_gpio_fsync(); + +/** @brief Toggles GPIO FSYNC output pin. + */ +void si_toggle_gpio_fsync(); + +/* + * Common + */ + +/** @brief Disable core interrupts. + */ +void si_disable_irq(); + +/** @brief Enable core interrupts. + */ +void si_enable_irq(); + +/* + * Error codes + */ + +/** @brief In case of error, print message, file and line and block program execution. + * @param[in] rc Error code (rc). + */ +#define SI_CHECK_RC(rc) \ + do { \ + if (si_print_error_if_any(rc)) { \ + INV_MSG(INV_MSG_LEVEL_ERROR, "At %s (line %d)", __FILE__, __LINE__); \ + si_sleep_us(100000); \ + while (1) \ + ; \ + } \ + } while (0) + +/** @brief Check return code from driver and print message if it is an error. + * @param[in] rc Return code from the driver. + * @return Error code (rc). + */ +int si_print_error_if_any(int rc); + +/* + * Message + */ + +enum inv_msg_level { + INV_MSG_LEVEL_OFF = 0, + INV_MSG_LEVEL_ERROR, + INV_MSG_LEVEL_WARNING, + INV_MSG_LEVEL_INFO, + INV_MSG_LEVEL_VERBOSE, + INV_MSG_LEVEL_DEBUG, + INV_MSG_LEVEL_MAX +}; +#define INV_MSG(level, ...) inv_msg(level, __VA_ARGS__) + +/** @brief Display a message. + * @param[in] level Level for the message + * @param[in] str Message string + * @param[in] ... Optional arguments + * @return none + */ +void inv_msg(int level, const char *str, ...); + +#endif /* !_SYSTEM_INTERFACE_H_ */ diff --git a/examples/tilt/README.md b/examples/tilt/README.md new file mode 100644 index 0000000..52b92b7 --- /dev/null +++ b/examples/tilt/README.md @@ -0,0 +1,37 @@ +# tilt + +This application demonstrates how to configure and use tilt feature. + +Once the example is running, tilting the board with an angle of 30 degrees or more will generate a tilt. By default, a tilt event is generated when the position is held for 4 seconds. + +## Command interface + +This application allows the following command to be sent through UART: +* `a`: to toggle Tilt configuration (High-Performance/Low-Power, High-Performance by default) +* `p`: to toggle Power-Save mode usage (enable/disable, disabled by default) +* `c`: to print current configuration. +* `h`: to print helps screen. + +## Terminal output + +### Data format + +Data are printed on the terminal as follow: + +``` + us TILT +``` + +With: +* ``: Time in microsecond read from MCU clock when latest INT1 was fired. + +### Example of output + +``` +[I] ### +[I] ### Example Tilt +[I] ### +[I] 6374731 us TILT +[I] 11683617 us TILT +``` + diff --git a/examples/tilt/tilt.c b/examples/tilt/tilt.c new file mode 100644 index 0000000..b11c4d9 --- /dev/null +++ b/examples/tilt/tilt.c @@ -0,0 +1,330 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/* Driver */ +#include "imu/inv_imu_driver.h" +#include "imu/inv_imu_apex.h" + +/* Board drivers */ +#include "system_interface.h" + +#include /* NULL */ + +/* + * This example showcases how to configure and use Tilt. + */ + +/* + * Select communication link between SmartMotion and IMU. + * SPI4: `UI_SPI4` + * I2C: `UI_I2C` + */ +#define SERIF_TYPE UI_SPI4 + +/* WOM threshold */ +#if INV_IMU_HFSR_SUPPORTED +#define WOM_THRESHOLD 7 /* ~50 mg */ +#else +#define WOM_THRESHOLD 13 /* ~50 mg */ +#endif + +/* Static variables */ +static inv_imu_device_t imu_dev; /* Driver structure */ +static volatile int int1_flag; /* Flag set when INT1 is received */ +static volatile uint64_t int1_timestamp; /* Store timestamp when int from IMU fires */ + +/* Static variables for command interface */ +static uint8_t use_lp_config; /* Indicates if pedometer is configured in Low Power mode */ +static uint8_t use_power_save; /* Indicates if power save mode is being used */ + +/* Static functions definition */ +static int setup_mcu(); +static int setup_imu(); +static int configure_tilt(); +static void int_cb(void *context, unsigned int int_num); +static int get_uart_command(); +static int print_help(); +static int print_current_config(); + +/* Main function implementation */ +int main(void) +{ + int rc = 0; + + rc |= setup_mcu(); + SI_CHECK_RC(rc); + + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + INV_MSG(INV_MSG_LEVEL_INFO, "### Example Tilt"); + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + + /* Reset commands interface states */ + use_lp_config = 0; + use_power_save = 0; + + rc |= setup_imu(); + SI_CHECK_RC(rc); + + /* Reset timestamp and interrupt flag */ + int1_flag = 0; + int1_timestamp = 0; + + do { + /* Poll device for data */ + if (int1_flag) { + uint8_t int_status3; + uint64_t timestamp; + + si_disable_irq(); + /* Clear interrupt flag */ + int1_flag = 0; + /* Retrieve timestamp */ + timestamp = int1_timestamp; + si_enable_irq(); + + /* Read Tilt interrupt status */ + rc |= inv_imu_read_reg(&imu_dev, INT_STATUS3, 1, &int_status3); + SI_CHECK_RC(rc); + + if (int_status3 & (INT_STATUS3_TILT_DET_INT_MASK)) + INV_MSG(INV_MSG_LEVEL_INFO, " %10llu us TILT", timestamp); + } + + rc |= get_uart_command(); + } while (rc == 0); + + return rc; +} + +/* Initializes MCU peripherals. */ +static int setup_mcu() +{ + int rc = 0; + + rc |= si_board_init(); + + /* Configure UART for log */ + rc |= si_config_uart_for_print(SI_UART_ID_FTDI, INV_MSG_LEVEL_DEBUG); + + /* Configure GPIO to call `int_cb` when INT1 fires. */ + rc |= si_init_gpio_int(SI_GPIO_INT1, int_cb); + + /* Init timer peripheral for sleep and get_time */ + rc |= si_init_timers(); + + /* Initialize serial interface between MCU and IMU */ + rc |= si_io_imu_init(SERIF_TYPE); + + return rc; +} + +static int setup_imu() +{ + int rc = 0; + inv_imu_serif_t imu_serif; + uint8_t whoami; + inv_imu_int1_pin_config_t int1_pin_config; + inv_imu_interrupt_parameter_t int1_config = { (inv_imu_interrupt_value)0 }; + + /* Initialize serial interface between MCU and IMU */ + imu_serif.context = 0; /* no need */ + imu_serif.read_reg = si_io_imu_read_reg; + imu_serif.write_reg = si_io_imu_write_reg; + imu_serif.max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */ + imu_serif.max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */ + imu_serif.serif_type = SERIF_TYPE; + + /* Init device */ + rc |= inv_imu_init(&imu_dev, &imu_serif, NULL); + SI_CHECK_RC(rc); + +#if SERIF_TYPE == UI_SPI4 + /* Configure slew-rate to 19 ns (required when using EVB) */ + rc |= inv_imu_set_spi_slew_rate(&imu_dev, DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS); + SI_CHECK_RC(rc); +#endif + + /* Check WHOAMI */ + rc |= inv_imu_get_who_am_i(&imu_dev, &whoami); + SI_CHECK_RC(rc); + if (whoami != INV_IMU_WHOAMI) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Erroneous WHOAMI value."); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Read 0x%02x", whoami); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Expected 0x%02x", INV_IMU_WHOAMI); + return INV_ERROR; + } + + /* + * Configure interrupts pins + * - Polarity High + * - Pulse mode + * - Push-Pull drive + */ + int1_pin_config.int_polarity = INT_CONFIG_INT1_POLARITY_HIGH; + int1_pin_config.int_mode = INT_CONFIG_INT1_MODE_PULSED; + int1_pin_config.int_drive = INT_CONFIG_INT1_DRIVE_CIRCUIT_PP; + rc |= inv_imu_set_pin_config_int1(&imu_dev, &int1_pin_config); + + /* Configure interrupts sources */ + int1_config.INV_TILT_DET = INV_IMU_ENABLE; + rc |= inv_imu_set_config_int1(&imu_dev, &int1_config); + + /* Disabling FIFO usage to optimize power consumption */ + rc |= inv_imu_configure_fifo(&imu_dev, INV_IMU_FIFO_DISABLED); + + /* Set 2X averaging to minimize power consumption */ + rc |= inv_imu_set_accel_lp_avg(&imu_dev, ACCEL_CONFIG1_ACCEL_FILT_AVG_2); + + /* Enable accel in LP mode */ + rc |= inv_imu_enable_accel_low_power_mode(&imu_dev); + + rc |= configure_tilt(); + + return rc; +} + +static int configure_tilt() +{ + int rc = 0; + inv_imu_apex_parameters_t apex_inputs; + + /* Disable Tilt before configuring it */ + rc |= inv_imu_apex_disable_tilt(&imu_dev); + + if (use_lp_config) { + rc |= inv_imu_set_accel_frequency(&imu_dev, ACCEL_CONFIG0_ODR_25_HZ); + rc |= inv_imu_apex_set_frequency(&imu_dev, APEX_CONFIG1_DMP_ODR_25Hz); + } else { + rc |= inv_imu_set_accel_frequency(&imu_dev, ACCEL_CONFIG0_ODR_50_HZ); + rc |= inv_imu_apex_set_frequency(&imu_dev, APEX_CONFIG1_DMP_ODR_50Hz); + } + + /* Set APEX parameters */ + rc |= inv_imu_apex_init_parameters_struct(&imu_dev, &apex_inputs); + /* Parameters can be modified here if needed */ + apex_inputs.power_save = + use_power_save ? APEX_CONFIG0_DMP_POWER_SAVE_EN : APEX_CONFIG0_DMP_POWER_SAVE_DIS; + rc |= inv_imu_apex_configure_parameters(&imu_dev, &apex_inputs); + + /* If POWER_SAVE mode is enabled, WOM has to be enabled */ + if (use_power_save) { + /* Configure and enable WOM to wake-up the DMP once it goes in power save mode */ + rc |= inv_imu_configure_wom(&imu_dev, WOM_THRESHOLD, WOM_THRESHOLD, WOM_THRESHOLD, + WOM_CONFIG_WOM_INT_MODE_ANDED, WOM_CONFIG_WOM_INT_DUR_1_SMPL); + rc |= inv_imu_enable_wom(&imu_dev); + } + + /* Enable Tilt */ + rc |= inv_imu_apex_enable_tilt(&imu_dev); + + return rc; +} + +/* IMU interrupt handler. */ +static void int_cb(void *context, unsigned int int_num) +{ + (void)context; + + if (int_num == SI_GPIO_INT1) { + int1_timestamp = si_get_time_us(); + int1_flag = 1; + } +} + +/* Get command from user through UART */ +static int get_uart_command() +{ + int rc = 0; + char cmd = 0; + + rc |= si_get_uart_command(SI_UART_ID_FTDI, &cmd); + SI_CHECK_RC(rc); + + switch (cmd) { + case 'a': /* Use Low Power or High Perf Tilt configuration */ + use_lp_config = !use_lp_config; + INV_MSG(INV_MSG_LEVEL_INFO, "Tilt configured in %s mode.", + use_lp_config ? "Low Power" : "High Performance"); + rc |= configure_tilt(); + break; + case 'p': /* Toggle power-save mode usage */ + use_power_save = !use_power_save; + INV_MSG(INV_MSG_LEVEL_INFO, "Power-Save mode: %s", use_power_save ? "Enabled" : "Disabled"); + rc |= configure_tilt(); + break; + case 'c': + rc |= print_current_config(); + break; + case 'h': + case 'H': + rc |= print_help(); + break; + case 0: + break; /* No command received */ + default: + INV_MSG(INV_MSG_LEVEL_INFO, "Unknown command : %c", cmd); + rc |= print_help(); + break; + } + + return rc; +} + +/* Help for UART command interface */ +static int print_help() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Help"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'a' : Toggle Tilt config (High Perf and Low Power)"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'p' : Toggle Power-Save mode usage"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'c' : Print current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "# 'h' : Print this helper"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Print current sample configuration */ +static int print_current_config() +{ + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Current configuration"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Tilt configuration: %s mode", + use_lp_config ? "Low Power" : "High Performance"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Power-Save: %s", use_power_save ? "Enabled" : "Disabled"); + INV_MSG(INV_MSG_LEVEL_INFO, "#"); + + si_sleep_us(2000000); /* Give user some time to read */ + + return 0; +} + +/* Get time implementation for IMU driver */ +uint64_t inv_imu_get_time_us(void) +{ + return si_get_time_us(); +} + +/* Sleep implementation for IMU driver */ +void inv_imu_sleep_us(uint32_t us) +{ + si_sleep_us(us); +} diff --git a/examples/wom/README.md b/examples/wom/README.md new file mode 100644 index 0000000..d068b02 --- /dev/null +++ b/examples/wom/README.md @@ -0,0 +1,41 @@ +# wom + +This application demonstrates how to configure and use wom (Wake-On-Motion) feature. + +## Terminal output + +### Data format + +Data are printed on the terminal as follow: + +``` + us WOM X= Y= Z= +``` + +With: +* ``: Time in microsecond read from MCU clock when latest INT1 was fired. +* ``: 1 when motion through X axis exceeds configured threshold, 0 otherwise. +* ``: 1 when motion through Y axis exceeds configured threshold, 0 otherwise. +* ``: 1 when motion through Z axis exceeds configured threshold, 0 otherwise. + +### Example of output + +``` +[I] ### +[I] ### Example WOM +[I] ### +[I] 1846092 us WOM X=0 Y=0 Z=1 +[I] 1926230 us WOM X=1 Y=0 Z=1 +[I] 2006377 us WOM X=1 Y=0 Z=1 +[I] 2086516 us WOM X=1 Y=0 Z=1 +[I] 2166653 us WOM X=1 Y=0 Z=1 +[I] 2246786 us WOM X=1 Y=1 Z=0 +[I] 2326920 us WOM X=1 Y=1 Z=1 +[I] 2407051 us WOM X=1 Y=0 Z=1 +[I] 2487187 us WOM X=0 Y=0 Z=1 +[I] 2567319 us WOM X=0 Y=0 Z=1 +[I] 2647450 us WOM X=1 Y=1 Z=1 +[I] 2727583 us WOM X=1 Y=0 Z=1 +[I] 2807717 us WOM X=0 Y=0 Z=1 +``` + diff --git a/examples/wom/wom.c b/examples/wom/wom.c new file mode 100644 index 0000000..1a5a8bb --- /dev/null +++ b/examples/wom/wom.c @@ -0,0 +1,218 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/* Driver */ +#include "imu/inv_imu_driver.h" + +/* Board drivers */ +#include "system_interface.h" + +#include /* NULL */ + +/* + * This example showcases how to configure and use WOM. + */ + +/* + * Select communication link between SmartMotion and IMU. + * SPI4: `UI_SPI4` + * I2C: `UI_I2C` + */ +#define SERIF_TYPE UI_SPI4 + +/* WOM threshold in mg */ +#if INV_IMU_HFSR_SUPPORTED +#define WOM_THRESHOLD 25 /* ~200 mg */ +#else +#define WOM_THRESHOLD 50 /* ~200 mg */ +#endif + +/* Static variables */ +static inv_imu_device_t imu_dev; /* Driver structure */ +static volatile int int1_flag; /* Flag set when INT1 is received */ +static volatile uint64_t int1_timestamp; /* Store timestamp when int from IMU fires */ + +/* Static functions definition */ +static int setup_mcu(); +static int setup_imu(); +static void int_cb(void *context, unsigned int int_num); + +/* Main function implementation */ +int main(void) +{ + int rc = 0; + + rc |= setup_mcu(); + SI_CHECK_RC(rc); + + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + INV_MSG(INV_MSG_LEVEL_INFO, "### Example WOM"); + INV_MSG(INV_MSG_LEVEL_INFO, "###"); + + rc |= setup_imu(); + SI_CHECK_RC(rc); + + /* Reset timestamp and interrupt flag */ + int1_flag = 0; + int1_timestamp = 0; + + do { + /* Poll device for data */ + if (int1_flag) { + uint8_t int_status; + uint64_t timestamp; + + si_disable_irq(); + /* Clear interrupt flag */ + int1_flag = 0; + /* Retrieve timestamp */ + timestamp = int1_timestamp; + si_enable_irq(); + + /* Read WOM interrupt status */ + rc |= inv_imu_read_reg(&imu_dev, INT_STATUS2, 1, &int_status); + SI_CHECK_RC(rc); + + if (int_status & (INT_STATUS2_WOM_X_INT_MASK | INT_STATUS2_WOM_Y_INT_MASK | + INT_STATUS2_WOM_Z_INT_MASK)) { + INV_MSG(INV_MSG_LEVEL_INFO, " %10llu us WOM X=%d Y=%d Z=%d", timestamp, + (int_status & INT_STATUS2_WOM_X_INT_MASK) ? 1 : 0, + (int_status & INT_STATUS2_WOM_Y_INT_MASK) ? 1 : 0, + (int_status & INT_STATUS2_WOM_Z_INT_MASK) ? 1 : 0); + } + } + } while (rc == 0); + + return rc; +} + +/* Initializes MCU peripherals. */ +static int setup_mcu() +{ + int rc = 0; + + rc |= si_board_init(); + + /* Configure UART for log */ + rc |= si_config_uart_for_print(SI_UART_ID_FTDI, INV_MSG_LEVEL_DEBUG); + + /* Configure GPIO to call `int_cb` when INT1 fires. */ + rc |= si_init_gpio_int(SI_GPIO_INT1, int_cb); + + /* Init timer peripheral for sleep and get_time */ + rc |= si_init_timers(); + + /* Initialize serial interface between MCU and IMU */ + rc |= si_io_imu_init(SERIF_TYPE); + + return rc; +} + +static int setup_imu() +{ + int rc = 0; + inv_imu_serif_t imu_serif; + uint8_t whoami; + inv_imu_int1_pin_config_t int1_pin_config; + inv_imu_interrupt_parameter_t int1_config = { (inv_imu_interrupt_value)0 }; + + /* Initialize serial interface between MCU and IMU */ + imu_serif.context = 0; /* no need */ + imu_serif.read_reg = si_io_imu_read_reg; + imu_serif.write_reg = si_io_imu_write_reg; + imu_serif.max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */ + imu_serif.max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */ + imu_serif.serif_type = SERIF_TYPE; + + /* Init device */ + rc |= inv_imu_init(&imu_dev, &imu_serif, NULL); + SI_CHECK_RC(rc); + +#if SERIF_TYPE == UI_SPI4 + /* Configure slew-rate to 19 ns (required when using EVB) */ + rc |= inv_imu_set_spi_slew_rate(&imu_dev, DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS); + SI_CHECK_RC(rc); +#endif + + /* Check WHOAMI */ + rc |= inv_imu_get_who_am_i(&imu_dev, &whoami); + SI_CHECK_RC(rc); + if (whoami != INV_IMU_WHOAMI) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Erroneous WHOAMI value."); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Read 0x%02x", whoami); + INV_MSG(INV_MSG_LEVEL_ERROR, " - Expected 0x%02x", INV_IMU_WHOAMI); + return INV_ERROR; + } + + /* + * Configure interrupts pins + * - Polarity High + * - Pulse mode + * - Push-Pull drive + */ + int1_pin_config.int_polarity = INT_CONFIG_INT1_POLARITY_HIGH; + int1_pin_config.int_mode = INT_CONFIG_INT1_MODE_PULSED; + int1_pin_config.int_drive = INT_CONFIG_INT1_DRIVE_CIRCUIT_PP; + rc |= inv_imu_set_pin_config_int1(&imu_dev, &int1_pin_config); + + /* Configure interrupts sources */ + int1_config.INV_WOM_X = INV_IMU_ENABLE; + int1_config.INV_WOM_Y = INV_IMU_ENABLE; + int1_config.INV_WOM_Z = INV_IMU_ENABLE; + rc |= inv_imu_set_config_int1(&imu_dev, &int1_config); + + /* + * Optimize power consumption: + * - Disable FIFO usage. + * - Set 2X averaging. + * - Use Low-Power mode at low frequency. + */ + rc |= inv_imu_configure_fifo(&imu_dev, INV_IMU_FIFO_DISABLED); + rc |= inv_imu_set_accel_lp_avg(&imu_dev, ACCEL_CONFIG1_ACCEL_FILT_AVG_2); + rc |= inv_imu_set_accel_frequency(&imu_dev, ACCEL_CONFIG0_ODR_12_5_HZ); + rc |= inv_imu_enable_accel_low_power_mode(&imu_dev); + + /* Configure and enable WOM */ + rc |= inv_imu_configure_wom(&imu_dev, WOM_THRESHOLD, WOM_THRESHOLD, WOM_THRESHOLD, + WOM_CONFIG_WOM_INT_MODE_ORED, WOM_CONFIG_WOM_INT_DUR_1_SMPL); + rc |= inv_imu_enable_wom(&imu_dev); + + return rc; +} + +/* IMU interrupt handler. */ +static void int_cb(void *context, unsigned int int_num) +{ + (void)context; + + if (int_num == SI_GPIO_INT1) { + int1_timestamp = si_get_time_us(); + int1_flag = 1; + } +} + +/* Get time implementation for IMU driver */ +uint64_t inv_imu_get_time_us(void) +{ + return si_get_time_us(); +} + +/* Sleep implementation for IMU driver */ +void inv_imu_sleep_us(uint32_t us) +{ + si_sleep_us(us); +} diff --git a/icm42670p/imu/README.md b/icm42670p/imu/README.md new file mode 100644 index 0000000..63c5661 --- /dev/null +++ b/icm42670p/imu/README.md @@ -0,0 +1,84 @@ +# Overview + +The eMD driver is TDK Invensense's reference code to drive our IMU from a microcontroller-based system. It is coded in C language and organized around modules. + +## Common files + +All modules rely on the following files. + +Files: +* `imu/inv_imu_transport.h`: Definition of the abstraction layer used to communicate with the IMU. +* `imu/inv_imu_transport.c`: Implementation of the abstraction layer used to communicate with the IMU. +* `imu/inv_imu.h`: Describes IMU specificities and capabilities. +* `imu/inv_imu_regmap_rev_a.h`: Exposes register map using positions and masks. +* `imu/inv_imu_defs.h`: Defines possible values for most used registers. +* `imu/inv_imu_extfunc.h`: Defines system functions required by the driver, which shall be implemented in the application layer. + +## Driver + +The **driver** contains the generic functionalities required to operate our IMU. + +Depends on: +* **Common files** + +Files: +* `imu/inv_imu_driver.h`: Definition of the driver API. +* `imu/inv_imu_driver.c`: Implementation of the driver API. +* `imu/inv_imu_version.h`: Contains the driver's version as a string. + +## Self-test + +The **self-test** module contains the code required to operate IMU's self-test. + +Depends on: +* **Driver** +* **Common files** + +Files: +* `imu/inv_imu_selftest.h`: Definition of the self-test module API. +* `imu/inv_imu_selftest.c`: Implementation of the self-test module API. + +## APEX + +The **APEX** module provides API to operate algorithm features (APEX) on-chip. The following features are available: +* Pedometer (step count and step detector) +* FreeFall detection, including Low-G and High-G detection. +* Tilt detection +* Significant Motion Detection + +Depends on: +* **Driver** +* **Common files** + +Files: +* `imu/inv_imu_apex.h`: Definition of the APEX module API. +* `imu/inv_imu_apex.c`: Implementation of the APEX module API. + +# Initializing driver + +Please, follow these steps: +* On your application code, create a local variable of type `inv_imu_device_t` and another one of type `inv_imu_serif_t`: `inv_imu_device_t imu_dev;` and `inv_imu_serif_t imu_serif;` +* Initialize serif structure: + * Provide an implementation of the `read_reg` and `write_reg` functions and initialize the corresponding pointers. + * Configure the `max_read` and `max_write` fields which indicates the maximum numbers of bytes allowed per transaction. + * Configure the `serif_type` field which indicates the serial interface used. Available options are listed in `inv_imu_transport.h`. + * The `context` field can be set to 0. +* Initialize sensor event callback pointer in `inv_imu_adv_var_t` structure. This function will be called when a new sensor data will be available. +* Call the `inv_imu_adv_init` function, providing the device and serif objects as well as a callback which will be executed when a new sample is received. + +Example of initialization in C: +```C +inv_imu_device_t imu_dev; +inv_imu_serif_t imu_serif; + +/* Initialize serial interface */ +imu_serif.read_reg = si_io_imu_read_reg; +imu_serif.write_reg = si_io_imu_write_reg; +imu_serif.max_read = 32768; +imu_serif.max_write = 32768; +imu_serif.serif_type = SERIF_TYPE; +imu_serif.context = 0; + +/* Init device */ +rc |= inv_imu_init(&imu_dev, &imu_serif, sensor_event_cb); +``` diff --git a/icm42670p/imu/inv_imu.h b/icm42670p/imu/inv_imu.h new file mode 100644 index 0000000..37a96ba --- /dev/null +++ b/icm42670p/imu/inv_imu.h @@ -0,0 +1,50 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#ifndef _INV_IMU_H_ +#define _INV_IMU_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/** @defgroup IMU IMU + * @brief Describes IMU + * @{ + */ + +/** @file inv_imu.h */ + + +/* Device ID */ +#define ICM42670P + +/* Device description */ +#define INV_IMU_STRING_ID "ICM42670P" +#define INV_IMU_WHOAMI 0x67 +#define INV_IMU_REV INV_IMU_REV_A +#define INV_IMU_IS_GYRO_SUPPORTED 1 +#define INV_IMU_HFSR_SUPPORTED 0 + + +#ifdef __cplusplus +} +#endif + +#endif /* #ifndef _INV_IMU_H_ */ + +/** @} */ diff --git a/icm42670p/imu/inv_imu_apex.c b/icm42670p/imu/inv_imu_apex.c new file mode 100644 index 0000000..828172b --- /dev/null +++ b/icm42670p/imu/inv_imu_apex.c @@ -0,0 +1,354 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#include "imu/inv_imu_apex.h" +#include "imu/inv_imu_extfunc.h" + +int inv_imu_apex_enable_ff(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_start_dmp(s); + + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + value &= ~APEX_CONFIG1_FF_ENABLE_MASK; + value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_EN; + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_apex_disable_ff(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + value &= ~APEX_CONFIG1_FF_ENABLE_MASK; + value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_DIS; + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_apex_enable_smd(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_start_dmp(s); + + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + value &= ~APEX_CONFIG1_SMD_ENABLE_MASK; + value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_EN; + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_apex_disable_smd(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + value &= ~APEX_CONFIG1_SMD_ENABLE_MASK; + value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_DIS; + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_apex_init_parameters_struct(inv_imu_device_t *s, inv_imu_apex_parameters_t *apex_inputs) +{ + int status = 0; + (void)s; + + /* Default parameters at POR */ + apex_inputs->pedo_amp_th = APEX_CONFIG3_PEDO_AMP_TH_62_MG; + apex_inputs->pedo_step_cnt_th = 0x5; + apex_inputs->pedo_step_det_th = 0x2; + apex_inputs->pedo_sb_timer_th = APEX_CONFIG4_PEDO_SB_TIMER_TH_150_SAMPLES; + apex_inputs->pedo_hi_enrgy_th = APEX_CONFIG4_PEDO_HI_ENRGY_TH_104_MG; + apex_inputs->tilt_wait_time = APEX_CONFIG5_TILT_WAIT_TIME_4_S; + apex_inputs->power_save_time = APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_8_S; + apex_inputs->power_save = APEX_CONFIG0_DMP_POWER_SAVE_EN; + apex_inputs->sensitivity_mode = APEX_CONFIG9_SENSITIVITY_MODE_NORMAL; + apex_inputs->low_energy_amp_th = APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_80_MG; + apex_inputs->smd_sensitivity = APEX_CONFIG9_SMD_SENSITIVITY_0; + apex_inputs->ff_debounce_duration = APEX_CONFIG9_FF_DEBOUNCE_DURATION_2000_MS; + apex_inputs->ff_max_duration_cm = APEX_CONFIG12_FF_MAX_DURATION_204_CM; + apex_inputs->ff_min_duration_cm = APEX_CONFIG12_FF_MIN_DURATION_10_CM; + apex_inputs->lowg_peak_th = APEX_CONFIG10_LOWG_PEAK_TH_563_MG; + apex_inputs->lowg_peak_hyst = APEX_CONFIG5_LOWG_PEAK_TH_HYST_156_MG; + apex_inputs->lowg_samples_th = APEX_CONFIG10_LOWG_TIME_TH_1_SAMPLE; + apex_inputs->highg_peak_th = APEX_CONFIG11_HIGHG_PEAK_TH_2500_MG; + apex_inputs->highg_peak_hyst = APEX_CONFIG5_HIGHG_PEAK_TH_HYST_156_MG; + apex_inputs->highg_samples_th = APEX_CONFIG11_HIGHG_TIME_TH_1_SAMPLE; + + return status; +} + +int inv_imu_apex_configure_parameters(inv_imu_device_t * s, + const inv_imu_apex_parameters_t *apex_inputs) +{ + int status = 0; + uint8_t data; + uint8_t apexConfig[7]; + APEX_CONFIG1_PED_ENABLE_t pedo_state; + APEX_CONFIG1_TILT_ENABLE_t tilt_state; + APEX_CONFIG1_FF_ENABLE_t ff_state; + APEX_CONFIG1_SMD_ENABLE_t smd_state; + + /* DMP cannot be configured if it is running, hence make sure all APEX algorithms are off */ + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &data); + pedo_state = (APEX_CONFIG1_PED_ENABLE_t)(data & APEX_CONFIG1_PED_ENABLE_MASK); + tilt_state = (APEX_CONFIG1_TILT_ENABLE_t)(data & APEX_CONFIG1_TILT_ENABLE_MASK); + ff_state = (APEX_CONFIG1_FF_ENABLE_t)(data & APEX_CONFIG1_FF_ENABLE_MASK); + smd_state = (APEX_CONFIG1_SMD_ENABLE_t)(data & APEX_CONFIG1_SMD_ENABLE_MASK); + if (pedo_state == APEX_CONFIG1_PED_ENABLE_EN) + return INV_ERROR; + if (tilt_state == APEX_CONFIG1_TILT_ENABLE_EN) + return INV_ERROR; + if (ff_state == APEX_CONFIG1_FF_ENABLE_EN) + return INV_ERROR; + if (smd_state == APEX_CONFIG1_SMD_ENABLE_EN) + return INV_ERROR; + + status |= inv_imu_switch_on_mclk(s); + + /* Power Save mode and low energy amplitude threshold (for Pedometer in Slow Walk mode) */ + /* APEX_CONFIG2_MREG1 */ + apexConfig[0] = (uint8_t)apex_inputs->power_save_time | (uint8_t)apex_inputs->low_energy_amp_th; + + /* Pedometer parameters */ + /* APEX_CONFIG3_MREG1 */ + apexConfig[1] = (uint8_t)apex_inputs->pedo_amp_th | + (apex_inputs->pedo_step_cnt_th & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK); + + /* APEX_CONFIG4_MREG1 */ + apexConfig[2] = ((apex_inputs->pedo_step_det_th << APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS) & + APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK) | + (uint8_t)apex_inputs->pedo_sb_timer_th | (uint8_t)apex_inputs->pedo_hi_enrgy_th; + + /* Tilt, Lowg and highg parameters */ + /* APEX_CONFIG5_MREG1 */ + apexConfig[3] = (uint8_t)apex_inputs->tilt_wait_time | (uint8_t)apex_inputs->lowg_peak_hyst | + (uint8_t)apex_inputs->highg_peak_hyst; + + status |= inv_imu_write_reg(s, APEX_CONFIG2_MREG1, 4, &apexConfig[0]); + + /* APEX_CONFIG0 */ + status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &apexConfig[0]); + apexConfig[0] &= ~APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK; + apexConfig[0] |= apex_inputs->power_save; + status |= inv_imu_write_reg(s, APEX_CONFIG0, 1, &apexConfig[0]); + + /* free fall parameter, SMD parameter and parameters for Pedometer in Slow Walk mode */ + /* APEX_CONFIG9_MREG1 */ + apexConfig[0] = (uint8_t)apex_inputs->ff_debounce_duration | + (uint8_t)apex_inputs->smd_sensitivity | (uint8_t)apex_inputs->sensitivity_mode; + + /* Lowg and highg parameters and free fall parameters */ + /* APEX_CONFIG10_MREG1 */ + apexConfig[1] = (uint8_t)apex_inputs->lowg_peak_th | (uint8_t)apex_inputs->lowg_samples_th; + + /* APEX_CONFIG11_MREG1 */ + apexConfig[2] = (uint8_t)apex_inputs->highg_peak_th | (uint8_t)apex_inputs->highg_samples_th; + + status |= inv_imu_write_reg(s, APEX_CONFIG9_MREG1, 3, &apexConfig[0]); + + /* APEX_CONFIG12_MREG1 */ + apexConfig[0] = + (uint8_t)apex_inputs->ff_max_duration_cm | (uint8_t)apex_inputs->ff_min_duration_cm; + + status |= inv_imu_write_reg(s, APEX_CONFIG12_MREG1, 1, &apexConfig[0]); + + status |= inv_imu_switch_off_mclk(s); + + return status; +} + +int inv_imu_apex_get_parameters(inv_imu_device_t *s, inv_imu_apex_parameters_t *apex_params) +{ + int status = 0; + uint8_t data[7]; + uint8_t value; + + status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &value); + apex_params->power_save = + (APEX_CONFIG0_DMP_POWER_SAVE_t)(value & APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK); + + /* Access continuous config registers (CONFIG2-CONFIG11) */ + status |= inv_imu_read_reg(s, APEX_CONFIG2_MREG1, sizeof(data), &data[0]); + + /* Get params from apex_config2 : dmp_power_save_time and low_energy_amp_th */ + apex_params->power_save_time = + (APEX_CONFIG2_DMP_POWER_SAVE_TIME_t)(data[0] & APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_MASK); + apex_params->low_energy_amp_th = + (APEX_CONFIG2_LOW_ENERGY_AMP_TH_t)(data[0] & APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_MASK); + + /* Get params from apex_config3 : pedo_amp_th and pedo_step_cnt_th */ + apex_params->pedo_amp_th = + (APEX_CONFIG3_PEDO_AMP_TH_t)(data[1] & APEX_CONFIG3_PED_AMP_TH_SEL_MASK); + apex_params->pedo_step_cnt_th = + (data[1] & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK) >> APEX_CONFIG3_PED_STEP_CNT_TH_SEL_POS; + + /* Get params from apex_config4 : pedo_step_det_th, pedo_sb_timer_th and pedo_hi_enrgy_th */ + apex_params->pedo_step_det_th = + (data[2] & APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK) >> APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS; + apex_params->pedo_sb_timer_th = + (APEX_CONFIG4_PEDO_SB_TIMER_TH_t)(data[2] & APEX_CONFIG4_PED_SB_TIMER_TH_SEL_MASK); + apex_params->pedo_hi_enrgy_th = + (APEX_CONFIG4_PEDO_HI_ENRGY_TH_t)(data[2] & APEX_CONFIG4_PED_HI_EN_TH_SEL_MASK); + + /* Get params from apex_config5 : tilt_wait_time, lowg_peak_hyst and highg_peak_hyst */ + apex_params->tilt_wait_time = + (APEX_CONFIG5_TILT_WAIT_TIME_t)(data[3] & APEX_CONFIG5_TILT_WAIT_TIME_SEL_MASK); + apex_params->lowg_peak_hyst = + (APEX_CONFIG5_LOWG_PEAK_TH_HYST_t)(data[3] & APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_MASK); + apex_params->highg_peak_hyst = + (APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t)(data[3] & APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_MASK); + + /* Get params from apex_config9 : ff_debounce_duration, smd_sensitivity and sensitivity_mode */ + apex_params->ff_debounce_duration = + (APEX_CONFIG9_FF_DEBOUNCE_DURATION_t)(data[4] & APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_MASK); + apex_params->smd_sensitivity = + (APEX_CONFIG9_SMD_SENSITIVITY_t)(data[4] & APEX_CONFIG9_SMD_SENSITIVITY_SEL_MASK); + apex_params->sensitivity_mode = + (APEX_CONFIG9_SENSITIVITY_MODE_t)(data[4] & APEX_CONFIG9_SENSITIVITY_MODE_MASK); + + /* Get params from apex_config10 : lowg_peak_th and lowg_samples_th */ + apex_params->lowg_peak_th = + (APEX_CONFIG10_LOWG_PEAK_TH_t)(data[5] & APEX_CONFIG10_LOWG_PEAK_TH_SEL_MASK); + apex_params->lowg_samples_th = + (APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t)(data[5] & APEX_CONFIG10_LOWG_TIME_TH_SEL_MASK); + + /* Get params from apex_config11 : highg_peak_th and highg_samples_th */ + apex_params->highg_peak_th = + (APEX_CONFIG11_HIGHG_PEAK_TH_t)(data[6] & APEX_CONFIG11_HIGHG_PEAK_TH_SEL_MASK); + apex_params->highg_samples_th = + (APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t)(data[6] & APEX_CONFIG11_HIGHG_TIME_TH_SEL_MASK); + + /* Access apex reg 12 */ + status |= inv_imu_read_reg(s, APEX_CONFIG12_MREG1, 1, &data[0]); + + /* Get params from apex_config12 : ff_max_duration_cm and ff_min_duration_cm */ + apex_params->ff_max_duration_cm = + (APEX_CONFIG12_FF_MAX_DURATION_t)(data[0] & APEX_CONFIG12_FF_MAX_DURATION_SEL_MASK); + apex_params->ff_min_duration_cm = + (APEX_CONFIG12_FF_MIN_DURATION_t)(data[0] & APEX_CONFIG12_FF_MIN_DURATION_SEL_MASK); + + return status; +} + +int inv_imu_apex_set_frequency(inv_imu_device_t *s, const APEX_CONFIG1_DMP_ODR_t frequency) +{ + uint8_t value; + int status = 0; + + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + + value &= ~APEX_CONFIG1_DMP_ODR_MASK; + value |= frequency; + + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + return status; +} + +int inv_imu_apex_enable_pedometer(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_start_dmp(s); + + /* Enable Pedometer */ + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + value &= ~APEX_CONFIG1_PED_ENABLE_MASK; + value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_EN; + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_apex_disable_pedometer(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + /* Disable Pedometer */ + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + value &= ~APEX_CONFIG1_PED_ENABLE_MASK; + value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_DIS; + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_apex_enable_tilt(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_start_dmp(s); + + /* Enable Tilt */ + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + value &= ~APEX_CONFIG1_TILT_ENABLE_MASK; + value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_EN; + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_apex_disable_tilt(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + /* Disable Tilt */ + status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value); + value &= ~APEX_CONFIG1_TILT_ENABLE_MASK; + value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_DIS; + status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_apex_get_data_activity(inv_imu_device_t *s, inv_imu_apex_step_activity_t *apex_activity) +{ + uint8_t data[4]; + int status = inv_imu_read_reg(s, APEX_DATA0, 4, data); + + apex_activity->step_cnt = (uint16_t)((data[1] << 8) | data[0]); + apex_activity->step_cadence = data[2]; + apex_activity->activity_class = data[3] & APEX_DATA3_ACTIVITY_CLASS_MASK; + + return status; +} + +int inv_imu_apex_get_data_free_fall(inv_imu_device_t *s, uint16_t *freefall_duration) +{ + uint8_t data[2]; + int status = inv_imu_read_reg(s, APEX_DATA4, 2, &data[0]); + + *freefall_duration = (uint16_t)((data[1] << 8) | data[0]); + + return status; +} diff --git a/icm42670p/imu/inv_imu_apex.h b/icm42670p/imu/inv_imu_apex.h new file mode 100644 index 0000000..9f43e9a --- /dev/null +++ b/icm42670p/imu/inv_imu_apex.h @@ -0,0 +1,179 @@ +/* + * + * Copyright (c) [2017] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/** @defgroup Apex APEX + * @brief High-level functions to drive APEX features + * @{ + */ + +/** @file inv_imu_apex.h */ + +#ifndef _INV_IMU_APEX_H_ +#define _INV_IMU_APEX_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "imu/inv_imu_driver.h" + +/** IMU APEX inputs parameters definition */ +typedef struct { + APEX_CONFIG3_PEDO_AMP_TH_t pedo_amp_th; + uint8_t pedo_step_cnt_th; + uint8_t pedo_step_det_th; + APEX_CONFIG4_PEDO_SB_TIMER_TH_t pedo_sb_timer_th; + APEX_CONFIG4_PEDO_HI_ENRGY_TH_t pedo_hi_enrgy_th; + APEX_CONFIG5_TILT_WAIT_TIME_t tilt_wait_time; + APEX_CONFIG2_DMP_POWER_SAVE_TIME_t power_save_time; + APEX_CONFIG0_DMP_POWER_SAVE_t power_save; + APEX_CONFIG9_SENSITIVITY_MODE_t sensitivity_mode; + APEX_CONFIG2_LOW_ENERGY_AMP_TH_t low_energy_amp_th; + APEX_CONFIG9_SMD_SENSITIVITY_t smd_sensitivity; + APEX_CONFIG9_FF_DEBOUNCE_DURATION_t ff_debounce_duration; + APEX_CONFIG12_FF_MAX_DURATION_t ff_max_duration_cm; + APEX_CONFIG12_FF_MIN_DURATION_t ff_min_duration_cm; + APEX_CONFIG10_LOWG_PEAK_TH_t lowg_peak_th; + APEX_CONFIG5_LOWG_PEAK_TH_HYST_t lowg_peak_hyst; + APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t lowg_samples_th; + APEX_CONFIG11_HIGHG_PEAK_TH_t highg_peak_th; + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t highg_peak_hyst; + APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t highg_samples_th; +} inv_imu_apex_parameters_t; + +/** APEX pedometer outputs */ +typedef struct inv_imu_apex_step_activity { + /** Number of steps taken */ + uint16_t step_cnt; + + /** Walk/run cadence in number of samples. + * Format is u6.2. (at 50Hz and 2Hz walk frequency, if the cadence + * is 25 samples, the register will output 100). + */ + uint8_t step_cadence; + + /** Detected activity. + * Unknown (0), Walk (1) or Run (2) + */ + uint8_t activity_class; +} inv_imu_apex_step_activity_t; + +/** @brief Enable Free Fall. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_enable_ff(inv_imu_device_t *s); + +/** @brief Disable Free Fall. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_disable_ff(inv_imu_device_t *s); + +/** @brief Enable Significant Motion Detection. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + * @warning SMD requires to have the pedometer enabled to work. + */ +int inv_imu_apex_enable_smd(inv_imu_device_t *s); + +/** @brief Disable Significant Motion Detection. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_disable_smd(inv_imu_device_t *s); + +/** @brief Fill the APEX parameters structure with all the default parameters for APEX algorithms. + * @param[in] s Pointer to device. + * @param[out] apex_inputs Default input parameters. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_init_parameters_struct(inv_imu_device_t * s, + inv_imu_apex_parameters_t *apex_inputs); + +/** @brief Configures DMP parameters for APEX algorithms. + * @param[in] s Pointer to device. + * @param[in] apex_inputs The requested input parameters. + * @return 0 on success, negative value on error. + * @warning APEX inputs can't change on the fly, this should be called before enabling + * any APEX features. + * @warning This API can't be called twice within 10 ms. + */ +int inv_imu_apex_configure_parameters(inv_imu_device_t * s, + const inv_imu_apex_parameters_t *apex_inputs); + +/** @brief Returns current DMP parameters for APEX algorithms. + * @param[in] s Pointer to device. + * @param[out] apex_params The current parameter, fetched from registers. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_get_parameters(inv_imu_device_t *s, inv_imu_apex_parameters_t *apex_params); + +/** @brief Configure DMP Output Data Rate for APEX algorithms. + * @param[in] s Pointer to device. + * @param[in] frequency The requested frequency. + * @return 0 on success, negative value on error. + * @warning Accel frequency must be higher or equal to DMP frequency. + */ +int inv_imu_apex_set_frequency(inv_imu_device_t *s, const APEX_CONFIG1_DMP_ODR_t frequency); + +/** @brief Enable APEX algorithm Pedometer. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_enable_pedometer(inv_imu_device_t *s); + +/** @brief Disable APEX algorithm Pedometer. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_disable_pedometer(inv_imu_device_t *s); + +/** @brief Enable APEX algorithm Tilt. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_enable_tilt(inv_imu_device_t *s); + +/** @brief Disable APEX algorithm Tilt. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_disable_tilt(inv_imu_device_t *s); + +/** @brief Retrieve APEX pedometer outputs and format them + * @param[in] s Pointer to device. + * @param[out] apex_activity Apex step and activity data value. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_get_data_activity(inv_imu_device_t * s, + inv_imu_apex_step_activity_t *apex_activity); + +/** @brief Retrieve APEX free fall outputs and format them + * @param[in] s Pointer to device. + * @param[out] freefall_duration Free fall duration in number of sample. + * @return 0 on success, negative value on error. + */ +int inv_imu_apex_get_data_free_fall(inv_imu_device_t *s, uint16_t *freefall_duration); + +#ifdef __cplusplus +} +#endif + +#endif /* _INV_IMU_APEX_H_ */ + +/** @} */ diff --git a/icm42670p/imu/inv_imu_defs.h b/icm42670p/imu/inv_imu_defs.h new file mode 100644 index 0000000..54f1154 --- /dev/null +++ b/icm42670p/imu/inv_imu_defs.h @@ -0,0 +1,1002 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#ifndef _INV_IMU_DEFS_H_ +#define _INV_IMU_DEFS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/** @file inv_imu_defs.h + * File exposing the device register map + */ + +#include + +#define INV_IMU_REV_A 0xA +#define INV_IMU_REV_B 0XB + +/* Include device definition */ +#include "imu/inv_imu.h" + +#if INV_IMU_REV == INV_IMU_REV_A +#include "imu/inv_imu_regmap_rev_a.h" +#elif INV_IMU_REV == INV_IMU_REV_B +#include "imu/inv_imu_regmap_rev_b.h" +#endif + +#define ACCEL_DATA_SIZE 6 +#define GYRO_DATA_SIZE 6 +#define TEMP_DATA_SIZE 2 + +#define FIFO_HEADER_SIZE 1 +#define FIFO_ACCEL_DATA_SIZE ACCEL_DATA_SIZE +#define FIFO_GYRO_DATA_SIZE GYRO_DATA_SIZE +#define FIFO_TEMP_DATA_SIZE 1 +#define FIFO_TS_FSYNC_SIZE 2 +#define FIFO_TEMP_HIGH_RES_SIZE 1 +#define FIFO_ACCEL_GYRO_HIGH_RES_SIZE 3 + +#define FIFO_16BYTES_PACKET_SIZE \ + (FIFO_HEADER_SIZE + FIFO_ACCEL_DATA_SIZE + FIFO_GYRO_DATA_SIZE + FIFO_TEMP_DATA_SIZE + \ + FIFO_TS_FSYNC_SIZE) +#define FIFO_20BYTES_PACKET_SIZE \ + (FIFO_HEADER_SIZE + FIFO_ACCEL_DATA_SIZE + FIFO_GYRO_DATA_SIZE + FIFO_TEMP_DATA_SIZE + \ + FIFO_TS_FSYNC_SIZE + FIFO_TEMP_HIGH_RES_SIZE + FIFO_ACCEL_GYRO_HIGH_RES_SIZE) + +#define FIFO_HEADER_ODR_ACCEL 0x01 +#define FIFO_HEADER_ODR_GYRO 0x02 +#define FIFO_HEADER_FSYNC 0x04 +#define FIFO_HEADER_TMST 0x08 +#define FIFO_HEADER_HEADER_20 0x10 +#define FIFO_HEADER_GYRO 0x20 +#define FIFO_HEADER_ACC 0x40 +#define FIFO_HEADER_MSG 0x80 + +#define INVALID_VALUE_FIFO ((int16_t)0x8000) +#define INVALID_VALUE_FIFO_1B ((int8_t)0x80) +#define OUT_OF_BOUND_TEMPERATURE_NEG_FIFO_1B ((int8_t)0x81) +#define OUT_OF_BOUND_TEMPERATURE_POS_FIFO_1B ((int8_t)0x7F) + +/** Describe the content of the FIFO header */ +typedef union { + unsigned char Byte; + struct { +#if INV_IMU_IS_GYRO_SUPPORTED + unsigned char gyro_odr_different : 1; +#else + unsigned char reserved1 : 1; +#endif + unsigned char accel_odr_different : 1; +#if INV_IMU_IS_GYRO_SUPPORTED + unsigned char fsync_bit : 1; +#else + unsigned char reserved2 : 1; +#endif + unsigned char timestamp_bit : 1; + unsigned char twentybits_bit : 1; +#if INV_IMU_IS_GYRO_SUPPORTED + unsigned char gyro_bit : 1; +#else + unsigned char reserved3 : 1; +#endif + unsigned char accel_bit : 1; + unsigned char msg_bit : 1; + } bits; +} fifo_header_t; + +/* ---------------------------------------------------------------------------- + * Device registers description + * + * Next section defines some of the registers bitfield and declare corresponding + * accessors. + * Note that descriptors and accessors are not provided for all the registers + * but only for the most useful ones. + * For all details on registers and bitfields functionalities please refer to + * the device datasheet. + * ---------------------------------------------------------------------------- */ + +/* --------------------------------------------------------------------------- + * register bank 0 + * ---------------------------------------------------------------------------- */ + +/* + * DEVICE_CONFIG + * Register Name : DEVICE_CONFIG + */ + +/* SPI_MODE */ +typedef enum { + DEVICE_CONFIG_SPI_MODE_1_2 = (0x1 << DEVICE_CONFIG_SPI_MODE_POS), + DEVICE_CONFIG_SPI_MODE_0_3 = (0x0 << DEVICE_CONFIG_SPI_MODE_POS), +} DEVICE_CONFIG_SPI_MODE_t; + +/* SPI_AP_4WIRE */ +typedef enum { + DEVICE_CONFIG_SPI_4WIRE = (0x1 << DEVICE_CONFIG_SPI_AP_4WIRE_POS), + DEVICE_CONFIG_SPI_3WIRE = (0x0 << DEVICE_CONFIG_SPI_AP_4WIRE_POS), +} DEVICE_CONFIG_SPI_AP_4WIRE_t; + +/* + * SIGNAL_PATH_RESET + * Register Name: SIGNAL_PATH_RESET + */ + +/* SOFT_RESET_DEVICE_CONFIG */ +typedef enum { + SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_EN = + (0x01 << SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_POS), + SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_DIS = + (0x00 << SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_POS), +} SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_t; + +/* FIFO_FLUSH */ +typedef enum { + SIGNAL_PATH_RESET_FIFO_FLUSH_EN = (0x01 << SIGNAL_PATH_RESET_FIFO_FLUSH_POS), + SIGNAL_PATH_RESET_FIFO_FLUSH_DIS = (0x00 << SIGNAL_PATH_RESET_FIFO_FLUSH_POS), +} SIGNAL_PATH_RESET_FIFO_FLUSH_t; + +/* + * DRIVE_CONFIG3 + * Register Name: DRIVE_CONFIG3 + */ + +/* SPI_SLEW_RATE */ +typedef enum { + DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_2_NS = (0x05 << DRIVE_CONFIG3_SPI_SLEW_RATE_POS), + DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_8_NS = (0x04 << DRIVE_CONFIG3_SPI_SLEW_RATE_POS), + DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_14_NS = (0x03 << DRIVE_CONFIG3_SPI_SLEW_RATE_POS), + DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_19_NS = (0x02 << DRIVE_CONFIG3_SPI_SLEW_RATE_POS), + DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_36_NS = (0x01 << DRIVE_CONFIG3_SPI_SLEW_RATE_POS), + DRIVE_CONFIG3_SPI_SLEW_RATE_MAX_60_NS = (0x00 << DRIVE_CONFIG3_SPI_SLEW_RATE_POS), +} DRIVE_CONFIG3_SPI_SLEW_RATE_t; + +/* + * INT_CONFIG + * Register Name: INT_CONFIG + */ + +/* INT2_MODE */ +typedef enum { + INT_CONFIG_INT2_MODE_LATCHED = (0x01 << INT_CONFIG_INT2_MODE_POS), + INT_CONFIG_INT2_MODE_PULSED = (0x00 << INT_CONFIG_INT2_MODE_POS), +} INT_CONFIG_INT2_MODE_t; + +/* INT2_DRIVE_CIRCUIT */ +typedef enum { + INT_CONFIG_INT2_DRIVE_CIRCUIT_PP = (0x01 << INT_CONFIG_INT2_DRIVE_CIRCUIT_POS), + INT_CONFIG_INT2_DRIVE_CIRCUIT_OD = (0x00 << INT_CONFIG_INT2_DRIVE_CIRCUIT_POS), +} INT_CONFIG_INT2_DRIVE_CIRCUIT_t; + +/* INT2_POLARITY */ +typedef enum { + INT_CONFIG_INT2_POLARITY_HIGH = (0x01 << INT_CONFIG_INT2_POLARITY_POS), + INT_CONFIG_INT2_POLARITY_LOW = (0x00 << INT_CONFIG_INT2_POLARITY_POS), +} INT_CONFIG_INT2_POLARITY_t; + +/* INT1_MODE */ +typedef enum { + INT_CONFIG_INT1_MODE_LATCHED = (0x01 << INT_CONFIG_INT1_MODE_POS), + INT_CONFIG_INT1_MODE_PULSED = (0x00 << INT_CONFIG_INT1_MODE_POS), +} INT_CONFIG_INT1_MODE_t; + +/* INT1_DRIVE_CIRCUIT */ +typedef enum { + INT_CONFIG_INT1_DRIVE_CIRCUIT_PP = (0x01 << INT_CONFIG_INT1_DRIVE_CIRCUIT_POS), + INT_CONFIG_INT1_DRIVE_CIRCUIT_OD = (0x00 << INT_CONFIG_INT1_DRIVE_CIRCUIT_POS), +} INT_CONFIG_INT1_DRIVE_CIRCUIT_t; + +/* INT1_POLARITY */ +typedef enum { + INT_CONFIG_INT1_POLARITY_HIGH = 0x01, + INT_CONFIG_INT1_POLARITY_LOW = 0x00, +} INT_CONFIG_INT1_POLARITY_t; + +/* + * PWR_MGMT0 + * Register Name: PWR_MGMT0 + */ + +/* ACCEL_LP_CLK_SEL */ +typedef enum { + PWR_MGMT0_ACCEL_LP_CLK_WUOSC = (0x00 << PWR_MGMT0_ACCEL_LP_CLK_SEL_POS), + PWR_MGMT0_ACCEL_LP_CLK_RCOSC = (0x01 << PWR_MGMT0_ACCEL_LP_CLK_SEL_POS), +} PWR_MGMT0_ACCEL_LP_CLK_t; + +/* IDLE */ +typedef enum { + PWR_MGMT0_IDLE_DIS = (0x01 << PWR_MGMT0_IDLE_POS), + PWR_MGMT0_IDLE_EN = (0x00 << PWR_MGMT0_IDLE_POS), +} PWR_MGMT0_IDLE_t; + +#if INV_IMU_IS_GYRO_SUPPORTED +/* GYRO_MODE */ +typedef enum { + PWR_MGMT0_GYRO_MODE_LN = (0x03 << PWR_MGMT0_GYRO_MODE_POS), + PWR_MGMT0_GYRO_MODE_LP = (0x02 << PWR_MGMT0_GYRO_MODE_POS), + PWR_MGMT0_GYRO_MODE_STANDBY = (0x01 << PWR_MGMT0_GYRO_MODE_POS), + PWR_MGMT0_GYRO_MODE_OFF = (0x00 << PWR_MGMT0_GYRO_MODE_POS), +} PWR_MGMT0_GYRO_MODE_t; +#endif + +/* ACCEL_MODE */ +typedef enum { + PWR_MGMT0_ACCEL_MODE_LN = 0x03, + PWR_MGMT0_ACCEL_MODE_LP = 0x02, + PWR_MGMT0_ACCEL_MODE_OFF = 0x00, +} PWR_MGMT0_ACCEL_MODE_t; + +#if INV_IMU_IS_GYRO_SUPPORTED +/* + * GYRO_CONFIG0 + * Register Name: GYRO_CONFIG0 + */ + +/* GYRO_FS_SEL*/ +typedef enum { +#if INV_IMU_HFSR_SUPPORTED + GYRO_CONFIG0_FS_SEL_500dps = (3 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS), + GYRO_CONFIG0_FS_SEL_1000dps = (2 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS), + GYRO_CONFIG0_FS_SEL_2000dps = (1 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS), + GYRO_CONFIG0_FS_SEL_4000dps = (0 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS), +#else + GYRO_CONFIG0_FS_SEL_250dps = (3 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS), + GYRO_CONFIG0_FS_SEL_500dps = (2 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS), + GYRO_CONFIG0_FS_SEL_1000dps = (1 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS), + GYRO_CONFIG0_FS_SEL_2000dps = (0 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS), +#endif +} GYRO_CONFIG0_FS_SEL_t; + +/* GYRO_ODR */ +typedef enum { + GYRO_CONFIG0_ODR_1_5625_HZ = 0xF, + GYRO_CONFIG0_ODR_3_125_HZ = 0xE, + GYRO_CONFIG0_ODR_6_25_HZ = 0xD, + GYRO_CONFIG0_ODR_12_5_HZ = 0xC, + GYRO_CONFIG0_ODR_25_HZ = 0xB, + GYRO_CONFIG0_ODR_50_HZ = 0xA, + GYRO_CONFIG0_ODR_100_HZ = 0x9, + GYRO_CONFIG0_ODR_200_HZ = 0x8, + GYRO_CONFIG0_ODR_400_HZ = 0x7, + GYRO_CONFIG0_ODR_800_HZ = 0x6, + GYRO_CONFIG0_ODR_1600_HZ = 0x5, +} GYRO_CONFIG0_ODR_t; +#endif + +/* + * ACCEL_CONFIG0 + * Register Name: ACCEL_CONFIG0 + */ + +/* ACCEL_FS_SEL */ +typedef enum { +#if INV_IMU_HFSR_SUPPORTED + ACCEL_CONFIG0_FS_SEL_4g = (0x3 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS), + ACCEL_CONFIG0_FS_SEL_8g = (0x2 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS), + ACCEL_CONFIG0_FS_SEL_16g = (0x1 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS), + ACCEL_CONFIG0_FS_SEL_32g = (0x0 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS), +#else + ACCEL_CONFIG0_FS_SEL_2g = (0x3 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS), + ACCEL_CONFIG0_FS_SEL_4g = (0x2 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS), + ACCEL_CONFIG0_FS_SEL_8g = (0x1 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS), + ACCEL_CONFIG0_FS_SEL_16g = (0x0 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS), +#endif +} ACCEL_CONFIG0_FS_SEL_t; + +/* ACCEL_ODR */ +typedef enum { + ACCEL_CONFIG0_ODR_1_5625_HZ = 0xF, + ACCEL_CONFIG0_ODR_3_125_HZ = 0xE, + ACCEL_CONFIG0_ODR_6_25_HZ = 0xD, + ACCEL_CONFIG0_ODR_12_5_HZ = 0xC, + ACCEL_CONFIG0_ODR_25_HZ = 0xB, + ACCEL_CONFIG0_ODR_50_HZ = 0xA, + ACCEL_CONFIG0_ODR_100_HZ = 0x9, + ACCEL_CONFIG0_ODR_200_HZ = 0x8, + ACCEL_CONFIG0_ODR_400_HZ = 0x7, + ACCEL_CONFIG0_ODR_800_HZ = 0x6, + ACCEL_CONFIG0_ODR_1600_HZ = 0x5, +} ACCEL_CONFIG0_ODR_t; + +#if INV_IMU_IS_GYRO_SUPPORTED +/* + * GYRO_CONFIG1 + * Register Name: GYRO_CONFIG1 + */ + +/* GYRO_UI_FILT_BW_IND */ +typedef enum { + GYRO_CONFIG1_GYRO_FILT_BW_16 = (0x07 << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS), + GYRO_CONFIG1_GYRO_FILT_BW_25 = (0x06 << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS), + GYRO_CONFIG1_GYRO_FILT_BW_34 = (0x05 << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS), + GYRO_CONFIG1_GYRO_FILT_BW_53 = (0x04 << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS), + GYRO_CONFIG1_GYRO_FILT_BW_73 = (0x03 << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS), + GYRO_CONFIG1_GYRO_FILT_BW_121 = (0x02 << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS), + GYRO_CONFIG1_GYRO_FILT_BW_180 = (0x01 << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS), + GYRO_CONFIG1_GYRO_FILT_BW_NO_FILTER = (0x00 << GYRO_CONFIG1_GYRO_UI_FILT_BW_POS), +} GYRO_CONFIG1_GYRO_FILT_BW_t; +#endif + +/* + * ACCEL_CONFIG1 + * Register Name: ACCEL_CONFIG1 + */ + +/* ACCEL_UI_AVG_IND */ +typedef enum { + ACCEL_CONFIG1_ACCEL_FILT_AVG_64 = (0x5 << ACCEL_CONFIG1_ACCEL_UI_AVG_POS), + ACCEL_CONFIG1_ACCEL_FILT_AVG_32 = (0x4 << ACCEL_CONFIG1_ACCEL_UI_AVG_POS), + ACCEL_CONFIG1_ACCEL_FILT_AVG_16 = (0x3 << ACCEL_CONFIG1_ACCEL_UI_AVG_POS), + ACCEL_CONFIG1_ACCEL_FILT_AVG_8 = (0x2 << ACCEL_CONFIG1_ACCEL_UI_AVG_POS), + ACCEL_CONFIG1_ACCEL_FILT_AVG_4 = (0x1 << ACCEL_CONFIG1_ACCEL_UI_AVG_POS), + ACCEL_CONFIG1_ACCEL_FILT_AVG_2 = (0x0 << ACCEL_CONFIG1_ACCEL_UI_AVG_POS), +} ACCEL_CONFIG1_ACCEL_FILT_AVG_t; + +/* ACCEL_UI_FILT_BW_IND */ +typedef enum { + ACCEL_CONFIG1_ACCEL_FILT_BW_16 = (0x7 << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS), + ACCEL_CONFIG1_ACCEL_FILT_BW_25 = (0x6 << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS), + ACCEL_CONFIG1_ACCEL_FILT_BW_34 = (0x5 << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS), + ACCEL_CONFIG1_ACCEL_FILT_BW_53 = (0x4 << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS), + ACCEL_CONFIG1_ACCEL_FILT_BW_73 = (0x3 << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS), + ACCEL_CONFIG1_ACCEL_FILT_BW_121 = (0x2 << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS), + ACCEL_CONFIG1_ACCEL_FILT_BW_180 = (0x1 << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS), + ACCEL_CONFIG1_ACCEL_FILT_BW_NO_FILTER = (0x0 << ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS), +} ACCEL_CONFIG1_ACCEL_FILT_BW_t; + +/* + * APEX_CONFIG0 + * Register Name: APEX_CONFIG0 + */ + +/* DMP_POWER_SAVE_EN */ +typedef enum { + APEX_CONFIG0_DMP_POWER_SAVE_EN = (0x1 << APEX_CONFIG0_DMP_POWER_SAVE_EN_POS), + APEX_CONFIG0_DMP_POWER_SAVE_DIS = (0x0 << APEX_CONFIG0_DMP_POWER_SAVE_EN_POS), +} APEX_CONFIG0_DMP_POWER_SAVE_t; + +/* DMP_INIT_EN */ +typedef enum { + APEX_CONFIG0_DMP_INIT_EN = (0x01 << APEX_CONFIG0_DMP_INIT_EN_POS), + APEX_CONFIG0_DMP_INIT_DIS = (0x00 << APEX_CONFIG0_DMP_INIT_EN_POS), +} APEX_CONFIG0_DMP_INIT_t; + +/* DMP_MEM_RESET */ +typedef enum { + APEX_CONFIG0_DMP_MEM_RESET_APEX_ST_EN = (0x01 << APEX_CONFIG0_DMP_MEM_RESET_EN_POS), + APEX_CONFIG0_DMP_MEM_RESET_DIS = (0x00 << APEX_CONFIG0_DMP_MEM_RESET_EN_POS), +} APEX_CONFIG0_DMP_MEM_RESET_t; + +/* + * APEX_CONFIG1 + * Register Name: APEX_CONFIG1 + */ + +/* SMD_ENABLE */ +typedef enum { + APEX_CONFIG1_SMD_ENABLE_DIS = (0x00 << APEX_CONFIG1_SMD_ENABLE_POS), + APEX_CONFIG1_SMD_ENABLE_EN = (0x01 << APEX_CONFIG1_SMD_ENABLE_POS), +} APEX_CONFIG1_SMD_ENABLE_t; + +/* FF_ENABLE */ +typedef enum { + APEX_CONFIG1_FF_ENABLE_DIS = (0x00 << APEX_CONFIG1_FF_ENABLE_POS), + APEX_CONFIG1_FF_ENABLE_EN = (0x01 << APEX_CONFIG1_FF_ENABLE_POS), +} APEX_CONFIG1_FF_ENABLE_t; + +/* TILT_ENABLE */ +typedef enum { + APEX_CONFIG1_TILT_ENABLE_DIS = (0x0 << APEX_CONFIG1_TILT_ENABLE_POS), + APEX_CONFIG1_TILT_ENABLE_EN = (0x1 << APEX_CONFIG1_TILT_ENABLE_POS), +} APEX_CONFIG1_TILT_ENABLE_t; + +/* PED_ENABLE */ +typedef enum { + APEX_CONFIG1_PED_ENABLE_DIS = (0x0 << APEX_CONFIG1_PED_ENABLE_POS), + APEX_CONFIG1_PED_ENABLE_EN = (0x1 << APEX_CONFIG1_PED_ENABLE_POS), +} APEX_CONFIG1_PED_ENABLE_t; + +/* DMP_ODR */ +typedef enum { + APEX_CONFIG1_DMP_ODR_25Hz = (0x0 << APEX_CONFIG1_DMP_ODR_POS), + APEX_CONFIG1_DMP_ODR_50Hz = (0x2 << APEX_CONFIG1_DMP_ODR_POS), + APEX_CONFIG1_DMP_ODR_100Hz = (0x3 << APEX_CONFIG1_DMP_ODR_POS), + APEX_CONFIG1_DMP_ODR_400Hz = (0x1 << APEX_CONFIG1_DMP_ODR_POS), +} APEX_CONFIG1_DMP_ODR_t; + +/* + * WOM_CONFIG + * Register Name: WOM_CONFIG + */ + +/* WOM_INT_DUR */ +typedef enum { + WOM_CONFIG_WOM_INT_DUR_1_SMPL = (0x00 << WOM_CONFIG_WOM_INT_DUR_POS), + WOM_CONFIG_WOM_INT_DUR_2_SMPL = (0x01 << WOM_CONFIG_WOM_INT_DUR_POS), + WOM_CONFIG_WOM_INT_DUR_3_SMPL = (0x02 << WOM_CONFIG_WOM_INT_DUR_POS), + WOM_CONFIG_WOM_INT_DUR_4_SMPL = (0x03 << WOM_CONFIG_WOM_INT_DUR_POS), +} WOM_CONFIG_WOM_INT_DUR_t; + +/* WOM_INT_MODE */ +typedef enum { + WOM_CONFIG_WOM_INT_MODE_ANDED = (0x01 << WOM_CONFIG_WOM_INT_MODE_POS), + WOM_CONFIG_WOM_INT_MODE_ORED = (0x00 << WOM_CONFIG_WOM_INT_MODE_POS), +} WOM_CONFIG_WOM_INT_MODE_t; + +/* WOM_MODE */ +typedef enum { + WOM_CONFIG_WOM_MODE_CMP_PREV = (0x01 << WOM_CONFIG_WOM_MODE_POS), + WOM_CONFIG_WOM_MODE_CMP_INIT = (0x00 << WOM_CONFIG_WOM_MODE_POS), +} WOM_CONFIG_WOM_MODE_t; + +/* WOM_ENABLE */ +typedef enum { + WOM_CONFIG_WOM_EN_ENABLE = (0x01 << WOM_CONFIG_WOM_EN_POS), + WOM_CONFIG_WOM_EN_DISABLE = (0x00 << WOM_CONFIG_WOM_EN_POS), +} WOM_CONFIG_WOM_EN_t; + +/* + * FIFO_CONFIG1 + * Register Name: FIFO_CONFIG + */ + +/* FIFO_MODE */ +typedef enum { + FIFO_CONFIG1_FIFO_MODE_SNAPSHOT = (0x01 << FIFO_CONFIG1_FIFO_MODE_POS), + FIFO_CONFIG1_FIFO_MODE_STREAM = (0x00 << FIFO_CONFIG1_FIFO_MODE_POS) +} FIFO_CONFIG1_FIFO_MODE_t; + +/* FIFO_BYPASS */ +typedef enum { + FIFO_CONFIG1_FIFO_BYPASS_ON = (0x01 << FIFO_CONFIG1_FIFO_BYPASS_POS), + FIFO_CONFIG1_FIFO_BYPASS_OFF = (0x00 << FIFO_CONFIG1_FIFO_BYPASS_POS), +} FIFO_CONFIG1_FIFO_BYPASS_t; + +/* + * APEX_DATA3 + * Register Name: APEX_DATA3 + */ + +/* DMP_IDLE */ +typedef enum { + APEX_DATA3_DMP_IDLE_ON = (0x01 << APEX_DATA3_DMP_IDLE_POS), + APEX_DATA3_DMP_IDLE_OFF = (0x00 << APEX_DATA3_DMP_IDLE_POS), +} APEX_DATA3_DMP_IDLE_OFF_t; + +/* ACTIVITY_CLASS */ +typedef enum { + APEX_DATA3_ACTIVITY_CLASS_OTHER = 0x0, + APEX_DATA3_ACTIVITY_CLASS_WALK = 0x1, + APEX_DATA3_ACTIVITY_CLASS_RUN = 0x2, +} APEX_DATA3_ACTIVITY_CLASS_t; + +/* + * INTF_CONFIG0 + * Register Name: INTF_CONFIG0 + */ + +/* FIFO_COUNT_REC */ +typedef enum { + INTF_CONFIG0_FIFO_COUNT_REC_RECORD = (0x01 << INTF_CONFIG0_FIFO_COUNT_FORMAT_POS), + INTF_CONFIG0_FIFO_COUNT_REC_BYTE = (0x00 << INTF_CONFIG0_FIFO_COUNT_FORMAT_POS), +} INTF_CONFIG0_FIFO_COUNT_REC_t; + +/* FIFO_COUNT_ENDIAN */ +typedef enum { + INTF_CONFIG0_FIFO_COUNT_BIG_ENDIAN = (0x01 << INTF_CONFIG0_FIFO_COUNT_ENDIAN_POS), + INTF_CONFIG0_FIFO_COUNT_LITTLE_ENDIAN = (0x00 << INTF_CONFIG0_FIFO_COUNT_ENDIAN_POS), +} INTF_CONFIG0_FIFO_COUNT_ENDIAN_t; + +/* DATA_ENDIAN */ +typedef enum { + INTF_CONFIG0_DATA_BIG_ENDIAN = (0x01 << INTF_CONFIG0_SENSOR_DATA_ENDIAN_POS), + INTF_CONFIG0_DATA_LITTLE_ENDIAN = (0x00 << INTF_CONFIG0_SENSOR_DATA_ENDIAN_POS), +} INTF_CONFIG0_DATA_ENDIAN_t; + +/* --------------------------------------------------------------------------- + * register bank MREG1 + * ---------------------------------------------------------------------------- */ + +/* + * TMST_CONFIG1_MREG1 + * Register Name: TMST_CONFIG1 + */ + +/* TMST_RES */ +typedef enum { + TMST_CONFIG1_RESOL_16us = (0x01 << TMST_CONFIG1_TMST_RES_POS), + TMST_CONFIG1_RESOL_1us = (0x00 << TMST_CONFIG1_TMST_RES_POS), +} TMST_CONFIG1_RESOL_t; + +/* TMST_FSYNC */ +typedef enum { + TMST_CONFIG1_TMST_FSYNC_EN = (0x01 << TMST_CONFIG1_TMST_FSYNC_EN_POS), + TMST_CONFIG1_TMST_FSYNC_DIS = (0x00 << TMST_CONFIG1_TMST_FSYNC_EN_POS), +} TMST_CONFIG1_TMST_FSYNC_EN_t; + +/* TMST_EN */ +typedef enum { + TMST_CONFIG1_TMST_EN = 0x01, + TMST_CONFIG1_TMST_DIS = 0x00, +} TMST_CONFIG1_TMST_EN_t; + +/* + * FIFO_CONFIG5_MREG1 + * Register Name: FIFO_CONFIG5 + */ +/* FIFO_WM_GT_TH */ +typedef enum { + FIFO_CONFIG5_WM_GT_TH_EN = (0x1 << FIFO_CONFIG5_FIFO_WM_GT_TH_POS), + FIFO_CONFIG5_WM_GT_TH_DIS = (0x0 << FIFO_CONFIG5_FIFO_WM_GT_TH_POS), +} FIFO_CONFIG5_WM_GT_t; + +/* FIFO_HIRES_EN */ +typedef enum { + FIFO_CONFIG5_HIRES_EN = (0x1 << FIFO_CONFIG5_FIFO_HIRES_EN_POS), + FIFO_CONFIG5_HIRES_DIS = (0x0 << FIFO_CONFIG5_FIFO_HIRES_EN_POS), +} FIFO_CONFIG5_HIRES_t; + +/* FIFO_TMST_FSYNC_EN */ +typedef enum { + FIFO_CONFIG5_TMST_FSYNC_EN = (0x1 << FIFO_CONFIG5_FIFO_TMST_FSYNC_EN_POS), + FIFO_CONFIG5_TMST_FSYNC_DIS = (0x0 << FIFO_CONFIG5_FIFO_TMST_FSYNC_EN_POS), +} FIFO_CONFIG5_TMST_FSYNC_t; + +#if INV_IMU_IS_GYRO_SUPPORTED +/* FIFO_GYRO_EN */ +typedef enum { + FIFO_CONFIG5_GYRO_EN = (0x1 << FIFO_CONFIG5_FIFO_GYRO_EN_POS), + FIFO_CONFIG5_GYRO_DIS = (0x0 << FIFO_CONFIG5_FIFO_GYRO_EN_POS), +} FIFO_CONFIG5_GYRO_t; +#endif + +/* FIFO_ACCEL_EN*/ +typedef enum { + FIFO_CONFIG5_ACCEL_EN = 0x01, + FIFO_CONFIG5_ACCEL_DIS = 0x00, +} FIFO_CONFIG5_ACCEL_t; + +#if INV_IMU_IS_GYRO_SUPPORTED +/* + * FSYNC_CONFIG_MREG1 + * Register Name: FSYNC_CONFIG + */ + +/* FSYNC_UI_SEL */ +typedef enum { + FSYNC_CONFIG_UI_SEL_NO = (0x0 << FSYNC_CONFIG_FSYNC_UI_SEL_POS), + FSYNC_CONFIG_UI_SEL_TEMP = (0x1 << FSYNC_CONFIG_FSYNC_UI_SEL_POS), + FSYNC_CONFIG_UI_SEL_GYRO_X = (0x2 << FSYNC_CONFIG_FSYNC_UI_SEL_POS), + FSYNC_CONFIG_UI_SEL_GYRO_Y = (0x3 << FSYNC_CONFIG_FSYNC_UI_SEL_POS), + FSYNC_CONFIG_UI_SEL_GYRO_Z = (0x4 << FSYNC_CONFIG_FSYNC_UI_SEL_POS), + FSYNC_CONFIG_UI_SEL_ACCEL_X = (0x5 << FSYNC_CONFIG_FSYNC_UI_SEL_POS), + FSYNC_CONFIG_UI_SEL_ACCEL_Y = (0x6 << FSYNC_CONFIG_FSYNC_UI_SEL_POS), + FSYNC_CONFIG_UI_SEL_ACCEL_Z = (0x7 << FSYNC_CONFIG_FSYNC_UI_SEL_POS), +} FSYNC_CONFIG_UI_SEL_t; +#endif + +/* + * ST_CONFIG_MREG1 + * Register Name: ST_CONFIG + */ +typedef enum { + ST_CONFIG_16_SAMPLES = (0 << ST_CONFIG_ST_NUMBER_SAMPLE_POS), + ST_CONFIG_200_SAMPLES = (1 << ST_CONFIG_ST_NUMBER_SAMPLE_POS), +} ST_CONFIG_NUM_SAMPLES_t; + +typedef enum { + ST_CONFIG_ACCEL_ST_LIM_50 = (7 << ST_CONFIG_ACCEL_ST_LIM_POS), +} ST_CONFIG_ACCEL_ST_LIM_t; + +#if INV_IMU_IS_GYRO_SUPPORTED +typedef enum { + ST_CONFIG_GYRO_ST_LIM_50 = (7 << ST_CONFIG_GYRO_ST_LIM_POS), +} ST_CONFIG_GYRO_ST_LIM_t; +#endif + +/* + * SELFTEST_MREG1 + * Register Name: SELFTEST + */ + +/* GYRO_ST_EN and ACCEL_ST_EN */ +typedef enum { + SELFTEST_DIS = 0, + SELFTEST_ACCEL_EN = SELFTEST_ACCEL_ST_EN_MASK, +#if INV_IMU_IS_GYRO_SUPPORTED + SELFTEST_GYRO_EN = SELFTEST_GYRO_ST_EN_MASK, + SELFTEST_EN = (SELFTEST_ACCEL_ST_EN_MASK | SELFTEST_GYRO_ST_EN_MASK) +#endif +} SELFTEST_ACCEL_GYRO_ST_EN_t; + +/* + * OTP_CONFIG_MREG1 + * Register Name: OTP_CONFIG + */ + +/* OTP_CONFIG */ +typedef enum { + OTP_CONFIG_OTP_COPY_TRIM = (1 << OTP_CONFIG_OTP_COPY_MODE_POS), + OTP_CONFIG_OTP_COPY_ST_DATA = (3 << OTP_CONFIG_OTP_COPY_MODE_POS), +} OTP_CONFIG_COPY_MODE_t; + +/* + * APEX_CONFIG2_MREG1 + * Register Name: APEX_CONFIG2 +*/ + +/* LOW_ENERGY_AMP_TH_SEL */ +typedef enum { + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_30_MG = (0 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_35_MG = (1 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_40_MG = (2 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_45_MG = (3 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_50_MG = (4 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_55_MG = (5 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_60_MG = (6 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_65_MG = (7 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_70_MG = (8 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_75_MG = (9 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_80_MG = (10 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_85_MG = (11 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_90_MG = (12 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_95_MG = (13 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_100_MG = (14 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_105_MG = (15 << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS), +} APEX_CONFIG2_LOW_ENERGY_AMP_TH_t; + +/* DMP_POWER_SAVE_TIME_SEL */ +typedef enum { + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_0_S = 0x0, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_4_S = 0x1, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_8_S = 0x2, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_12_S = 0x3, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_16_S = 0x4, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_20_S = 0x5, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_24_S = 0x6, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_28_S = 0x7, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_32_S = 0x8, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_36_S = 0x9, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_40_S = 0xA, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_44_S = 0xB, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_48_S = 0xC, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_52_S = 0xD, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_56_S = 0xE, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_60_S = 0xF, +} APEX_CONFIG2_DMP_POWER_SAVE_TIME_t; + +/* + * APEX_CONFIG3_MREG1 + * Register Name: APEX_CONFIG3 +*/ + +/* PEDO_AMP_TH_SEL */ +typedef enum { + APEX_CONFIG3_PEDO_AMP_TH_30_MG = (0 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_34_MG = (1 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_38_MG = (2 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_42_MG = (3 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_46_MG = (4 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_50_MG = (5 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_54_MG = (6 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_58_MG = (7 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_62_MG = (8 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_66_MG = (9 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_70_MG = (10 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_74_MG = (11 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_78_MG = (12 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_82_MG = (13 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_86_MG = (14 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), + APEX_CONFIG3_PEDO_AMP_TH_90_MG = (15 << APEX_CONFIG3_PED_AMP_TH_SEL_POS), +} APEX_CONFIG3_PEDO_AMP_TH_t; + +/* + * APEX_CONFIG4_MREG1 + * Register Name: APEX_CONFIG4 +*/ + +/* PEDO_SB_TIMER_TH_SEL */ +typedef enum { + APEX_CONFIG4_PEDO_SB_TIMER_TH_50_SAMPLES = (0 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS), + APEX_CONFIG4_PEDO_SB_TIMER_TH_75_SAMPLES = (1 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS), + APEX_CONFIG4_PEDO_SB_TIMER_TH_100_SAMPLES = (2 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS), + APEX_CONFIG4_PEDO_SB_TIMER_TH_125_SAMPLES = (3 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS), + APEX_CONFIG4_PEDO_SB_TIMER_TH_150_SAMPLES = (4 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS), + APEX_CONFIG4_PEDO_SB_TIMER_TH_175_SAMPLES = (5 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS), + APEX_CONFIG4_PEDO_SB_TIMER_TH_200_SAMPLES = (6 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS), + APEX_CONFIG4_PEDO_SB_TIMER_TH_225_SAMPLES = (7 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS), +} APEX_CONFIG4_PEDO_SB_TIMER_TH_t; + +/* PEDO_HI_ENRGY_TH_SEL */ +typedef enum { + APEX_CONFIG4_PEDO_HI_ENRGY_TH_88_MG = (0 << APEX_CONFIG4_PED_HI_EN_TH_SEL_POS), + APEX_CONFIG4_PEDO_HI_ENRGY_TH_104_MG = (1 << APEX_CONFIG4_PED_HI_EN_TH_SEL_POS), + APEX_CONFIG4_PEDO_HI_ENRGY_TH_133_MG = (2 << APEX_CONFIG4_PED_HI_EN_TH_SEL_POS), + APEX_CONFIG4_PEDO_HI_ENRGY_TH_155_MG = (3 << APEX_CONFIG4_PED_HI_EN_TH_SEL_POS), +} APEX_CONFIG4_PEDO_HI_ENRGY_TH_t; + +/* + * APEX_CONFIG5_MREG1 + * Register Name: APEX_CONFIG5 +*/ + +/* TILT_WAIT_TIME_SEL */ +typedef enum { + APEX_CONFIG5_TILT_WAIT_TIME_0_S = (0 << APEX_CONFIG5_TILT_WAIT_TIME_SEL_POS), + APEX_CONFIG5_TILT_WAIT_TIME_2_S = (1 << APEX_CONFIG5_TILT_WAIT_TIME_SEL_POS), + APEX_CONFIG5_TILT_WAIT_TIME_4_S = (2 << APEX_CONFIG5_TILT_WAIT_TIME_SEL_POS), + APEX_CONFIG5_TILT_WAIT_TIME_6_S = (3 << APEX_CONFIG5_TILT_WAIT_TIME_SEL_POS), +} APEX_CONFIG5_TILT_WAIT_TIME_t; + +/* LOWG_PEAK_TH_HYST_SEL */ +typedef enum { + APEX_CONFIG5_LOWG_PEAK_TH_HYST_31_MG = (0 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_63_MG = (1 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_94_MG = (2 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_125_MG = (3 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_156_MG = (4 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_188_MG = (5 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_219_MG = (6 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_250_MG = (7 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS), +} APEX_CONFIG5_LOWG_PEAK_TH_HYST_t; + +/* HIGHG_PEAK_TH_HYST_SEL */ +typedef enum { + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_31_MG = (0 << APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_63_MG = (1 << APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_94_MG = (2 << APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_125_MG = (3 << APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_156_MG = (4 << APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_188_MG = (5 << APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_219_MG = (6 << APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_250_MG = (7 << APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS), +} APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t; + +/* + * APEX_CONFIG9_MREG1 + * Register Name: APEX_CONFIG9 +*/ + +/* FF_DEBOUNCE_DURATION_SEL */ +typedef enum { + APEX_CONFIG9_FF_DEBOUNCE_DURATION_0_MS = (0 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1250_MS = (1 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1375_MS = (2 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1500_MS = (3 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1625_MS = (4 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1750_MS = (5 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1875_MS = (6 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2000_MS = (7 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2125_MS = (8 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2250_MS = (9 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2375_MS = (10 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2500_MS = (11 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2625_MS = (12 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2750_MS = (13 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2875_MS = (14 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_3000_MS = (15 << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS), +} APEX_CONFIG9_FF_DEBOUNCE_DURATION_t; + +/* SMD_SENSITIVITY_SEL */ +typedef enum { + APEX_CONFIG9_SMD_SENSITIVITY_0 = (0 << APEX_CONFIG9_SMD_SENSITIVITY_SEL_POS), + APEX_CONFIG9_SMD_SENSITIVITY_1 = (1 << APEX_CONFIG9_SMD_SENSITIVITY_SEL_POS), + APEX_CONFIG9_SMD_SENSITIVITY_2 = (2 << APEX_CONFIG9_SMD_SENSITIVITY_SEL_POS), + APEX_CONFIG9_SMD_SENSITIVITY_3 = (3 << APEX_CONFIG9_SMD_SENSITIVITY_SEL_POS), + APEX_CONFIG9_SMD_SENSITIVITY_4 = (4 << APEX_CONFIG9_SMD_SENSITIVITY_SEL_POS), +} APEX_CONFIG9_SMD_SENSITIVITY_t; + +/* SMD_SENSITIVITY_MODE */ +typedef enum { + APEX_CONFIG9_SENSITIVITY_MODE_NORMAL = (0 << APEX_CONFIG9_SENSITIVITY_MODE_POS), + APEX_CONFIG9_SENSITIVITY_MODE_SLOW_WALK = (1 << APEX_CONFIG9_SENSITIVITY_MODE_POS), +} APEX_CONFIG9_SENSITIVITY_MODE_t; + +/* + * APEX_CONFIG10_MREG1 + * Register Name: APEX_CONFIG10 +*/ + +/* LOWG_PEAK_TH_SEL */ +typedef enum { + APEX_CONFIG10_LOWG_PEAK_TH_31_MG = (0x00 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_63_MG = (0x01 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_94_MG = (0x02 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_125_MG = (0x03 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_156_MG = (0x04 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_188_MG = (0x05 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_219_MG = (0x06 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_250_MG = (0x07 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_281_MG = (0x08 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_313_MG = (0x09 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_344_MG = (0x0A << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_375_MG = (0x0B << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_406_MG = (0x0C << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_438_MG = (0x0D << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_469_MG = (0x0E << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_500_MG = (0x0F << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_531_MG = (0x10 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_563_MG = (0x11 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_594_MG = (0x12 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_625_MG = (0x13 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_656_MG = (0x14 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_688_MG = (0x15 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_719_MG = (0x16 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_750_MG = (0x17 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_781_MG = (0x18 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_813_MG = (0x19 << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_844_MG = (0x1A << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_875_MG = (0x1B << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_906_MG = (0x1C << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_938_MG = (0x1D << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_969_MG = (0x1E << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), + APEX_CONFIG10_LOWG_PEAK_TH_1000_MG = (0x1F << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS), +} APEX_CONFIG10_LOWG_PEAK_TH_t; + +/* LOWG_TIME_TH_SEL */ +typedef enum { + APEX_CONFIG10_LOWG_TIME_TH_1_SAMPLE = (0x00 << APEX_CONFIG10_LOWG_TIME_TH_SEL_POS), + APEX_CONFIG10_LOWG_TIME_TH_2_SAMPLES = (0x01 << APEX_CONFIG10_LOWG_TIME_TH_SEL_POS), + APEX_CONFIG10_LOWG_TIME_TH_3_SAMPLES = (0x02 << APEX_CONFIG10_LOWG_TIME_TH_SEL_POS), + APEX_CONFIG10_LOWG_TIME_TH_4_SAMPLES = (0x03 << APEX_CONFIG10_LOWG_TIME_TH_SEL_POS), + APEX_CONFIG10_LOWG_TIME_TH_5_SAMPLES = (0x04 << APEX_CONFIG10_LOWG_TIME_TH_SEL_POS), + APEX_CONFIG10_LOWG_TIME_TH_6_SAMPLES = (0x05 << APEX_CONFIG10_LOWG_TIME_TH_SEL_POS), + APEX_CONFIG10_LOWG_TIME_TH_7_SAMPLES = (0x06 << APEX_CONFIG10_LOWG_TIME_TH_SEL_POS), + APEX_CONFIG10_LOWG_TIME_TH_8_SAMPLES = (0x07 << APEX_CONFIG10_LOWG_TIME_TH_SEL_POS), +} APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t; + +/* + * APEX_CONFIG11_MREG1 + * Register Name: APEX_CONFIG11 +*/ + +/* HIGHG_PEAK_TH_SEL */ +typedef enum { + APEX_CONFIG11_HIGHG_PEAK_TH_250_MG = (0x00 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_500_MG = (0x01 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_750_MG = (0x02 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_1000MG = (0x03 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_1250_MG = (0x04 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_1500_MG = (0x05 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_1750_MG = (0x06 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_2000_MG = (0x07 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_2250_MG = (0x08 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_2500_MG = (0x09 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_2750_MG = (0x0A << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_3000_MG = (0x0B << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_3250_MG = (0x0C << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_3500_MG = (0x0D << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_3750_MG = (0x0E << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_4000_MG = (0x0F << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_4250_MG = (0x10 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_4500_MG = (0x11 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_4750_MG = (0x12 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_5000_MG = (0x13 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_5250_MG = (0x14 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_5500_MG = (0x15 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_5750_MG = (0x16 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_6000_MG = (0x17 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_6250_MG = (0x18 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_6500_MG = (0x19 << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_6750_MG = (0x1A << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_7000_MG = (0x1B << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_7250_MG = (0x1C << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_7500_MG = (0x1D << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), + APEX_CONFIG11_HIGHG_PEAK_TH_7750_MG = (0x1E << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS), +} APEX_CONFIG11_HIGHG_PEAK_TH_t; + +/* HIGHG_TIME_TH_SEL */ +typedef enum { + APEX_CONFIG11_HIGHG_TIME_TH_1_SAMPLE = (0x00 << APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS), + APEX_CONFIG11_HIGHG_TIME_TH_2_SAMPLES = (0x01 << APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS), + APEX_CONFIG11_HIGHG_TIME_TH_3_SAMPLES = (0x02 << APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS), + APEX_CONFIG11_HIGHG_TIME_TH_4_SAMPLES = (0x03 << APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS), + APEX_CONFIG11_HIGHG_TIME_TH_5_SAMPLES = (0x04 << APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS), + APEX_CONFIG11_HIGHG_TIME_TH_6_SAMPLES = (0x05 << APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS), + APEX_CONFIG11_HIGHG_TIME_TH_7_SAMPLES = (0x06 << APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS), + APEX_CONFIG11_HIGHG_TIME_TH_8_SAMPLES = (0x07 << APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS), +} APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t; + +/* + * FDR_CONFIG_MREG1 + * Register Name: FDR_CONFIG + */ + +/* FDR_SEL */ +typedef enum { + FDR_CONFIG_FDR_SEL_DIS = (0x0 << FDR_CONFIG_FDR_SEL_POS), + FDR_CONFIG_FDR_SEL_FACTOR_2 = (0x8 << FDR_CONFIG_FDR_SEL_POS), + FDR_CONFIG_FDR_SEL_FACTOR_4 = (0x9 << FDR_CONFIG_FDR_SEL_POS), + FDR_CONFIG_FDR_SEL_FACTOR_8 = (0xA << FDR_CONFIG_FDR_SEL_POS), + FDR_CONFIG_FDR_SEL_FACTOR_16 = (0xB << FDR_CONFIG_FDR_SEL_POS), + FDR_CONFIG_FDR_SEL_FACTOR_32 = (0xC << FDR_CONFIG_FDR_SEL_POS), + FDR_CONFIG_FDR_SEL_FACTOR_64 = (0xD << FDR_CONFIG_FDR_SEL_POS), + FDR_CONFIG_FDR_SEL_FACTOR_128 = (0xE << FDR_CONFIG_FDR_SEL_POS), + FDR_CONFIG_FDR_SEL_FACTOR_256 = (0xF << FDR_CONFIG_FDR_SEL_POS), +} FDR_CONFIG_FDR_SEL_t; + +/* + * APEX_CONFIG12_MREG1 + * Register Name: APEX_CONFIG12 +*/ +/* FF_MAX_DURATION_SEL */ +typedef enum { + APEX_CONFIG12_FF_MAX_DURATION_102_CM = (0 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_120_CM = (1 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_139_CM = (2 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_159_CM = (3 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_181_CM = (4 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_204_CM = (5 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_228_CM = (6 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_254_CM = (7 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_281_CM = (8 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_310_CM = (9 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_339_CM = (10 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_371_CM = (11 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_403_CM = (12 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_438_CM = (13 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_473_CM = (14 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), + APEX_CONFIG12_FF_MAX_DURATION_510_CM = (15 << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS), +} APEX_CONFIG12_FF_MAX_DURATION_t; + +/* FF_MIN_DURATION_SEL */ +typedef enum { + APEX_CONFIG12_FF_MIN_DURATION_10_CM = (0 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_12_CM = (1 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_13_CM = (2 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_16_CM = (3 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_18_CM = (4 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_20_CM = (5 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_23_CM = (6 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_25_CM = (7 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_28_CM = (8 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_31_CM = (9 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_34_CM = (10 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_38_CM = (11 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_41_CM = (12 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_45_CM = (13 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_48_CM = (14 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), + APEX_CONFIG12_FF_MIN_DURATION_52_CM = (15 << APEX_CONFIG12_FF_MIN_DURATION_SEL_POS), +} APEX_CONFIG12_FF_MIN_DURATION_t; + +/* --------------------------------------------------------------------------- + * register bank MREG2 + * ---------------------------------------------------------------------------- */ + +/* + * OTP_CTRL7_MREG2 + * Register Name: OTP_CTRL7 +*/ + +/* OTP_CTRL7 */ +typedef enum { + OTP_CTRL7_OTP_RELOAD_EN = (1 << OTP_CTRL7_OTP_RELOAD_POS), + OTP_CTRL7_OTP_RELOAD_DIS = (0 << OTP_CTRL7_OTP_RELOAD_POS), +} OTP_CTRL7_OTP_RELOAD_t; + +/* OTP_PWR_DOWN */ +typedef enum { + OTP_CTRL7_PWR_DOWN_EN = (1 << OTP_CTRL7_OTP_PWR_DOWN_POS), + OTP_CTRL7_PWR_DOWN_DIS = (0 << OTP_CTRL7_OTP_PWR_DOWN_POS), +} OTP_CTRL7_PWR_DOWN_t; + +#ifdef __cplusplus +} +#endif + +#endif /* #ifndef _INV_IMU_DEFS_H_ */ diff --git a/icm42670p/imu/inv_imu_driver.c b/icm42670p/imu/inv_imu_driver.c new file mode 100644 index 0000000..1bfe9fe --- /dev/null +++ b/icm42670p/imu/inv_imu_driver.c @@ -0,0 +1,1685 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#include "imu/inv_imu_driver.h" +#include "imu/inv_imu_extfunc.h" +#include "imu/inv_imu_version.h" + +#include /* NULL */ +#include /* memset */ + +#if INV_IMU_HFSR_SUPPORTED +/* Address of DMP_CONFIG1 register */ +#define DMP_CONFIG1_MREG1 0x2c +/* SRAM first bank ID */ +#define SRAM_START_BANK 0x50 +#endif + +/* Static functions declaration */ +static int select_rcosc(inv_imu_device_t *s); +static int select_wuosc(inv_imu_device_t *s); +static int configure_serial_interface(inv_imu_device_t *s); +static int init_hardware_from_ui(inv_imu_device_t *s); +#if INV_IMU_HFSR_SUPPORTED +static int read_and_check_sram(struct inv_imu_device *self, const uint8_t *data, uint32_t offset, + uint32_t size); +#endif + +int inv_imu_init(inv_imu_device_t *s, const struct inv_imu_serif *serif, + void (*sensor_event_cb)(inv_imu_sensor_event_t *event)) +{ + int status = 0; + + memset(s, 0, sizeof(*s)); + + /* Verify validity of `serif` variable */ + if (serif == NULL || serif->read_reg == NULL || serif->write_reg == NULL) + return INV_ERROR; + + s->transport.serif = *serif; + + /* Supply ramp time max is 3 ms */ + inv_imu_sleep_us(3000); + + /* Register sensor event callback */ + s->sensor_event_cb = sensor_event_cb; + + /* Make sure `need_mclk_cnt` is cleared */ + s->transport.need_mclk_cnt = 0; + + /* Configure serial interface so we can trigger device reset */ + status |= configure_serial_interface(s); + + /* Reset device */ + status |= inv_imu_device_reset(s); + + /* Init transport layer */ + status |= inv_imu_init_transport(s); + + /* Read and set endianness for further processing */ + status |= inv_imu_get_endianness(s); + + /* Initialize hardware */ + status |= init_hardware_from_ui(s); + + /* Set default value for sensor start/stop time */ +#if INV_IMU_IS_GYRO_SUPPORTED + s->gyro_start_time_us = UINT32_MAX; +#endif + s->accel_start_time_us = UINT32_MAX; + + return status; +} + +int inv_imu_device_reset(inv_imu_device_t *s) +{ + int status = INV_ERROR_SUCCESS; + uint8_t data; + + /* Ensure BLK_SEL_R and BLK_SEL_W are set to 0 */ + data = 0; + status |= inv_imu_write_reg(s, BLK_SEL_R, 1, &data); + status |= inv_imu_write_reg(s, BLK_SEL_W, 1, &data); + + /* Trigger soft reset */ + data = (uint8_t)SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_EN; + status |= inv_imu_write_reg(s, SIGNAL_PATH_RESET, 1, &data); + + /* Wait 1ms for soft reset to be effective */ + inv_imu_sleep_us(1000); + + /* Re-configure serial interface since it was reset */ + status |= configure_serial_interface(s); + + /* Clear the reset done interrupt */ + status |= inv_imu_read_reg(s, INT_STATUS, 1, &data); + if (data != INT_STATUS_RESET_DONE_INT_MASK) + status |= INV_ERROR_UNEXPECTED; + + return status; +} + +int inv_imu_get_who_am_i(inv_imu_device_t *s, uint8_t *who_am_i) +{ + return inv_imu_read_reg(s, WHO_AM_I, 1, who_am_i); +} + +int inv_imu_enable_accel_low_power_mode(inv_imu_device_t *s) +{ + int status = 0; + PWR_MGMT0_ACCEL_MODE_t accel_mode; +#if INV_IMU_IS_GYRO_SUPPORTED + PWR_MGMT0_GYRO_MODE_t gyro_mode; +#endif + ACCEL_CONFIG0_ODR_t acc_odr_bitfield; + uint32_t accel_odr_us = 0; + uint8_t pwr_mgmt0_reg; + uint8_t accel_config0_reg; + uint8_t value; + + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + accel_mode = (PWR_MGMT0_ACCEL_MODE_t)(pwr_mgmt0_reg & PWR_MGMT0_ACCEL_MODE_MASK); +#if INV_IMU_IS_GYRO_SUPPORTED + gyro_mode = (PWR_MGMT0_GYRO_MODE_t)(pwr_mgmt0_reg & PWR_MGMT0_GYRO_MODE_MASK); +#endif + + /* Check if the accelerometer is the only one enabled */ + if ((accel_mode != PWR_MGMT0_ACCEL_MODE_LP) +#if INV_IMU_IS_GYRO_SUPPORTED + && ((gyro_mode == PWR_MGMT0_GYRO_MODE_OFF) || (gyro_mode == PWR_MGMT0_GYRO_MODE_STANDBY)) +#endif + ) { + /* Get accelerometer's ODR for next required wait */ + status |= inv_imu_read_reg(s, ACCEL_CONFIG0, 1, &accel_config0_reg); + acc_odr_bitfield = (ACCEL_CONFIG0_ODR_t)(accel_config0_reg & ACCEL_CONFIG0_ACCEL_ODR_MASK); + accel_odr_us = inv_imu_convert_odr_bitfield_to_us(acc_odr_bitfield); + /* Select the RC OSC as clock source for the accelerometer */ + status |= select_rcosc(s); + } + + /* Enable/Switch the accelerometer in/to low power mode */ + /* Read a new time because select_rcosc() modified it */ + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + pwr_mgmt0_reg &= ~PWR_MGMT0_ACCEL_MODE_MASK; + pwr_mgmt0_reg |= PWR_MGMT0_ACCEL_MODE_LP; + status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + inv_imu_sleep_us(200); + + if ((accel_mode != PWR_MGMT0_ACCEL_MODE_LP) +#if INV_IMU_IS_GYRO_SUPPORTED + && ((gyro_mode == PWR_MGMT0_GYRO_MODE_OFF) || (gyro_mode == PWR_MGMT0_GYRO_MODE_STANDBY)) +#endif + ) { + /* + * Wait one accelerometer ODR before switching to the WU OSC. + * if ODR is smaller than 200 us, we already waited for one ODR above. + */ + if (accel_odr_us > 200) + inv_imu_sleep_us(accel_odr_us - 200); + status |= select_wuosc(s); + } + + if (accel_mode == PWR_MGMT0_ACCEL_MODE_OFF && s->fifo_is_used) { + /* + * First data are wrong after accel enable using IIR filter + * There is no signal that says accel start-up has completed and data are stable + * using FIR filter. So keep track of the time at start-up to discard the invalid data, + * about 20 ms after enable. + */ + s->accel_start_time_us = inv_imu_get_time_us(); + } + + /* Enable the automatic RCOSC power on so that FIFO is entirely powered on */ + status |= inv_imu_read_reg(s, FIFO_CONFIG6_MREG1, 1, &value); + value &= ~FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_MASK; + status |= inv_imu_write_reg(s, FIFO_CONFIG6_MREG1, 1, &value); + + return status; +} + +int inv_imu_enable_accel_low_noise_mode(inv_imu_device_t *s) +{ + int status = 0; + PWR_MGMT0_ACCEL_MODE_t accel_mode; +#if INV_IMU_IS_GYRO_SUPPORTED + PWR_MGMT0_GYRO_MODE_t gyro_mode; +#endif + ACCEL_CONFIG0_ODR_t acc_odr_bitfield; + uint8_t pwr_mgmt0_reg; + uint8_t accel_config0_reg; + + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + accel_mode = (PWR_MGMT0_ACCEL_MODE_t)(pwr_mgmt0_reg & PWR_MGMT0_ACCEL_MODE_MASK); +#if INV_IMU_IS_GYRO_SUPPORTED + gyro_mode = (PWR_MGMT0_GYRO_MODE_t)(pwr_mgmt0_reg & PWR_MGMT0_GYRO_MODE_MASK); +#endif + + /* Check if the accelerometer is the only one enabled */ + if ((accel_mode == PWR_MGMT0_ACCEL_MODE_LP) +#if INV_IMU_IS_GYRO_SUPPORTED + && ((gyro_mode == PWR_MGMT0_GYRO_MODE_OFF) || (gyro_mode == PWR_MGMT0_GYRO_MODE_STANDBY)) +#endif + ) { + uint32_t accel_odr_us; + /* Get accelerometer's ODR for next required wait */ + status |= inv_imu_read_reg(s, ACCEL_CONFIG0, 1, &accel_config0_reg); + acc_odr_bitfield = (ACCEL_CONFIG0_ODR_t)(accel_config0_reg & ACCEL_CONFIG0_ACCEL_ODR_MASK); + accel_odr_us = inv_imu_convert_odr_bitfield_to_us(acc_odr_bitfield); + /* Select the RC OSC as clock source for the accelerometer */ + status |= select_rcosc(s); + /* Wait one accel ODR before switching to low noise mode */ + inv_imu_sleep_us(accel_odr_us); + } + + /* Enable/Switch the accelerometer in/to low noise mode */ + /* Read a new time because select_rcosc() modified it */ + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + pwr_mgmt0_reg &= ~PWR_MGMT0_ACCEL_MODE_MASK; + pwr_mgmt0_reg |= PWR_MGMT0_ACCEL_MODE_LN; + status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + inv_imu_sleep_us(200); + + if (accel_mode == PWR_MGMT0_ACCEL_MODE_OFF && s->fifo_is_used) { + /* + * First data are wrong after accel enable using IIR filter + * There is no signal that says accel start-up has completed and data are stable + * using FIR filter. So keep track of the time at start-up to discard the invalid data, + * about 20ms after enable. + */ + s->accel_start_time_us = inv_imu_get_time_us(); + } + + return status; +} + +int inv_imu_disable_accel(inv_imu_device_t *s) +{ + int status = 0; + int stop_fifo_usage = 0; + uint32_t accel_odr_us; +#if INV_IMU_IS_GYRO_SUPPORTED + PWR_MGMT0_GYRO_MODE_t gyro_mode; +#endif + ACCEL_CONFIG0_ODR_t acc_odr_bitfield; + uint8_t pwr_mgmt0_reg; + uint8_t accel_config0_reg; + uint8_t fifo_cfg_6_reg; + + /* Get accelerometer's ODR for next required wait */ + status |= inv_imu_read_reg(s, ACCEL_CONFIG0, 1, &accel_config0_reg); + acc_odr_bitfield = (ACCEL_CONFIG0_ODR_t)(accel_config0_reg & ACCEL_CONFIG0_ACCEL_ODR_MASK); + accel_odr_us = inv_imu_convert_odr_bitfield_to_us(acc_odr_bitfield); + + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); +#if INV_IMU_IS_GYRO_SUPPORTED + gyro_mode = (PWR_MGMT0_GYRO_MODE_t)(pwr_mgmt0_reg & PWR_MGMT0_GYRO_MODE_MASK); +#endif + if (s->fifo_is_used +#if INV_IMU_IS_GYRO_SUPPORTED + && (gyro_mode == PWR_MGMT0_GYRO_MODE_OFF) +#endif + ) { + /* + * Accel is off and gyro is about to be turned off. + * Flush FIFO so that there is no old data at next enable time + */ + stop_fifo_usage = 1; + } + + /* Check if accel is the last sensor enabled and bit rcosc dis is not set */ + status |= inv_imu_read_reg(s, FIFO_CONFIG6_MREG1, 1, &fifo_cfg_6_reg); + if (((fifo_cfg_6_reg & FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_MASK) == 0) +#if INV_IMU_IS_GYRO_SUPPORTED + && (gyro_mode == PWR_MGMT0_GYRO_MODE_OFF) +#endif + ) { + /* + * Disable the automatic RCOSC power on to avoid extra power consumption + * in sleep mode (all sensors and clocks off) + */ + fifo_cfg_6_reg |= ((1 & FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_MASK) + << FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_POS); + status |= inv_imu_write_reg(s, FIFO_CONFIG6_MREG1, 1, &fifo_cfg_6_reg); + inv_imu_sleep_us(accel_odr_us); + } + + pwr_mgmt0_reg &= ~PWR_MGMT0_ACCEL_MODE_MASK; + pwr_mgmt0_reg |= PWR_MGMT0_ACCEL_MODE_OFF; + status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + + if (stop_fifo_usage && s->fifo_is_used) { + /* Reset FIFO explicitly so there is no data left in FIFO once all sensors are off */ + status |= inv_imu_reset_fifo(s); + } + + return status; +} + +int inv_imu_set_accel_frequency(inv_imu_device_t *s, const ACCEL_CONFIG0_ODR_t frequency) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, ACCEL_CONFIG0, 1, &data); + data &= ~ACCEL_CONFIG0_ACCEL_ODR_MASK; + data |= frequency; + status |= inv_imu_write_reg(s, ACCEL_CONFIG0, 1, &data); + + return status; +} + +int inv_imu_set_accel_lp_avg(inv_imu_device_t *s, ACCEL_CONFIG1_ACCEL_FILT_AVG_t acc_avg) +{ + uint8_t value; + int status = 0; + + status |= inv_imu_read_reg(s, ACCEL_CONFIG1, 1, &value); + if (status) + return status; + + value &= ~ACCEL_CONFIG1_ACCEL_UI_AVG_MASK; + value |= acc_avg; + + status |= inv_imu_write_reg(s, ACCEL_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_set_accel_ln_bw(inv_imu_device_t *s, ACCEL_CONFIG1_ACCEL_FILT_BW_t acc_bw) +{ + uint8_t value; + int status = 0; + + status |= inv_imu_read_reg(s, ACCEL_CONFIG1, 1, &value); + if (status) + return status; + + value &= ~ACCEL_CONFIG1_ACCEL_UI_FILT_BW_MASK; + value |= acc_bw; + + status |= inv_imu_write_reg(s, ACCEL_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_set_accel_fsr(inv_imu_device_t *s, ACCEL_CONFIG0_FS_SEL_t accel_fsr_g) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, ACCEL_CONFIG0, 1, &data); + data &= ~ACCEL_CONFIG0_ACCEL_UI_FS_SEL_MASK; + data |= accel_fsr_g; + status |= inv_imu_write_reg(s, ACCEL_CONFIG0, 1, &data); + + return status; +} + +int inv_imu_get_accel_fsr(inv_imu_device_t *s, ACCEL_CONFIG0_FS_SEL_t *accel_fsr_g) +{ + int status = 0; + uint8_t accel_config0_reg; + + if ((s->fifo_highres_enabled) && (s->fifo_is_used == INV_IMU_FIFO_ENABLED)) + *accel_fsr_g = ACCEL_CONFIG0_FS_SEL_MAX; + else { + status |= inv_imu_read_reg(s, ACCEL_CONFIG0, 1, &accel_config0_reg); + *accel_fsr_g = + (ACCEL_CONFIG0_FS_SEL_t)(accel_config0_reg & ACCEL_CONFIG0_ACCEL_UI_FS_SEL_MASK); + } + + return status; +} + +#if INV_IMU_IS_GYRO_SUPPORTED +int inv_imu_enable_gyro_low_noise_mode(inv_imu_device_t *s) +{ + int status = 0; + PWR_MGMT0_ACCEL_MODE_t accel_mode; + PWR_MGMT0_GYRO_MODE_t gyro_mode; + ACCEL_CONFIG0_ODR_t acc_odr_bitfield; + uint8_t pwr_mgmt0_reg; + uint8_t accel_config0_reg; + + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + accel_mode = (PWR_MGMT0_ACCEL_MODE_t)(pwr_mgmt0_reg & PWR_MGMT0_ACCEL_MODE_MASK); + gyro_mode = (PWR_MGMT0_GYRO_MODE_t)(pwr_mgmt0_reg & PWR_MGMT0_GYRO_MODE_MASK); + /* Check if the accelerometer is the only one enabled */ + if ((accel_mode == PWR_MGMT0_ACCEL_MODE_LP) && + ((gyro_mode == PWR_MGMT0_GYRO_MODE_OFF) || (gyro_mode == PWR_MGMT0_GYRO_MODE_STANDBY))) { + uint32_t accel_odr_us; + /* Get accelerometer's ODR for next required wait */ + status |= inv_imu_read_reg(s, ACCEL_CONFIG0, 1, &accel_config0_reg); + acc_odr_bitfield = (ACCEL_CONFIG0_ODR_t)(accel_config0_reg & ACCEL_CONFIG0_ACCEL_ODR_MASK); + accel_odr_us = inv_imu_convert_odr_bitfield_to_us(acc_odr_bitfield); + /* Select the RC OSC as clock source for the accelerometer */ + status |= select_rcosc(s); + /* Wait one accel ODR before enabling the gyroscope */ + inv_imu_sleep_us(accel_odr_us); + } + + /* Enable/Switch the gyroscope in/to low noise mode */ + /* Read a new time because select_rcosc() modified it */ + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + pwr_mgmt0_reg &= ~PWR_MGMT0_GYRO_MODE_MASK; + pwr_mgmt0_reg |= (uint8_t)PWR_MGMT0_GYRO_MODE_LN; + status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + inv_imu_sleep_us(200); + + if (gyro_mode == PWR_MGMT0_GYRO_MODE_OFF && s->fifo_is_used) { + /* + * First data are wrong after gyro enable using IIR filter + * There is no signal that says Gyro start-up has completed and data are stable + * using FIR filter and the Gyro max start-up time is 40ms. So keep track of the time + * at start-up to discard the invalid data, about 60ms after enable. + */ + s->gyro_start_time_us = inv_imu_get_time_us(); + } + + return status; +} + +int inv_imu_disable_gyro(inv_imu_device_t *s) +{ + int status = 0; + int stop_fifo_usage = 0; + ACCEL_CONFIG0_ODR_t acc_odr_bitfield; + uint32_t accel_odr_us; + PWR_MGMT0_ACCEL_MODE_t accel_mode; + uint8_t pwr_mgmt0_reg; + uint8_t accel_config0_reg; + uint8_t fifo_cfg_6_reg; + + /* Get accelerometer's ODR for next required wait */ + status |= inv_imu_read_reg(s, ACCEL_CONFIG0, 1, &accel_config0_reg); + acc_odr_bitfield = (ACCEL_CONFIG0_ODR_t)(accel_config0_reg & ACCEL_CONFIG0_ACCEL_ODR_MASK); + accel_odr_us = inv_imu_convert_odr_bitfield_to_us(acc_odr_bitfield); + + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + accel_mode = (PWR_MGMT0_ACCEL_MODE_t)(pwr_mgmt0_reg & PWR_MGMT0_ACCEL_MODE_MASK); + + if ((accel_mode == PWR_MGMT0_ACCEL_MODE_OFF) && s->fifo_is_used) { + /* + * Accel is off and gyro is about to be turned off. + * Flush FIFO so that there is no old data at next enable time + */ + stop_fifo_usage = 1; + } + + /* Check if the accelerometer is enabled in low power mode */ + if (accel_mode == PWR_MGMT0_ACCEL_MODE_LP) { + /* Select the RC OSC as clock source for the accelerometer */ + status |= select_rcosc(s); + } + + /* Check if gyro is the last sensor enabled and bit rcosc dis is not set */ + status |= inv_imu_read_reg(s, FIFO_CONFIG6_MREG1, 1, &fifo_cfg_6_reg); + if ((accel_mode == PWR_MGMT0_ACCEL_MODE_OFF) && + ((fifo_cfg_6_reg & FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_MASK) == 0)) { + GYRO_CONFIG0_ODR_t gyro_odr_bitfield; + uint32_t gyro_odr_us; + uint8_t gyro_config0_reg; + + /* Read gyro odr to apply it to the sleep */ + status |= inv_imu_read_reg(s, GYRO_CONFIG0, 1, &gyro_config0_reg); + gyro_odr_bitfield = (GYRO_CONFIG0_ODR_t)(gyro_config0_reg & GYRO_CONFIG0_GYRO_ODR_MASK); + gyro_odr_us = inv_imu_convert_odr_bitfield_to_us(gyro_odr_bitfield); + + /* + * Disable the automatic RCOSC power on to avoid extra power consumption + * in sleep mode (all sensors and clocks off) + */ + fifo_cfg_6_reg |= ((1 & FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_MASK) + << FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_POS); + status |= inv_imu_write_reg(s, FIFO_CONFIG6_MREG1, 1, &fifo_cfg_6_reg); + inv_imu_sleep_us(gyro_odr_us); + } + + /* Read a new time because select_rcosc() modified it */ + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + pwr_mgmt0_reg &= ~PWR_MGMT0_GYRO_MODE_MASK; + pwr_mgmt0_reg |= PWR_MGMT0_GYRO_MODE_OFF; + status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &pwr_mgmt0_reg); + + if (accel_mode == PWR_MGMT0_ACCEL_MODE_LP) { + /* Wait based on accelerometer ODR */ + inv_imu_sleep_us(2 * accel_odr_us); + /* Select the WU OSC as clock source for the accelerometer */ + status |= select_wuosc(s); + } + + if (stop_fifo_usage && s->fifo_is_used) { + /* Reset FIFO explicitly so there is no data left in FIFO once all sensors are off */ + status |= inv_imu_reset_fifo(s); + } + + return status; +} + +int inv_imu_set_gyro_frequency(inv_imu_device_t *s, const GYRO_CONFIG0_ODR_t frequency) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, GYRO_CONFIG0, 1, &data); + data &= ~GYRO_CONFIG0_GYRO_ODR_MASK; + data |= frequency; + status |= inv_imu_write_reg(s, GYRO_CONFIG0, 1, &data); + + return status; +} + +int inv_imu_set_gyro_ln_bw(inv_imu_device_t *s, GYRO_CONFIG1_GYRO_FILT_BW_t gyr_bw) +{ + uint8_t value; + int status = 0; + + status |= inv_imu_read_reg(s, GYRO_CONFIG1, 1, &value); + if (status) + return status; + + value &= ~GYRO_CONFIG1_GYRO_UI_FILT_BW_MASK; + value |= gyr_bw; + + status |= inv_imu_write_reg(s, GYRO_CONFIG1, 1, &value); + + return status; +} + +int inv_imu_set_gyro_fsr(inv_imu_device_t *s, GYRO_CONFIG0_FS_SEL_t gyro_fsr_dps) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, GYRO_CONFIG0, 1, &data); + data &= ~GYRO_CONFIG0_GYRO_UI_FS_SEL_MASK; + data |= gyro_fsr_dps; + status |= inv_imu_write_reg(s, GYRO_CONFIG0, 1, &data); + + return status; +} + +int inv_imu_get_gyro_fsr(inv_imu_device_t *s, GYRO_CONFIG0_FS_SEL_t *gyro_fsr_dps) +{ + int status = 0; + uint8_t gyro_config0_reg; + + if ((s->fifo_highres_enabled) && (s->fifo_is_used == INV_IMU_FIFO_ENABLED)) + *gyro_fsr_dps = GYRO_CONFIG0_FS_SEL_MAX; + else { + status |= inv_imu_read_reg(s, GYRO_CONFIG0, 1, &gyro_config0_reg); + *gyro_fsr_dps = + (GYRO_CONFIG0_FS_SEL_t)(gyro_config0_reg & GYRO_CONFIG0_GYRO_UI_FS_SEL_MASK); + } + + return status; +} + +int inv_imu_enable_fsync(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_switch_on_mclk(s); + + //Enable Fsync + status |= inv_imu_read_reg(s, FSYNC_CONFIG_MREG1, 1, &value); + value &= ~FSYNC_CONFIG_FSYNC_UI_SEL_MASK; + value |= (uint8_t)FSYNC_CONFIG_UI_SEL_TEMP; + status |= inv_imu_write_reg(s, FSYNC_CONFIG_MREG1, 1, &value); + + status |= inv_imu_read_reg(s, TMST_CONFIG1_MREG1, 1, &value); + value &= ~TMST_CONFIG1_TMST_FSYNC_EN_MASK; + value |= TMST_CONFIG1_TMST_FSYNC_EN; + status |= inv_imu_write_reg(s, TMST_CONFIG1_MREG1, 1, &value); + + status |= inv_imu_switch_off_mclk(s); + + return status; +} + +int inv_imu_disable_fsync(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_switch_on_mclk(s); + + // Disable Fsync + status |= inv_imu_read_reg(s, FSYNC_CONFIG_MREG1, 1, &value); + value &= ~FSYNC_CONFIG_FSYNC_UI_SEL_MASK; + value |= (uint8_t)FSYNC_CONFIG_UI_SEL_NO; + status |= inv_imu_write_reg(s, FSYNC_CONFIG_MREG1, 1, &value); + + status |= inv_imu_read_reg(s, TMST_CONFIG1_MREG1, 1, &value); + value &= ~TMST_CONFIG1_TMST_FSYNC_EN_MASK; + value |= TMST_CONFIG1_TMST_FSYNC_DIS; + status |= inv_imu_write_reg(s, TMST_CONFIG1_MREG1, 1, &value); + + status |= inv_imu_switch_off_mclk(s); + + return status; +} +#endif /* INV_IMU_IS_GYRO_SUPPORTED */ + +int inv_imu_set_spi_slew_rate(inv_imu_device_t *s, const DRIVE_CONFIG3_SPI_SLEW_RATE_t slew_rate) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_read_reg(s, DRIVE_CONFIG3, 1, &value); + value &= ~DRIVE_CONFIG3_SPI_SLEW_RATE_MASK; + value |= slew_rate; + status |= inv_imu_write_reg(s, DRIVE_CONFIG3, 1, &value); + + return status; +} + +int inv_imu_set_pin_config_int1(inv_imu_device_t *s, const inv_imu_int1_pin_config_t *conf) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_read_reg(s, INT_CONFIG, 1, &value); + value &= ~INT_CONFIG_INT1_POLARITY_MASK; + value &= ~INT_CONFIG_INT1_MODE_MASK; + value &= ~INT_CONFIG_INT1_DRIVE_CIRCUIT_MASK; + value |= conf->int_polarity; + value |= conf->int_mode; + value |= conf->int_drive; + status |= inv_imu_write_reg(s, INT_CONFIG, 1, &value); + + return status; +} + +int inv_imu_set_pin_config_int2(inv_imu_device_t *s, const inv_imu_int2_pin_config_t *conf) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_read_reg(s, INT_CONFIG, 1, &value); + value &= ~INT_CONFIG_INT2_POLARITY_MASK; + value &= ~INT_CONFIG_INT2_MODE_MASK; + value &= ~INT_CONFIG_INT2_DRIVE_CIRCUIT_MASK; + value |= conf->int_polarity; + value |= conf->int_mode; + value |= conf->int_drive; + status |= inv_imu_write_reg(s, INT_CONFIG, 1, &value); + + return status; +} + +int inv_imu_get_config_int1(inv_imu_device_t *s, inv_imu_interrupt_parameter_t *it) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, INT_SOURCE0, 1, &data); + it->INV_UI_FSYNC = (inv_imu_interrupt_value)((data & INT_SOURCE0_FSYNC_INT1_EN_MASK) >> + INT_SOURCE0_FSYNC_INT1_EN_POS); + it->INV_UI_DRDY = (inv_imu_interrupt_value)((data & INT_SOURCE0_DRDY_INT1_EN_MASK) >> + INT_SOURCE0_DRDY_INT1_EN_POS); + it->INV_FIFO_THS = (inv_imu_interrupt_value)((data & INT_SOURCE0_FIFO_THS_INT1_EN_MASK) >> + INT_SOURCE0_FIFO_THS_INT1_EN_POS); + it->INV_FIFO_FULL = (inv_imu_interrupt_value)((data & INT_SOURCE0_FIFO_FULL_INT1_EN_MASK) >> + INT_SOURCE0_FIFO_FULL_INT1_EN_POS); + + status |= inv_imu_read_reg(s, INT_SOURCE1, 1, &data); + it->INV_SMD = (inv_imu_interrupt_value)((data & INT_SOURCE1_SMD_INT1_EN_MASK) >> + INT_SOURCE1_SMD_INT1_EN_POS); + it->INV_WOM_X = (inv_imu_interrupt_value)((data & INT_SOURCE1_WOM_X_INT1_EN_MASK) >> + INT_SOURCE1_WOM_X_INT1_EN_POS); + it->INV_WOM_Y = (inv_imu_interrupt_value)((data & INT_SOURCE1_WOM_Y_INT1_EN_MASK) >> + INT_SOURCE1_WOM_Y_INT1_EN_POS); + it->INV_WOM_Z = (inv_imu_interrupt_value)((data & INT_SOURCE1_WOM_Z_INT1_EN_MASK) >> + INT_SOURCE1_WOM_Z_INT1_EN_POS); + + status |= inv_imu_read_reg(s, INT_SOURCE6_MREG1, 1, &data); + it->INV_FF = (inv_imu_interrupt_value)((data & INT_SOURCE6_FF_INT1_EN_MASK) >> + INT_SOURCE6_FF_INT1_EN_POS); + it->INV_LOWG = (inv_imu_interrupt_value)((data & INT_SOURCE6_LOWG_INT1_EN_MASK) >> + INT_SOURCE6_LOWG_INT1_EN_POS); + it->INV_STEP_DET = (inv_imu_interrupt_value)((data & INT_SOURCE6_STEP_DET_INT1_EN_MASK) >> + INT_SOURCE6_STEP_DET_INT1_EN_POS); + it->INV_STEP_CNT_OVFL = (inv_imu_interrupt_value)( + (data & INT_SOURCE6_STEP_CNT_OFL_INT1_EN_MASK) >> INT_SOURCE6_STEP_CNT_OFL_INT1_EN_POS); + it->INV_TILT_DET = (inv_imu_interrupt_value)((data & INT_SOURCE6_TILT_DET_INT1_EN_MASK) >> + INT_SOURCE6_TILT_DET_INT1_EN_POS); + + return status; +} + +int inv_imu_get_config_int2(inv_imu_device_t *s, inv_imu_interrupt_parameter_t *it) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, INT_SOURCE3, 1, &data); + it->INV_UI_FSYNC = (inv_imu_interrupt_value)((data & INT_SOURCE3_FSYNC_INT2_EN_MASK) >> + INT_SOURCE3_FSYNC_INT2_EN_POS); + it->INV_UI_DRDY = (inv_imu_interrupt_value)((data & INT_SOURCE3_DRDY_INT2_EN_MASK) >> + INT_SOURCE3_DRDY_INT2_EN_POS); + it->INV_FIFO_THS = (inv_imu_interrupt_value)((data & INT_SOURCE3_FIFO_THS_INT2_EN_MASK) >> + INT_SOURCE3_FIFO_THS_INT2_EN_POS); + it->INV_FIFO_FULL = (inv_imu_interrupt_value)((data & INT_SOURCE3_FIFO_FULL_INT2_EN_MASK) >> + INT_SOURCE3_FIFO_FULL_INT2_EN_POS); + + status |= inv_imu_read_reg(s, INT_SOURCE4, 1, &data); + it->INV_SMD = (inv_imu_interrupt_value)((data & INT_SOURCE4_SMD_INT2_EN_MASK) >> + INT_SOURCE4_SMD_INT2_EN_POS); + it->INV_WOM_X = (inv_imu_interrupt_value)((data & INT_SOURCE4_WOM_X_INT2_EN_MASK) >> + INT_SOURCE4_WOM_X_INT2_EN_POS); + it->INV_WOM_Y = (inv_imu_interrupt_value)((data & INT_SOURCE4_WOM_Y_INT2_EN_MASK) >> + INT_SOURCE4_WOM_Y_INT2_EN_POS); + it->INV_WOM_Z = (inv_imu_interrupt_value)((data & INT_SOURCE4_WOM_Z_INT2_EN_MASK) >> + INT_SOURCE4_WOM_Z_INT2_EN_POS); + + status |= inv_imu_read_reg(s, INT_SOURCE7_MREG1, 1, &data); + it->INV_FF = (inv_imu_interrupt_value)((data & INT_SOURCE7_FF_INT2_EN_MASK) >> + INT_SOURCE7_FF_INT2_EN_POS); + it->INV_LOWG = (inv_imu_interrupt_value)((data & INT_SOURCE7_LOWG_INT2_EN_MASK) >> + INT_SOURCE7_LOWG_INT2_EN_POS); + it->INV_STEP_DET = (inv_imu_interrupt_value)((data & INT_SOURCE7_STEP_DET_INT2_EN_MASK) >> + INT_SOURCE7_STEP_DET_INT2_EN_POS); + it->INV_STEP_CNT_OVFL = (inv_imu_interrupt_value)( + (data & INT_SOURCE7_STEP_CNT_OFL_INT2_EN_MASK) >> INT_SOURCE7_STEP_CNT_OFL_INT2_EN_POS); + it->INV_TILT_DET = (inv_imu_interrupt_value)((data & INT_SOURCE7_TILT_DET_INT2_EN_MASK) >> + INT_SOURCE7_TILT_DET_INT2_EN_POS); + + return status; +} + +int inv_imu_set_config_int1(inv_imu_device_t *s, const inv_imu_interrupt_parameter_t *it) +{ + int status = 0; + uint8_t data[2]; + + status |= inv_imu_read_reg(s, INT_SOURCE0, 2, &data[0]); + + data[0] &= ~(INT_SOURCE0_FSYNC_INT1_EN_MASK | INT_SOURCE0_DRDY_INT1_EN_MASK | + INT_SOURCE0_FIFO_THS_INT1_EN_MASK | INT_SOURCE0_FIFO_FULL_INT1_EN_MASK); + data[0] |= ((it->INV_UI_FSYNC != 0) << INT_SOURCE0_FSYNC_INT1_EN_POS); + data[0] |= ((it->INV_UI_DRDY != 0) << INT_SOURCE0_DRDY_INT1_EN_POS); + data[0] |= ((it->INV_FIFO_THS != 0) << INT_SOURCE0_FIFO_THS_INT1_EN_POS); + data[0] |= ((it->INV_FIFO_FULL != 0) << INT_SOURCE0_FIFO_FULL_INT1_EN_POS); + + data[1] &= ~(INT_SOURCE1_SMD_INT1_EN_MASK | INT_SOURCE1_WOM_X_INT1_EN_MASK | + INT_SOURCE1_WOM_Y_INT1_EN_MASK | INT_SOURCE1_WOM_Z_INT1_EN_MASK); + data[1] |= ((it->INV_SMD != 0) << INT_SOURCE1_SMD_INT1_EN_POS); + data[1] |= ((it->INV_WOM_X != 0) << INT_SOURCE1_WOM_X_INT1_EN_POS); + data[1] |= ((it->INV_WOM_Y != 0) << INT_SOURCE1_WOM_Y_INT1_EN_POS); + data[1] |= ((it->INV_WOM_Z != 0) << INT_SOURCE1_WOM_Z_INT1_EN_POS); + + status |= inv_imu_write_reg(s, INT_SOURCE0, 2, &data[0]); + + status |= inv_imu_read_reg(s, INT_SOURCE6_MREG1, 1, &data[0]); + + data[0] &= ~(INT_SOURCE6_FF_INT1_EN_MASK | INT_SOURCE6_LOWG_INT1_EN_MASK | + INT_SOURCE6_STEP_DET_INT1_EN_MASK | INT_SOURCE6_STEP_CNT_OFL_INT1_EN_MASK | + INT_SOURCE6_TILT_DET_INT1_EN_MASK); + data[0] |= ((it->INV_FF != 0) << INT_SOURCE6_FF_INT1_EN_POS); + data[0] |= ((it->INV_LOWG != 0) << INT_SOURCE6_LOWG_INT1_EN_POS); + data[0] |= ((it->INV_STEP_DET != 0) << INT_SOURCE6_STEP_DET_INT1_EN_POS); + data[0] |= ((it->INV_STEP_CNT_OVFL != 0) << INT_SOURCE6_STEP_CNT_OFL_INT1_EN_POS); + data[0] |= ((it->INV_TILT_DET != 0) << INT_SOURCE6_TILT_DET_INT1_EN_POS); + status |= inv_imu_write_reg(s, INT_SOURCE6_MREG1, 1, &data[0]); + + return status; +} + +int inv_imu_set_config_int2(inv_imu_device_t *s, const inv_imu_interrupt_parameter_t *it) +{ + int status = 0; + uint8_t data[2]; + + status |= inv_imu_read_reg(s, INT_SOURCE3, 2, &data[0]); + + data[0] &= ~(INT_SOURCE3_FSYNC_INT2_EN_MASK | INT_SOURCE3_DRDY_INT2_EN_MASK | + INT_SOURCE3_FIFO_THS_INT2_EN_MASK | INT_SOURCE3_FIFO_FULL_INT2_EN_MASK); + data[0] |= ((it->INV_UI_FSYNC != 0) << INT_SOURCE3_FSYNC_INT2_EN_POS); + data[0] |= ((it->INV_UI_DRDY != 0) << INT_SOURCE3_DRDY_INT2_EN_POS); + data[0] |= ((it->INV_FIFO_THS != 0) << INT_SOURCE3_FIFO_THS_INT2_EN_POS); + data[0] |= ((it->INV_FIFO_FULL != 0) << INT_SOURCE3_FIFO_FULL_INT2_EN_POS); + + data[1] &= ~(INT_SOURCE4_SMD_INT2_EN_MASK | INT_SOURCE4_WOM_X_INT2_EN_MASK | + INT_SOURCE4_WOM_Y_INT2_EN_MASK | INT_SOURCE4_WOM_Z_INT2_EN_MASK); + data[1] |= ((it->INV_SMD != 0) << INT_SOURCE4_SMD_INT2_EN_POS); + data[1] |= ((it->INV_WOM_X != 0) << INT_SOURCE4_WOM_X_INT2_EN_POS); + data[1] |= ((it->INV_WOM_Y != 0) << INT_SOURCE4_WOM_Y_INT2_EN_POS); + data[1] |= ((it->INV_WOM_Z != 0) << INT_SOURCE4_WOM_Z_INT2_EN_POS); + + status |= inv_imu_write_reg(s, INT_SOURCE3, 2, &data[0]); + + status |= inv_imu_read_reg(s, INT_SOURCE7_MREG1, 1, &data[0]); + + data[0] &= ~(INT_SOURCE7_FF_INT2_EN_MASK | INT_SOURCE7_LOWG_INT2_EN_MASK | + INT_SOURCE7_STEP_DET_INT2_EN_MASK | INT_SOURCE7_STEP_CNT_OFL_INT2_EN_MASK | + INT_SOURCE7_TILT_DET_INT2_EN_MASK); + data[0] |= ((it->INV_FF != 0) << INT_SOURCE7_FF_INT2_EN_POS); + data[0] |= ((it->INV_LOWG != 0) << INT_SOURCE7_LOWG_INT2_EN_POS); + data[0] |= ((it->INV_STEP_DET != 0) << INT_SOURCE7_STEP_DET_INT2_EN_POS); + data[0] |= ((it->INV_STEP_CNT_OVFL != 0) << INT_SOURCE7_STEP_CNT_OFL_INT2_EN_POS); + data[0] |= ((it->INV_TILT_DET != 0) << INT_SOURCE7_TILT_DET_INT2_EN_POS); + + status |= inv_imu_write_reg(s, INT_SOURCE7_MREG1, 1, &data[0]); + + return status; +} + +int inv_imu_get_data_from_registers(inv_imu_device_t *s) +{ + int status = 0; + uint8_t int_status; + uint8_t accel[ACCEL_DATA_SIZE]; +#if INV_IMU_IS_GYRO_SUPPORTED + uint8_t gyro[GYRO_DATA_SIZE]; +#endif + inv_imu_sensor_event_t event; + + /* Ensure data ready status bit is set */ + if (status |= inv_imu_read_reg(s, INT_STATUS_DRDY, 1, &int_status)) + return status; + + if (int_status & INT_STATUS_DRDY_DATA_RDY_INT_MASK) { + uint8_t temperature[2]; + /* Read temperature */ + status |= inv_imu_read_reg(s, TEMP_DATA1, 2, &temperature[0]); + format_s16_data(s->endianness_data, &temperature[0], &event.temperature); + + /* Read accel */ + status |= inv_imu_read_reg(s, ACCEL_DATA_X1, ACCEL_DATA_SIZE, &accel[0]); + format_s16_data(s->endianness_data, &accel[0], &event.accel[0]); + format_s16_data(s->endianness_data, &accel[2], &event.accel[1]); + format_s16_data(s->endianness_data, &accel[4], &event.accel[2]); + +#if INV_IMU_IS_GYRO_SUPPORTED + status |= inv_imu_read_reg(s, GYRO_DATA_X1, GYRO_DATA_SIZE, &gyro[0]); + format_s16_data(s->endianness_data, &gyro[0], &event.gyro[0]); + format_s16_data(s->endianness_data, &gyro[2], &event.gyro[1]); + format_s16_data(s->endianness_data, &gyro[4], &event.gyro[2]); +#endif + + /* call sensor event callback */ + if (s->sensor_event_cb) + s->sensor_event_cb(&event); + } + /*else: Data Ready was not reached*/ + + return status; +} + +int inv_imu_get_frame_count(inv_imu_device_t *s, uint16_t *frame_count) +{ + int status = 0; + + status |= inv_imu_read_reg(s, FIFO_COUNTH, 2, (uint8_t *)frame_count); + format_u16_data(INTF_CONFIG0_DATA_LITTLE_ENDIAN, (uint8_t *)frame_count, frame_count); + + return status; +} + +int inv_imu_decode_fifo_frame(inv_imu_device_t *s, const uint8_t *frame, + inv_imu_sensor_event_t *event) +{ + int status = 0; + uint16_t frame_idx = 0; + const fifo_header_t *header; + + event->sensor_mask = 0; + header = (fifo_header_t *)frame; + + frame_idx += FIFO_HEADER_SIZE; + + /* Check if frame is invalid */ + if (header->Byte == 0x80) { + /* Header shows that frame is invalid, no need to go further */ + return 0; + } + + /* Check MSG BIT */ + if (header->bits.msg_bit) { + /* MSG BIT set in FIFO header, return error */ + return INV_ERROR; + } + + /* Read Accel */ + if (header->bits.accel_bit) { + format_s16_data(s->endianness_data, &(frame[0 + frame_idx]), &event->accel[0]); + format_s16_data(s->endianness_data, &(frame[2 + frame_idx]), &event->accel[1]); + format_s16_data(s->endianness_data, &(frame[4 + frame_idx]), &event->accel[2]); + frame_idx += FIFO_ACCEL_DATA_SIZE; + } + +#if INV_IMU_IS_GYRO_SUPPORTED + /* Read Gyro */ + if (header->bits.gyro_bit) { + format_s16_data(s->endianness_data, &(frame[0 + frame_idx]), &event->gyro[0]); + format_s16_data(s->endianness_data, &(frame[2 + frame_idx]), &event->gyro[1]); + format_s16_data(s->endianness_data, &(frame[4 + frame_idx]), &event->gyro[2]); + frame_idx += FIFO_GYRO_DATA_SIZE; + } +#else + /* + * With 20-Bytes packets, 6B are reserved after accel value. + * Therefore, increment `fifo_idx` accordingly. + */ + if (header->bits.twentybits_bit) + frame_idx += FIFO_GYRO_DATA_SIZE; +#endif + + /* + * The coarse temperature (8 or 16B FIFO packet format) + * range is ± 64 degrees with 0.5°C resolution. + * but the fine temperature range (2 bytes) (20B FIFO packet format) is + * ± 256 degrees with (1/128)°C resolution + */ + if (header->bits.twentybits_bit) { + format_s16_data(s->endianness_data, &(frame[0 + frame_idx]), &event->temperature); + frame_idx += FIFO_TEMP_DATA_SIZE + FIFO_TEMP_HIGH_RES_SIZE; + + /* new temperature data */ + if (event->temperature != INVALID_VALUE_FIFO) + event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE); + } else { + /* cast to int8_t since FIFO is in 16 bits mode (temperature on 8 bits) */ + event->temperature = (int8_t)(frame[0 + frame_idx]); + frame_idx += FIFO_TEMP_DATA_SIZE; + + /* new temperature data */ + if (event->temperature != INVALID_VALUE_FIFO_1B) + event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE); + } + + if (header->bits.timestamp_bit +#if INV_IMU_IS_GYRO_SUPPORTED + || header->bits.fsync_bit +#endif + ) { + format_u16_data(s->endianness_data, &(frame[0 + frame_idx]), &event->timestamp_fsync); + frame_idx += FIFO_TS_FSYNC_SIZE; +#if INV_IMU_IS_GYRO_SUPPORTED + /* new fsync event */ + if (header->bits.fsync_bit) + event->sensor_mask |= (1 << INV_SENSOR_FSYNC_EVENT); +#endif + } + + if (header->bits.accel_bit && + ((event->accel[0] != INVALID_VALUE_FIFO) && (event->accel[1] != INVALID_VALUE_FIFO) && + (event->accel[2] != INVALID_VALUE_FIFO))) { + if (s->accel_start_time_us == UINT32_MAX) { + event->sensor_mask |= (1 << INV_SENSOR_ACCEL); + } else { + if (((inv_imu_get_time_us() - s->accel_start_time_us) >= ACC_STARTUP_TIME_US) +#if INV_IMU_IS_GYRO_SUPPORTED + && !header->bits.fsync_bit +#endif + ) { + /* Discard first data after startup to let output to settle */ + s->accel_start_time_us = UINT32_MAX; + event->sensor_mask |= (1 << INV_SENSOR_ACCEL); + } + } + + if ((event->sensor_mask & (1 << INV_SENSOR_ACCEL)) && (header->bits.twentybits_bit)) { + event->accel_high_res[0] = (frame[0 + frame_idx] >> 4) & 0xF; + event->accel_high_res[1] = (frame[1 + frame_idx] >> 4) & 0xF; + event->accel_high_res[2] = (frame[2 + frame_idx] >> 4) & 0xF; + } + } + +#if INV_IMU_IS_GYRO_SUPPORTED + if (header->bits.gyro_bit && + ((event->gyro[0] != INVALID_VALUE_FIFO) && (event->gyro[1] != INVALID_VALUE_FIFO) && + (event->gyro[2] != INVALID_VALUE_FIFO))) { + if (s->gyro_start_time_us == UINT32_MAX) { + event->sensor_mask |= (1 << INV_SENSOR_GYRO); + } else { + if (!header->bits.fsync_bit && + ((inv_imu_get_time_us() - s->gyro_start_time_us) >= GYR_STARTUP_TIME_US)) { + /* Discard first data after startup to let output to settle */ + s->gyro_start_time_us = UINT32_MAX; + event->sensor_mask |= (1 << INV_SENSOR_GYRO); + } + } + + if ((event->sensor_mask & (1 << INV_SENSOR_GYRO)) && (header->bits.twentybits_bit)) { + event->gyro_high_res[0] = (frame[0 + frame_idx]) & 0xF; + event->gyro_high_res[1] = (frame[1 + frame_idx]) & 0xF; + event->gyro_high_res[2] = (frame[2 + frame_idx]) & 0xF; + } + } +#endif + + return status; +} + +int inv_imu_get_data_from_fifo(inv_imu_device_t *s) +{ + int status = 0; + uint8_t int_status; + uint16_t packet_count = 0; + uint16_t packet_size = + s->fifo_highres_enabled ? FIFO_20BYTES_PACKET_SIZE : FIFO_16BYTES_PACKET_SIZE; + uint16_t fifo_idx = 0; + + /* Ensure data ready status bit is set */ + status |= inv_imu_read_reg(s, INT_STATUS, 1, &int_status); + if (!(int_status & INT_STATUS_FIFO_THS_INT_MASK) && + !(int_status & INT_STATUS_FIFO_FULL_INT_MASK)) + return 0; /* Neither FIFO THS nor FIFO_FULL is set, simply return here */ + + /* + * Make sure RCOSC is enabled to guarantee FIFO read. + * For power optimization, this call can be omitted under specific conditions: + * - If using WM interrupt and you can guarantee entire FIFO will be read at once. + * - If gyro is enabled or accel is in LN or LP+RCOSC mode. + * - In accel LP+WUOSC mode, if you wait 100 us after reading FIFO_COUNT and + * you can guarantee that the FIFO will be read within 1 ms. + * Please refer to the AN-000324 for more information. + */ + status |= inv_imu_switch_on_mclk(s); + + /* Read FIFO frame count */ + status |= inv_imu_get_frame_count(s, &packet_count); + + /* Check for error */ + if (status != INV_ERROR_SUCCESS) { + status |= inv_imu_switch_off_mclk(s); + return status; + } + + /* Read FIFO data */ + status |= inv_imu_read_reg(s, FIFO_DATA, packet_size * packet_count, s->fifo_data); + + /* Check for error */ + if (status != INV_ERROR_SUCCESS) { + status |= inv_imu_reset_fifo(s); + status |= inv_imu_switch_off_mclk(s); + return status; + } + + /* Process FIFO packets */ + for (uint16_t i = 0; i < packet_count; i++) { + inv_imu_sensor_event_t event; + + status |= inv_imu_decode_fifo_frame(s, &s->fifo_data[fifo_idx], &event); + fifo_idx += packet_size; + + /* Check for error */ + if (status != INV_ERROR_SUCCESS) { + status |= inv_imu_reset_fifo(s); + status |= inv_imu_switch_off_mclk(s); + return status; + } + + /* call sensor event callback */ + if (s->sensor_event_cb) + s->sensor_event_cb(&event); + } + + status |= inv_imu_switch_off_mclk(s); + + if (status < 0) + return status; + + return packet_count; +} + +uint32_t inv_imu_convert_odr_bitfield_to_us(uint32_t odr_bitfield) +{ + /* + odr bitfield - frequency : odr ms + 0 - N/A + 1 - N/A + 2 - N/A + 3 - N/A + 4 - N/A + 5 - 1.6k : 0.625ms + (default) 6 - 800 : 1.25ms + 7 - 400 : 2.5 ms + 8 - 200 : 5 ms + 9 - 100 : 10 ms + 10 - 50 : 20 ms + 11 - 25 : 40 ms + 12 - 12.5 : 80 ms + 13 - 6.25 : 160 ms + 14 - 3.125 : 320 ms + 15 - 1.5625 : 640 ms + */ + + switch (odr_bitfield) { + case ACCEL_CONFIG0_ODR_1600_HZ: + return 625; + case ACCEL_CONFIG0_ODR_800_HZ: + return 1250; + case ACCEL_CONFIG0_ODR_400_HZ: + return 2500; + case ACCEL_CONFIG0_ODR_200_HZ: + return 5000; + case ACCEL_CONFIG0_ODR_100_HZ: + return 10000; + case ACCEL_CONFIG0_ODR_50_HZ: + return 20000; + case ACCEL_CONFIG0_ODR_25_HZ: + return 40000; + case ACCEL_CONFIG0_ODR_12_5_HZ: + return 80000; + case ACCEL_CONFIG0_ODR_6_25_HZ: + return 160000; + case ACCEL_CONFIG0_ODR_3_125_HZ: + return 320000; + case ACCEL_CONFIG0_ODR_1_5625_HZ: + default: + return 640000; + } +} + +int inv_imu_set_timestamp_resolution(inv_imu_device_t * s, + const TMST_CONFIG1_RESOL_t timestamp_resol) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, TMST_CONFIG1_MREG1, 1, &data); + data &= ~TMST_CONFIG1_TMST_RES_MASK; + data |= timestamp_resol; + status |= inv_imu_write_reg(s, TMST_CONFIG1_MREG1, 1, &data); + + return status; +} + +int inv_imu_reset_fifo(inv_imu_device_t *s) +{ + int status = 0; + uint8_t fifo_flush_status = (uint8_t)SIGNAL_PATH_RESET_FIFO_FLUSH_EN; + + status |= inv_imu_switch_on_mclk(s); + + status |= inv_imu_write_reg(s, SIGNAL_PATH_RESET, 1, &fifo_flush_status); + inv_imu_sleep_us(10); + + /* Wait for FIFO flush (idle bit will go high at appropriate time and unlock flush) */ + while ((status == 0) && ((fifo_flush_status & SIGNAL_PATH_RESET_FIFO_FLUSH_MASK) == + (uint8_t)SIGNAL_PATH_RESET_FIFO_FLUSH_EN)) { + status |= inv_imu_read_reg(s, SIGNAL_PATH_RESET, 1, &fifo_flush_status); + } + + status |= inv_imu_switch_off_mclk(s); + + return status; +} + +int inv_imu_enable_high_resolution_fifo(inv_imu_device_t *s) +{ + uint8_t value; + int status = 0; + + /* set FIFO packets to 20bit format (i.e. high res is enabled) */ + s->fifo_highres_enabled = 1; + + status |= inv_imu_read_reg(s, FIFO_CONFIG5_MREG1, 1, &value); + value &= ~FIFO_CONFIG5_FIFO_HIRES_EN_MASK; + value |= FIFO_CONFIG5_HIRES_EN; + status |= inv_imu_write_reg(s, FIFO_CONFIG5_MREG1, 1, &value); + + return status; +} + +int inv_imu_disable_high_resolution_fifo(inv_imu_device_t *s) +{ + uint8_t value; + int status = 0; + + /* set FIFO packets to 16bit format (i.e. high res is disabled) */ + s->fifo_highres_enabled = 0; + + status |= inv_imu_read_reg(s, FIFO_CONFIG5_MREG1, 1, &value); + value &= ~FIFO_CONFIG5_FIFO_HIRES_EN_MASK; + value |= FIFO_CONFIG5_HIRES_DIS; + status |= inv_imu_write_reg(s, FIFO_CONFIG5_MREG1, 1, &value); + + return status; +} + +int inv_imu_configure_fifo(inv_imu_device_t *s, INV_IMU_FIFO_CONFIG_t fifo_config) +{ + int status = 0; + uint8_t data; + + s->fifo_is_used = fifo_config; + + inv_imu_switch_on_mclk(s); + + switch (fifo_config) { + case INV_IMU_FIFO_ENABLED: + /* Configure: + - FIFO record mode i.e FIFO count unit is packet + - FIFO snapshot mode i.e drop the data when the FIFO overflows + - Timestamp is logged in FIFO + - Little Endian fifo_count + */ + + status |= inv_imu_read_reg(s, INTF_CONFIG0, 1, &data); + data &= ~(INTF_CONFIG0_FIFO_COUNT_FORMAT_MASK | INTF_CONFIG0_FIFO_COUNT_ENDIAN_MASK); + data |= (uint8_t)INTF_CONFIG0_FIFO_COUNT_REC_RECORD | + (uint8_t)INTF_CONFIG0_FIFO_COUNT_LITTLE_ENDIAN; + status |= inv_imu_write_reg(s, INTF_CONFIG0, 1, &data); + + status |= inv_imu_read_reg(s, FIFO_CONFIG1, 1, &data); + data &= ~(FIFO_CONFIG1_FIFO_MODE_MASK | FIFO_CONFIG1_FIFO_BYPASS_MASK); + data |= (uint8_t)FIFO_CONFIG1_FIFO_MODE_SNAPSHOT | (uint8_t)FIFO_CONFIG1_FIFO_BYPASS_OFF; + status |= inv_imu_write_reg(s, FIFO_CONFIG1, 1, &data); + + status |= inv_imu_read_reg(s, TMST_CONFIG1_MREG1, 1, &data); + data &= ~TMST_CONFIG1_TMST_EN_MASK; + data |= TMST_CONFIG1_TMST_EN; + status |= inv_imu_write_reg(s, TMST_CONFIG1_MREG1, 1, &data); + + /* restart and reset FIFO configuration */ + status |= inv_imu_read_reg(s, FIFO_CONFIG5_MREG1, 1, &data); + data &= ~FIFO_CONFIG5_FIFO_ACCEL_EN_MASK; + data &= ~FIFO_CONFIG5_FIFO_HIRES_EN_MASK; +#if INV_IMU_IS_GYRO_SUPPORTED + data &= ~FIFO_CONFIG5_FIFO_GYRO_EN_MASK; + data &= ~FIFO_CONFIG5_FIFO_TMST_FSYNC_EN_MASK; + data |= (uint8_t)FIFO_CONFIG5_GYRO_EN; + data |= (uint8_t)FIFO_CONFIG5_TMST_FSYNC_EN; +#endif + data |= (uint8_t)FIFO_CONFIG5_ACCEL_EN; + data |= (uint8_t)FIFO_CONFIG5_WM_GT_TH_EN; + status |= inv_imu_write_reg(s, FIFO_CONFIG5_MREG1, 1, &data); + + /* Configure FIFO WM so that INT is triggered for each packet */ + data = 0x1; + status |= inv_imu_write_reg(s, FIFO_CONFIG2, 1, &data); + break; + + case INV_IMU_FIFO_DISABLED: + /* make sure FIFO is disabled */ + status |= inv_imu_read_reg(s, FIFO_CONFIG1, 1, &data); + data &= ~FIFO_CONFIG1_FIFO_BYPASS_MASK; + data |= (uint8_t)FIFO_CONFIG1_FIFO_BYPASS_ON; + status |= inv_imu_write_reg(s, FIFO_CONFIG1, 1, &data); + + /* restart and reset FIFO configuration */ + status |= inv_imu_read_reg(s, FIFO_CONFIG5_MREG1, 1, &data); + data &= ~FIFO_CONFIG5_FIFO_ACCEL_EN_MASK; +#if INV_IMU_IS_GYRO_SUPPORTED + data &= ~FIFO_CONFIG5_FIFO_GYRO_EN_MASK; + data &= ~FIFO_CONFIG5_FIFO_TMST_FSYNC_EN_MASK; + data |= (uint8_t)FIFO_CONFIG5_GYRO_DIS; + data |= (uint8_t)FIFO_CONFIG5_TMST_FSYNC_EN; +#endif + data |= (uint8_t)FIFO_CONFIG5_ACCEL_DIS; + status |= inv_imu_write_reg(s, FIFO_CONFIG5_MREG1, 1, &data); + break; + + default: + status = -1; + } + + status |= inv_imu_switch_off_mclk(s); + + return status; +} + +uint32_t inv_imu_get_timestamp_resolution_us(inv_imu_device_t *s) +{ + int status = 0; + uint8_t tmst_config1_reg; + TMST_CONFIG1_RESOL_t tmst_resol; + + status |= inv_imu_read_reg(s, TMST_CONFIG1_MREG1, 1, &tmst_config1_reg); + if (status < 0) + return INV_ERROR; + + tmst_resol = (TMST_CONFIG1_RESOL_t)(tmst_config1_reg & TMST_CONFIG1_TMST_RES_MASK); + + if (tmst_resol == TMST_CONFIG1_RESOL_16us) + return 16; + else if (tmst_resol == TMST_CONFIG1_RESOL_1us) + return 1; + + // Should not happen, return 0 + return 0; +} + +int inv_imu_configure_wom(inv_imu_device_t *s, const uint8_t wom_x_th, const uint8_t wom_y_th, + const uint8_t wom_z_th, WOM_CONFIG_WOM_INT_MODE_t wom_int, + WOM_CONFIG_WOM_INT_DUR_t wom_dur) +{ + int status = 0; + uint8_t data[3]; + uint8_t value; + + data[0] = wom_x_th; // Set X threshold + data[1] = wom_y_th; // Set Y threshold + data[2] = wom_z_th; // Set Z threshold + status |= inv_imu_write_reg(s, ACCEL_WOM_X_THR_MREG1, sizeof(data), &data[0]); + + // Compare current sample with the previous sample and WOM from the 3 axis are ORed or ANDed to produce WOM signal. + status |= inv_imu_read_reg(s, WOM_CONFIG, 1, &value); + value &= ~WOM_CONFIG_WOM_INT_MODE_MASK; + value |= (uint8_t)WOM_CONFIG_WOM_MODE_CMP_PREV | (uint8_t)wom_int; + + // Configure the number of overthreshold event to wait before producing the WOM signal. + value &= ~WOM_CONFIG_WOM_INT_DUR_MASK; + value |= (uint8_t)wom_dur; + status |= inv_imu_write_reg(s, WOM_CONFIG, 1, &value); + + return status; +} + +int inv_imu_enable_wom(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + /* Enable WOM */ + status |= inv_imu_read_reg(s, WOM_CONFIG, 1, &value); + value &= ~WOM_CONFIG_WOM_EN_MASK; + value |= (uint8_t)WOM_CONFIG_WOM_EN_ENABLE; + status |= inv_imu_write_reg(s, WOM_CONFIG, 1, &value); + + return status; +} + +int inv_imu_disable_wom(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + /* Disable WOM */ + status |= inv_imu_read_reg(s, WOM_CONFIG, 1, &value); + value &= ~WOM_CONFIG_WOM_EN_MASK; + value |= WOM_CONFIG_WOM_EN_DISABLE; + status |= inv_imu_write_reg(s, WOM_CONFIG, 1, &value); + + return status; +} + +int inv_imu_start_dmp(inv_imu_device_t *s) +{ + int status = 0; + + // On first enabling of DMP, reset internal state + if (!s->dmp_is_on) { + // Reset SRAM to 0's + status |= inv_imu_reset_dmp(s, APEX_CONFIG0_DMP_MEM_RESET_APEX_ST_EN); + if (status) + return status; + s->dmp_is_on = 1; + +#if INV_IMU_HFSR_SUPPORTED + { + uint8_t data; + static uint8_t ram_img[] = { + #include "dmp3Default_xian_hfsr_rom_patch.txt" + }; + + /* HFSR parts requires to prescale accel data using a patch in SRAM */ + status |= inv_imu_write_sram(s, ram_img, 320, sizeof(ram_img)); + + /* Set DMP start point to beginning of the patch i.e. SRAM start + offset = 320 */ + data = (320 / 32); + status |= inv_imu_write_reg(s, DMP_CONFIG1_MREG1, 1, &data); + } +#endif + } + + // Initialize DMP + status |= inv_imu_resume_dmp(s); + + return status; +} + +int inv_imu_resume_dmp(struct inv_imu_device *s) +{ + int status = 0; + uint8_t value; + uint64_t start; + + status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &value); + value &= ~APEX_CONFIG0_DMP_INIT_EN_MASK; + value |= (uint8_t)APEX_CONFIG0_DMP_INIT_EN; + status |= inv_imu_write_reg(s, APEX_CONFIG0, 1, &value); + + /* wait to make sure dmp_init_en = 0 */ + start = inv_imu_get_time_us(); + do { + inv_imu_read_reg(s, APEX_CONFIG0, 1, &value); + inv_imu_sleep_us(100); + + if ((value & APEX_CONFIG0_DMP_INIT_EN_MASK) == 0) + break; + + } while (inv_imu_get_time_us() - start < 50000); + + return status; +} + +int inv_imu_reset_dmp(inv_imu_device_t *s, const APEX_CONFIG0_DMP_MEM_RESET_t sram_reset) +{ + const int ref_timeout = 5000; /*50 ms*/ + int status = 0; + int timeout = ref_timeout; + uint8_t data_dmp_reset; + uint8_t value = 0; + + status |= inv_imu_switch_on_mclk(s); + + // Reset DMP internal memories + status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &value); + value &= ~APEX_CONFIG0_DMP_MEM_RESET_EN_MASK; + value |= (sram_reset & APEX_CONFIG0_DMP_MEM_RESET_EN_MASK); + status |= inv_imu_write_reg(s, APEX_CONFIG0, 1, &value); + + inv_imu_sleep_us(1000); + + // Make sure reset procedure has finished by reading back mem_reset_en bit + do { + inv_imu_sleep_us(10); + status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &data_dmp_reset); + } while ( + ((data_dmp_reset & APEX_CONFIG0_DMP_MEM_RESET_EN_MASK) != APEX_CONFIG0_DMP_MEM_RESET_DIS) && + timeout-- && !status); + + status |= inv_imu_switch_off_mclk(s); + + if (timeout <= 0) + return INV_ERROR_TIMEOUT; + + return status; +} + +int inv_imu_set_endianness(inv_imu_device_t *s, INTF_CONFIG0_DATA_ENDIAN_t endianness) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_read_reg(s, INTF_CONFIG0, 1, &value); + value &= ~INTF_CONFIG0_SENSOR_DATA_ENDIAN_MASK; + value |= (uint8_t)endianness; + status |= inv_imu_write_reg(s, INTF_CONFIG0, 1, &value); + + if (!status) + s->endianness_data = (uint8_t)endianness; + + return status; +} + +int inv_imu_get_endianness(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + status |= inv_imu_read_reg(s, INTF_CONFIG0, 1, &value); + if (!status) + s->endianness_data = value & INTF_CONFIG0_SENSOR_DATA_ENDIAN_MASK; + + return status; +} + +int inv_imu_configure_fifo_data_rate(inv_imu_device_t *s, FDR_CONFIG_FDR_SEL_t dec_factor) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, FDR_CONFIG_MREG1, 1, &data); + data &= (uint8_t)~FDR_CONFIG_FDR_SEL_MASK; + data |= (uint8_t)dec_factor; + status |= inv_imu_write_reg(s, FDR_CONFIG_MREG1, 1, &data); + + return status; +} + +const char *inv_imu_get_version(void) +{ + return INV_IMU_VERSION_STRING; +} + +#if INV_IMU_HFSR_SUPPORTED +int inv_imu_write_sram(struct inv_imu_device *s, const uint8_t *data, uint32_t offset, + uint32_t size) +{ + int rc = 0; + uint8_t memory_bank; + uint8_t dmp_memory_address; + + if (size + offset > 1280U) + return INV_ERROR_SIZE; + + /* make sure mclk is on */ + rc |= inv_imu_switch_on_mclk(s); + + /* Write memory pointed by data into DMP memory */ + memory_bank = (uint8_t)(SRAM_START_BANK + (offset / 256)); + dmp_memory_address = (uint8_t)(offset % 256); + rc |= inv_imu_write_reg(s, BLK_SEL_W, 1, &memory_bank); + inv_imu_sleep_us(10); + rc |= inv_imu_write_reg(s, MADDR_W, 1, &dmp_memory_address); + inv_imu_sleep_us(10); + + for (uint32_t i = offset; i < size + offset; i++) { + if (0 == (i % 256)) { + memory_bank = (uint8_t)(SRAM_START_BANK + (i / 256)); + dmp_memory_address = 0; + rc |= inv_imu_write_reg(s, BLK_SEL_W, 1, &memory_bank); + inv_imu_sleep_us(10); + rc |= inv_imu_write_reg(s, MADDR_W, 1, &dmp_memory_address); + inv_imu_sleep_us(10); + } + + rc |= inv_imu_write_reg(s, M_W, 1, &data[i - offset]); + inv_imu_sleep_us(10); + } + + memory_bank = 0; + rc |= inv_imu_write_reg(s, BLK_SEL_W, 1, &memory_bank); + + /* cancel mclk request */ + rc |= inv_imu_switch_off_mclk(s); + + rc |= read_and_check_sram(s, data, offset, size); + + return rc; +} +#endif + +/* + * Static functions definition + */ +static int select_rcosc(inv_imu_device_t *s) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data); + data &= ~PWR_MGMT0_ACCEL_LP_CLK_SEL_MASK; + data |= PWR_MGMT0_ACCEL_LP_CLK_RCOSC; + status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &data); + + return status; +} + +static int select_wuosc(inv_imu_device_t *s) +{ + int status = 0; + uint8_t data; + + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &data); + data &= ~PWR_MGMT0_ACCEL_LP_CLK_SEL_MASK; + data |= PWR_MGMT0_ACCEL_LP_CLK_WUOSC; + status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &data); + + return status; +} + +static int configure_serial_interface(inv_imu_device_t *s) +{ + uint8_t value; + int status = 0; + + /* Ensure BLK_SEL_R and BLK_SEL_W are set to 0 */ + value = 0; + status |= inv_imu_write_reg(s, BLK_SEL_R, 1, &value); + status |= inv_imu_write_reg(s, BLK_SEL_W, 1, &value); + + if (s->transport.serif.serif_type == UI_I2C) { + /* Enable I2C 50ns spike filtering */ + status |= inv_imu_read_reg(s, INTF_CONFIG1, 1, &value); + value &= ~(INTF_CONFIG1_I3C_SDR_EN_MASK | INTF_CONFIG1_I3C_DDR_EN_MASK); + status |= inv_imu_write_reg(s, INTF_CONFIG1, 1, &value); + } else { + /* Configure SPI */ + if (s->transport.serif.serif_type == UI_SPI4) + value = (uint8_t)DEVICE_CONFIG_SPI_4WIRE | (uint8_t)DEVICE_CONFIG_SPI_MODE_0_3; + else if (s->transport.serif.serif_type == UI_SPI3) + value = (uint8_t)DEVICE_CONFIG_SPI_3WIRE | (uint8_t)DEVICE_CONFIG_SPI_MODE_0_3; + else + return INV_ERROR_BAD_ARG; /* Not supported */ + status |= inv_imu_write_reg(s, DEVICE_CONFIG, 1, &value); + +#if INV_IMU_REV == INV_IMU_REV_A + /* Device operation in shared spi bus configuration (AN-000352) */ + status |= inv_imu_read_reg(s, INTF_CONFIG0, 1, &value); + value |= 0x3; + status |= inv_imu_write_reg(s, INTF_CONFIG0, 1, &value); +#endif + } + + return status; +} + +static int init_hardware_from_ui(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + +#if INV_IMU_IS_GYRO_SUPPORTED + /* Deactivate FSYNC by default */ + status |= inv_imu_disable_fsync(s); +#endif + + /* Set default timestamp resolution 16us (Mobile use cases) */ + status |= inv_imu_set_timestamp_resolution(s, TMST_CONFIG1_RESOL_16us); + + /* Enable FIFO: use 16-bit format by default (i.e. high res is disabled) */ + status |= inv_imu_configure_fifo(s, INV_IMU_FIFO_ENABLED); + + /* + * Disable the automatic RCOSC power on to avoid + * extra power consumption in sleep mode (all sensors and clocks off) + */ + status |= inv_imu_read_reg(s, FIFO_CONFIG6_MREG1, 1, &value); + value |= ((1 & FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_MASK) + << FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_POS); + status |= inv_imu_write_reg(s, FIFO_CONFIG6_MREG1, 1, &value); + + return status; +} + +#if INV_IMU_HFSR_SUPPORTED +static int read_and_check_sram(struct inv_imu_device *s, const uint8_t *data, uint32_t offset, + uint32_t size) +{ + int rc = 0; + uint8_t memory_bank; + uint8_t dmp_memory_address; + + /* make sure mclk is on */ + rc |= inv_imu_switch_on_mclk(s); + + /* Read DMP memory and check it against memory pointed by input parameter */ + memory_bank = (uint8_t)(SRAM_START_BANK + (offset / 256)); + dmp_memory_address = (uint8_t)(offset % 256); + + rc |= inv_imu_write_reg(s, BLK_SEL_R, 1, &memory_bank); + inv_imu_sleep_us(10); + rc |= inv_imu_write_reg(s, MADDR_R, 1, &dmp_memory_address); + inv_imu_sleep_us(10); + + for (uint32_t i = offset; i < size + offset; i++) { + uint8_t readByte; + + if (0 == (i % 256)) { + memory_bank = (uint8_t)(SRAM_START_BANK + (i / 256)); + dmp_memory_address = 0; + rc |= inv_imu_write_reg(s, BLK_SEL_R, 1, &memory_bank); + inv_imu_sleep_us(10); + rc |= inv_imu_write_reg(s, MADDR_R, 1, &dmp_memory_address); + inv_imu_sleep_us(10); + } + + rc |= inv_imu_read_reg(s, M_R, 1, &readByte); + inv_imu_sleep_us(10); + if (readByte != data[i - offset]) { + rc = -1; + break; + } + } + + memory_bank = 0; + rc |= inv_imu_write_reg(s, BLK_SEL_R, 1, &memory_bank); + + /* cancel mclk request */ + rc |= inv_imu_switch_off_mclk(s); + + return rc; +} +#endif diff --git a/icm42670p/imu/inv_imu_driver.h b/icm42670p/imu/inv_imu_driver.h new file mode 100644 index 0000000..cefd627 --- /dev/null +++ b/icm42670p/imu/inv_imu_driver.h @@ -0,0 +1,543 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/** @defgroup Driver Driver + * @brief High-level functions to drive the device + * @{ + */ + +/** @file inv_imu_driver.h */ + +#ifndef _INV_IMU_DRIVER_H_ +#define _INV_IMU_DRIVER_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "imu/inv_imu_defs.h" +#include "imu/inv_imu_transport.h" + +/** Max FSR values for accel */ +#if INV_IMU_HFSR_SUPPORTED +#define ACCEL_CONFIG0_FS_SEL_MAX ACCEL_CONFIG0_FS_SEL_32g +#else +#define ACCEL_CONFIG0_FS_SEL_MAX ACCEL_CONFIG0_FS_SEL_16g +#endif +/** Max user offset value for accel (mg) */ +#define ACCEL_OFFUSER_MAX_MG 1000 +/** Accel start-up time */ +#define ACC_STARTUP_TIME_US 10000 + +#if INV_IMU_IS_GYRO_SUPPORTED +/** Max FSR values for gyro */ +#if INV_IMU_HFSR_SUPPORTED +#define GYRO_CONFIG0_FS_SEL_MAX GYRO_CONFIG0_FS_SEL_4000dps +#else +#define GYRO_CONFIG0_FS_SEL_MAX GYRO_CONFIG0_FS_SEL_2000dps +#endif +/** Max user offset value for gyro (dps) */ +#define GYRO_OFFUSER_MAX_DPS 64 +/** Gyro start-up time */ +#define GYR_STARTUP_TIME_US 70000 +#endif + +/** Max buffer size mirrored from FIFO at polling time */ +#define FIFO_MIRRORING_SIZE 16 * 258 // packet size * max_count = 4kB + +/** Sensor identifier for UI control function */ +enum inv_imu_sensor { + INV_SENSOR_ACCEL, /**< Accelerometer */ +#if INV_IMU_IS_GYRO_SUPPORTED + INV_SENSOR_GYRO, /**< Gyroscope */ + INV_SENSOR_FSYNC_EVENT, /**< FSYNC */ +#else + INV_SENSOR_RESERVED1, + INV_SENSOR_RESERVED2, +#endif + INV_SENSOR_TEMPERATURE, /**< Chip temperature */ + INV_SENSOR_DMP_PEDOMETER_EVENT, /**< Pedometer: step detected */ + INV_SENSOR_DMP_PEDOMETER_COUNT, /**< Pedometer: step counter */ + INV_SENSOR_DMP_TILT, /**< Tilt */ + INV_SENSOR_DMP_FF, /**< FreeFall */ + INV_SENSOR_DMP_LOWG, /**< Low G */ + INV_SENSOR_DMP_SMD, /**< Significant Motion Detection */ + INV_SENSOR_MAX +}; + +/** Configure FIFO usage */ +typedef enum { + INV_IMU_FIFO_DISABLED = 0, /**< FIFO is disabled and data source is sensors registers */ + INV_IMU_FIFO_ENABLED = 1, /**< FIFO is used as data source */ +} INV_IMU_FIFO_CONFIG_t; + +/** Sensor event structure definition */ +typedef struct { + int sensor_mask; + uint16_t timestamp_fsync; + int16_t accel[3]; +#if INV_IMU_IS_GYRO_SUPPORTED + int16_t gyro[3]; +#endif + int16_t temperature; + int8_t accel_high_res[3]; +#if INV_IMU_IS_GYRO_SUPPORTED + int8_t gyro_high_res[3]; +#endif +} inv_imu_sensor_event_t; + +/** IMU driver states definition */ +typedef struct inv_imu_device { + /** Transport layer. + * @warning Must be the first one of inv_imu_device_t + */ + inv_imu_transport_t transport; + + /** callback executed by: + * * inv_imu_get_data_from_fifo (if FIFO is used). + * * inv_imu_get_data_from_registers (if FIFO isn't used). + * May be NULL if above API are not used by application + */ + void (*sensor_event_cb)(inv_imu_sensor_event_t *event); + + uint8_t fifo_data[FIFO_MIRRORING_SIZE]; /**< FIFO mirroring memory area */ + uint8_t dmp_is_on; /**< DMP started status */ + uint8_t endianness_data; /**< Data endianness configuration */ + uint8_t fifo_highres_enabled; /**< Highres mode configuration */ + INV_IMU_FIFO_CONFIG_t fifo_is_used; /**< FIFO configuration */ +#if INV_IMU_IS_GYRO_SUPPORTED + uint64_t gyro_start_time_us; /**< Gyro start time to discard first samples */ +#endif + uint64_t accel_start_time_us; /**< Accel start time to discard first samples */ +} inv_imu_device_t; + +/** Interrupt enum state for INT1, INT2, and IBI */ +typedef enum { INV_IMU_DISABLE = 0, INV_IMU_ENABLE } inv_imu_interrupt_value; + +/** Interrupt definition */ +typedef struct { + inv_imu_interrupt_value INV_UI_FSYNC; + inv_imu_interrupt_value INV_UI_DRDY; + inv_imu_interrupt_value INV_FIFO_THS; + inv_imu_interrupt_value INV_FIFO_FULL; + inv_imu_interrupt_value INV_SMD; + inv_imu_interrupt_value INV_WOM_X; + inv_imu_interrupt_value INV_WOM_Y; + inv_imu_interrupt_value INV_WOM_Z; + inv_imu_interrupt_value INV_FF; + inv_imu_interrupt_value INV_LOWG; + inv_imu_interrupt_value INV_STEP_DET; + inv_imu_interrupt_value INV_STEP_CNT_OVFL; + inv_imu_interrupt_value INV_TILT_DET; +} inv_imu_interrupt_parameter_t; + +/** INT1 pin configuration */ +typedef struct { + INT_CONFIG_INT1_POLARITY_t int_polarity; + INT_CONFIG_INT1_MODE_t int_mode; + INT_CONFIG_INT1_DRIVE_CIRCUIT_t int_drive; +} inv_imu_int1_pin_config_t; + +/** INT2 pin configuration */ +typedef struct { + INT_CONFIG_INT2_POLARITY_t int_polarity; + INT_CONFIG_INT2_MODE_t int_mode; + INT_CONFIG_INT2_DRIVE_CIRCUIT_t int_drive; +} inv_imu_int2_pin_config_t; + +/** @brief Initializes device. + * @param[in] s Pointer to device. + * @param[in] serif Pointer on serial interface structure. + * @param[in] sensor_event_cb Callback executed when a new sensor event is available. * + * @return 0 on success, negative value on error. + */ +int inv_imu_init(inv_imu_device_t *s, const struct inv_imu_serif *serif, + void (*sensor_event_cb)(inv_imu_sensor_event_t *event)); + +/** @brief Reset device by reloading OTPs. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_device_reset(inv_imu_device_t *s); + +/** @brief return WHOAMI value. + * @param[in] s Pointer to device. + * @param[out] who_am_i WHOAMI for device. + * @return 0 on success, negative value on error. + */ +int inv_imu_get_who_am_i(inv_imu_device_t *s, uint8_t *who_am_i); + +/** @brief Enable/put accel in low power mode. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_enable_accel_low_power_mode(inv_imu_device_t *s); + +/** @brief Enable/put accel in low noise mode. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_enable_accel_low_noise_mode(inv_imu_device_t *s); + +/** @brief Disable all 3 axes of accel. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_disable_accel(inv_imu_device_t *s); + +/** @brief Configure accel Output Data Rate. + * @param[in] s Pointer to device. + * @param[in] frequency The requested frequency. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_accel_frequency(inv_imu_device_t *s, const ACCEL_CONFIG0_ODR_t frequency); + +/** @brief Set accel Low-Power averaging value. + * @param[in] s Pointer to device. + * @param[in] acc_avg Requested averaging value. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_accel_lp_avg(inv_imu_device_t *s, ACCEL_CONFIG1_ACCEL_FILT_AVG_t acc_avg); + +/** @brief Set accel Low-Noise bandwidth value. + * @param[in] s Pointer to device. + * @param[in] acc_bw Requested averaging value. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_accel_ln_bw(inv_imu_device_t *s, ACCEL_CONFIG1_ACCEL_FILT_BW_t acc_bw); + +/** @brief Set accel full scale range. + * @param[in] s Pointer to device. + * @param[in] accel_fsr_g Requested full scale range. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_accel_fsr(inv_imu_device_t *s, ACCEL_CONFIG0_FS_SEL_t accel_fsr_g); + +/** @brief Access accel full scale range. + * @param[in] s Pointer to device. + * @param[out] accel_fsr_g Current full scale range. + * @return 0 on success, negative value on error. + */ +int inv_imu_get_accel_fsr(inv_imu_device_t *s, ACCEL_CONFIG0_FS_SEL_t *accel_fsr_g); + +#if INV_IMU_IS_GYRO_SUPPORTED +/** @brief Enable/put gyro in low noise mode. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_enable_gyro_low_noise_mode(inv_imu_device_t *s); + +/** @brief Disable all 3 axes of gyro. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_disable_gyro(inv_imu_device_t *s); + +/** @brief Configure gyro Output Data Rate. + * @param[in] s Pointer to device. + * @param[in] frequency The requested frequency. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_gyro_frequency(inv_imu_device_t *s, const GYRO_CONFIG0_ODR_t frequency); + +/** @brief Set gyro Low-Noise bandwidth value. + * @param[in] s Pointer to device. + * @param[in] gyr_bw Requested averaging value. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_gyro_ln_bw(inv_imu_device_t *s, GYRO_CONFIG1_GYRO_FILT_BW_t gyr_bw); + +/** @brief Set gyro full scale range. + * @param[in] s Pointer to device. + * @param[in] gyro_fsr_dps Requested full scale range. +* @return 0 on success, negative value on error. + */ +int inv_imu_set_gyro_fsr(inv_imu_device_t *s, GYRO_CONFIG0_FS_SEL_t gyro_fsr_dps); + +/** @brief Access gyro full scale range. + * @param[in] s Pointer to device. + * @param[out] gyro_fsr_dps Current full scale range. + * @return 0 on success, negative value on error. + */ +int inv_imu_get_gyro_fsr(inv_imu_device_t *s, GYRO_CONFIG0_FS_SEL_t *gyro_fsr_dps); + +/** @brief Enable fsync tagging functionality. + * * Enables fsync. + * * Enables timestamp to registers. Once fsync is enabled fsync counter is pushed to + * FIFO instead of timestamp. So timestamp is made available in registers. Note that + * this increase power consumption. + * * Enables fsync related interrupt. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_enable_fsync(inv_imu_device_t *s); + +/** @brief Disable fsync tagging functionality. + * * Disables fsync. + * * Disables timestamp to registers. Once fsync is disabled timestamp is pushed to FIFO + * instead of fsync counter. So in order to decrease power consumption, timestamp is no + * more available in registers. + * * Disables fsync related interrupt. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_disable_fsync(inv_imu_device_t *s); +#endif + +/** @brief Configure SPI slew-rate. + * @param[in] s Pointer to device. + * @param[in] slew_rate Requested slew-rate. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_spi_slew_rate(inv_imu_device_t *s, const DRIVE_CONFIG3_SPI_SLEW_RATE_t slew_rate); + +/** @brief Configure INT1 pin behavior. + * @param[in] s Pointer to device. + * @param[in] conf Structure with the requested configuration. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_pin_config_int1(inv_imu_device_t *s, const inv_imu_int1_pin_config_t *conf); + +/** @brief Configure INT2 pin behavior. + * @param[in] s Pointer to device. + * @param[in] conf Structure with the requested configuration. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_pin_config_int2(inv_imu_device_t *s, const inv_imu_int2_pin_config_t *conf); + +/** @brief Configure which interrupt source can trigger INT1. + * @param[in] s Pointer to device. + * @param[in] it Structure with the corresponding state to manage INT1. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_config_int1(inv_imu_device_t *s, const inv_imu_interrupt_parameter_t *it); + +/** @brief Retrieve interrupts configuration. + * @param[in] s Pointer to device. + * @param[out] it Structure with the corresponding state to manage INT1. + * @return 0 on success, negative value on error. + */ +int inv_imu_get_config_int1(inv_imu_device_t *s, inv_imu_interrupt_parameter_t *it); + +/** @brief Configure which interrupt source can trigger INT2. + * @param[in] s Pointer to device. + * @param[in] it Structure with the corresponding state to INT2. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_config_int2(inv_imu_device_t *s, const inv_imu_interrupt_parameter_t *it); + +/** @brief Retrieve interrupts configuration. + * @param[in] s Pointer to device. + * @param[out] it Structure with the corresponding state to manage INT2. + * @return 0 on success, negative value on error. + */ +int inv_imu_get_config_int2(inv_imu_device_t *s, inv_imu_interrupt_parameter_t *it); + +/** @brief Read all data registers. + * Then it calls sensor_event_cb function passed at init for each packet. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_get_data_from_registers(inv_imu_device_t *s); + +/** @brief Read FIFO frame count. + * @param[in] s Pointer to device. + * @param[out] frame_count Number of frame available in the FIFO. + * @return 0 on success, negative value on error. + */ +int inv_imu_get_frame_count(inv_imu_device_t *s, uint16_t *frame_count); + +/** @brief Decode FIFO frame. + * @param[in] s Pointer to device. + * @param[in] frame FIFO frame data. + * @param[out] event Data content coded as an event. + * @return 0 on success, negative value on error. + */ +int inv_imu_decode_fifo_frame(inv_imu_device_t *s, const uint8_t *frame, + inv_imu_sensor_event_t *event); + +/** @brief Read all available packets from the FIFO. + * For each packet function builds a sensor event containing packet data + * and validity information. Then it calls sensor_event_cb function passed + * at init for each packet. + * @param[in] s Pointer to device. + * @return Number of valid packets read on success, negative value on error. + */ +int inv_imu_get_data_from_fifo(inv_imu_device_t *s); + +/** @brief Converts ODR's enum to period in us. + * @param[in] odr_bitfield ACCEL_CONFIG0_ODR_t or GYRO_CONFIG0_ODR_t enum. + * @return The corresponding period in us. + */ +uint32_t inv_imu_convert_odr_bitfield_to_us(uint32_t odr_bitfield); + +/** @brief Set timestamp resolution. + * @param[in] s Pointer to device. + * @param[in] timestamp_resol Requested timestamp resolution. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_timestamp_resolution(inv_imu_device_t * s, + const TMST_CONFIG1_RESOL_t timestamp_resol); + +/** @brief Reset IMU FIFO. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_reset_fifo(inv_imu_device_t *s); + +/** @brief Enable 20 bits raw data in FIFO. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_enable_high_resolution_fifo(inv_imu_device_t *s); + +/** @brief Disable 20 bits raw data in FIFO. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_disable_high_resolution_fifo(inv_imu_device_t *s); + +/** @brief Configure FIFO. + * @param[in] s Pointer to device. + * @param[in] fifo_config FIFO configuration method. + * Enabled: data are pushed to FIFO and FIFO THS interrupt is set. + * Disabled: data are not pushed to FIFO and DRDY interrupt is set. + * @return 0 on success, negative value on error. + */ +int inv_imu_configure_fifo(inv_imu_device_t *s, INV_IMU_FIFO_CONFIG_t fifo_config); + +/** @brief Get timestamp resolution + * @param[in] s Pointer to device. + * @return The timestamp resolution in us or 0 in case of error + */ +uint32_t inv_imu_get_timestamp_resolution_us(inv_imu_device_t *s); + +/** @brief Enable Wake On Motion. + * @param[in] s Pointer to device. + * @param[in] wom_x_th Threshold value for the Wake on Motion Interrupt for X-axis accel. + * @param[in] wom_y_th Threshold value for the Wake on Motion Interrupt for Y-axis accel. + * @param[in] wom_z_th Threshold value for the Wake on Motion Interrupt for Z-axis accel. + * @param[in] wom_int Select which mode between AND/OR is used to generate interrupt. + * @param[in] wom_dur Select the number of over-threshold events to wait before generating + * interrupt. + * @return 0 on success, negative value on error. + */ +int inv_imu_configure_wom(inv_imu_device_t *s, const uint8_t wom_x_th, const uint8_t wom_y_th, + const uint8_t wom_z_th, WOM_CONFIG_WOM_INT_MODE_t wom_int, + WOM_CONFIG_WOM_INT_DUR_t wom_dur); + +/** @brief Enable Wake On Motion. + * WoM requests to have the accelerometer enabled to work. + * As a consequence FIFO water-mark interrupt is disabled to only trigger WoM interrupts. + * To have good performance, it's recommended to set accel ODR to 20 ms and in Low Power + * Mode. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_enable_wom(inv_imu_device_t *s); + +/** @brief Disable Wake On Motion. + * Fifo water-mark interrupt is re-enabled when WoM is disabled. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_disable_wom(inv_imu_device_t *s); + +/** @brief Start DMP for APEX algorithms and self-test. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_start_dmp(inv_imu_device_t *s); + +/** @brief Resume DMP operations. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_resume_dmp(struct inv_imu_device *s); + +/** @brief Reset DMP for APEX algorithms and self-test. + * @param[in] s Pointer to device. + * @param[in] sram_reset Reset mode for the SRAM. + * @return 0 on success, negative value on error. + */ +int inv_imu_reset_dmp(inv_imu_device_t *s, const APEX_CONFIG0_DMP_MEM_RESET_t sram_reset); + +/** @brief Set the UI endianness and set the inv_device endianness field. + * @param[in] s Pointer to device. + * @param[in] endianness Endianness to be set. + * @return 0 on success, negative value on error. + */ +int inv_imu_set_endianness(inv_imu_device_t *s, INTF_CONFIG0_DATA_ENDIAN_t endianness); + +/** @brief Read the UI endianness and set the inv_device endianness field. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_get_endianness(inv_imu_device_t *s); + +/** @brief Configure FIFO decimation. + * @param[in] s Pointer to device. + * @param[in] dec_factor Requested decimation factor value from 2 to 256. + * @return 0 on success, negative value on error. + */ +int inv_imu_configure_fifo_data_rate(inv_imu_device_t *s, FDR_CONFIG_FDR_SEL_t dec_factor); + +/** @brief Return driver version x.y.z-suffix as a char array + * @return driver version a char array "x.y.z-suffix" + */ +const char *inv_imu_get_version(void); + +/** @brief Converts two bytes in one unsigned half-word depending on endianness + * @param[in] endianness IMU's endianness. + * @param[in] in Pointer to input. + * @param[out] out Pointer to output. + */ +static inline void format_u16_data(uint8_t endianness, const uint8_t *in, uint16_t *out) +{ + *out = (uint16_t)(endianness == INTF_CONFIG0_DATA_BIG_ENDIAN ? (in[0] << 8) | in[1] : + (in[1] << 8) | in[0]); +} + +/** @brief Converts two bytes in one signed half-word depending on endianness + * @param[in] endianness IMU's endianness. + * @param[in] in Pointer to input. + * @param[out] out Pointer to output. + */ +static inline void format_s16_data(uint8_t endianness, const uint8_t *in, int16_t *out) +{ + *out = (int16_t)(endianness == INTF_CONFIG0_DATA_BIG_ENDIAN ? (in[0] << 8) | in[1] : + (in[1] << 8) | in[0]); +} + +#if INV_IMU_HFSR_SUPPORTED +/** @brief Write 'size' bytes pointed by 'data' in SRAM at offset given in parameters. + * @param[in] data pointer to data to be written in SRAM + * @param[in] offset offset in bytes from SRAM start address where data should be written + * @param[in] size number of bytes to write + * @return 0 in case of success, negative value on error. + */ +int inv_imu_write_sram(struct inv_imu_device *s, const uint8_t *data, uint32_t offset, + uint32_t size); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* _INV_IMU_DRIVER_H_ */ + +/** @} */ diff --git a/icm42670p/imu/inv_imu_extfunc.h b/icm42670p/imu/inv_imu_extfunc.h new file mode 100644 index 0000000..7d7ad35 --- /dev/null +++ b/icm42670p/imu/inv_imu_extfunc.h @@ -0,0 +1,50 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/** @defgroup ExtFunc ExtFunc + * @brief External functions (to be implemented in application) required by the driver. + * @{ + */ + +/** @file inv_imu_extfunc.h */ + +#ifndef _INV_IMU_EXTFUNC_H_ +#define _INV_IMU_EXTFUNC_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** @brief Sleep function. + * @param[in] us Sleep duration in microseconds (us). + */ +extern void inv_imu_sleep_us(uint32_t us); + +/** @brief Get time function. Value is expected to be on 64 bits with a 1 us resolution. + * @return The current time in us. + */ +extern uint64_t inv_imu_get_time_us(void); + +#ifdef __cplusplus +} +#endif + +#endif /* _INV_IMU_EXTFUNC_H_ */ + +/** @} */ diff --git a/icm42670p/imu/inv_imu_regmap_rev_a.h b/icm42670p/imu/inv_imu_regmap_rev_a.h new file mode 100644 index 0000000..694169a --- /dev/null +++ b/icm42670p/imu/inv_imu_regmap_rev_a.h @@ -0,0 +1,3369 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#ifndef _INV_IMU_REGMAP_REV_A_H_ +#define _INV_IMU_REGMAP_REV_A_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/** @file inv_imu_regmap_rev_a.h + * File exposing the device register map + */ + +#include + +/* BANK0 */ +#define MCLK_RDY 0x10000 +#define DEVICE_CONFIG 0x10001 +#define SIGNAL_PATH_RESET 0x10002 +#define DRIVE_CONFIG1 0x10003 +#define DRIVE_CONFIG2 0x10004 +#define DRIVE_CONFIG3 0x10005 +#define INT_CONFIG 0x10006 +#define TEMP_DATA1 0x10009 +#define TEMP_DATA0 0x1000a +#define ACCEL_DATA_X1 0x1000b +#define ACCEL_DATA_X0 0x1000c +#define ACCEL_DATA_Y1 0x1000d +#define ACCEL_DATA_Y0 0x1000e +#define ACCEL_DATA_Z1 0x1000f +#define ACCEL_DATA_Z0 0x10010 +#define GYRO_DATA_X1 0x10011 +#define GYRO_DATA_X0 0x10012 +#define GYRO_DATA_Y1 0x10013 +#define GYRO_DATA_Y0 0x10014 +#define GYRO_DATA_Z1 0x10015 +#define GYRO_DATA_Z0 0x10016 +#define TMST_FSYNCH 0x10017 +#define TMST_FSYNCL 0x10018 +#define APEX_DATA4 0x1001d +#define APEX_DATA5 0x1001e +#define PWR_MGMT0 0x1001f +#define GYRO_CONFIG0 0x10020 +#define ACCEL_CONFIG0 0x10021 +#define TEMP_CONFIG0 0x10022 +#define GYRO_CONFIG1 0x10023 +#define ACCEL_CONFIG1 0x10024 +#define APEX_CONFIG0 0x10025 +#define APEX_CONFIG1 0x10026 +#define WOM_CONFIG 0x10027 +#define FIFO_CONFIG1 0x10028 +#define FIFO_CONFIG2 0x10029 +#define FIFO_CONFIG3 0x1002a +#define INT_SOURCE0 0x1002b +#define INT_SOURCE1 0x1002c +#define INT_SOURCE3 0x1002d +#define INT_SOURCE4 0x1002e +#define FIFO_LOST_PKT0 0x1002f +#define FIFO_LOST_PKT1 0x10030 +#define APEX_DATA0 0x10031 +#define APEX_DATA1 0x10032 +#define APEX_DATA2 0x10033 +#define APEX_DATA3 0x10034 +#define INTF_CONFIG0 0x10035 +#define INTF_CONFIG1 0x10036 +#define INT_STATUS_DRDY 0x10039 +#define INT_STATUS 0x1003a +#define INT_STATUS2 0x1003b +#define INT_STATUS3 0x1003c +#define FIFO_COUNTH 0x1003d +#define FIFO_COUNTL 0x1003e +#define FIFO_DATA 0x1003f +#define WHO_AM_I 0x10075 +#define BLK_SEL_W 0x10079 +#define MADDR_W 0x1007a +#define M_W 0x1007b +#define BLK_SEL_R 0x1007c +#define MADDR_R 0x1007d +#define M_R 0x1007e + +/* MREG1 */ +#define TMST_CONFIG1_MREG1 0x00 +#define FIFO_CONFIG5_MREG1 0x01 +#define FIFO_CONFIG6_MREG1 0x02 +#define FSYNC_CONFIG_MREG1 0x03 +#define INT_CONFIG0_MREG1 0x04 +#define INT_CONFIG1_MREG1 0x05 +#define SENSOR_CONFIG3_MREG1 0x06 +#define ST_CONFIG_MREG1 0x13 +#define SELFTEST_MREG1 0x14 +#define INTF_CONFIG6_MREG1 0x23 +#define INTF_CONFIG10_MREG1 0x25 +#define INTF_CONFIG7_MREG1 0x28 +#define OTP_CONFIG_MREG1 0x2b +#define INT_SOURCE6_MREG1 0x2f +#define INT_SOURCE7_MREG1 0x30 +#define INT_SOURCE8_MREG1 0x31 +#define INT_SOURCE9_MREG1 0x32 +#define INT_SOURCE10_MREG1 0x33 +#define APEX_CONFIG2_MREG1 0x44 +#define APEX_CONFIG3_MREG1 0x45 +#define APEX_CONFIG4_MREG1 0x46 +#define APEX_CONFIG5_MREG1 0x47 +#define APEX_CONFIG9_MREG1 0x48 +#define APEX_CONFIG10_MREG1 0x49 +#define APEX_CONFIG11_MREG1 0x4a +#define ACCEL_WOM_X_THR_MREG1 0x4b +#define ACCEL_WOM_Y_THR_MREG1 0x4c +#define ACCEL_WOM_Z_THR_MREG1 0x4d +#define OFFSET_USER0_MREG1 0x4e +#define OFFSET_USER1_MREG1 0x4f +#define OFFSET_USER2_MREG1 0x50 +#define OFFSET_USER3_MREG1 0x51 +#define OFFSET_USER4_MREG1 0x52 +#define OFFSET_USER5_MREG1 0x53 +#define OFFSET_USER6_MREG1 0x54 +#define OFFSET_USER7_MREG1 0x55 +#define OFFSET_USER8_MREG1 0x56 +#define ST_STATUS1_MREG1 0x63 +#define ST_STATUS2_MREG1 0x64 +#define FDR_CONFIG_MREG1 0x66 +#define APEX_CONFIG12_MREG1 0x67 + +/* MREG3 */ +#define XA_ST_DATA_MREG3 0x5000 +#define YA_ST_DATA_MREG3 0x5001 +#define ZA_ST_DATA_MREG3 0x5002 +#define XG_ST_DATA_MREG3 0x5003 +#define YG_ST_DATA_MREG3 0x5004 +#define ZG_ST_DATA_MREG3 0x5005 + +/* MREG2 */ +#define OTP_CTRL7_MREG2 0x2806 + + +/* --------------------------------------------------------------------------- + * register BANK0 + * ---------------------------------------------------------------------------*/ + +/* + * MCLK_RDY + * Register Name : MCLK_RDY + */ + +/* + * mclk_rdy + * 0: Indicates internal clock is currently not running + * 1: Indicates internal clock is currently running + */ +#define MCLK_RDY_MCLK_RDY_POS 0x03 +#define MCLK_RDY_MCLK_RDY_MASK (0x01 << MCLK_RDY_MCLK_RDY_POS) + +/* + * DEVICE_CONFIG + * Register Name : DEVICE_CONFIG + */ + +/* + * spi_ap_4wire + * 0: AP interface uses 3-wire SPI mode + * 1: AP interface uses 4-wire SPI mode + */ +#define DEVICE_CONFIG_SPI_AP_4WIRE_POS 0x02 +#define DEVICE_CONFIG_SPI_AP_4WIRE_MASK (0x01 << DEVICE_CONFIG_SPI_AP_4WIRE_POS) + +/* + * spi_mode + * SPI mode selection + * + * 0: Mode 0 and Mode 3 + * 1: Mode 1 and Mode 2 + * + * If device is operating in non-SPI mode, user is not allowed to change the power-on default setting of this register. Change of this register setting will not take effect till AP_CS = 1. + */ +#define DEVICE_CONFIG_SPI_MODE_POS 0x00 +#define DEVICE_CONFIG_SPI_MODE_MASK 0x01 + + + +/* + * SIGNAL_PATH_RESET + * Register Name : SIGNAL_PATH_RESET + */ + +/* + * soft_reset_device_config + * Software Reset (auto clear bit) + * + * 0: Software reset not enabled + * 1: Software reset enabled + */ +#define SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_POS 0x04 +#define SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_MASK (0x01 << SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_POS) + +/* + * fifo_flush + * When set to 1, FIFO will get flushed. + * FIFO flush requires the following programming sequence: + * • Write FIFO_FLUSH =1 + * • Wait for 1.5 µs + * • Read FIFO_FLUSH, it should now be 0 + * Host can only program this register bit to 1. + */ +#define SIGNAL_PATH_RESET_FIFO_FLUSH_POS 0x02 +#define SIGNAL_PATH_RESET_FIFO_FLUSH_MASK (0x01 << SIGNAL_PATH_RESET_FIFO_FLUSH_POS) + + + +/* + * DRIVE_CONFIG1 + * Register Name : DRIVE_CONFIG1 + */ + +/* + * i3c_ddr_slew_rate + * Controls slew rate for output pin 14 when device is in I3CSM DDR protocol. + * While in I3CSM operation, the device automatically switches to use I3C_DDR_SLEW_RATE after receiving ENTHDR0 ccc command from the host. The device automatically switches back to I3C_SDR_SLEW_RATE after the host issues HDR_EXIT pattern. + * + * 000: MIN: 20 ns; TYP: 40 ns; MAX: 60 ns + * 001: MIN: 12 ns; TYP: 24 ns; MAX: 36 ns + * 010: MIN: 6 ns; TYP: 12 ns; MAX: 19 ns + * 011: MIN: 4 ns; TYP: 8 ns; MAX: 14 ns + * 100: MIN: 2 ns; TYP: 4 ns; MAX: 8 ns + * 101: MAX: 2 ns + * 110: Reserved + * 111: Reserved + * + * This register field should not be programmed in I3C/DDR mode. + */ +#define DRIVE_CONFIG1_I3C_DDR_SLEW_RATE_POS 0x03 +#define DRIVE_CONFIG1_I3C_DDR_SLEW_RATE_MASK (0x07 << DRIVE_CONFIG1_I3C_DDR_SLEW_RATE_POS) + +/* + * i3c_sdr_slew_rate + * Controls slew rate for output pin 14 in I3CSM SDR protocol. + * After device reset, I2C_SLEW_RATE is used by default. If I3CSM feature is enabled, the device automatically switches to use I3C_SDR_SLEW_RATE after receiving 0x7E+W message (an I3CSM broadcast message). + * + * 000: MIN: 20 ns; TYP: 40 ns; MAX: 60 ns + * 001: MIN: 12 ns; TYP: 24 ns; MAX: 36 ns + * 010: MIN: 6 ns; TYP: 12 ns; MAX: 19 ns + * 011: MIN: 4 ns; TYP: 8 ns; MAX: 14 ns + * 100: MIN: 2 ns; TYP: 4 ns; MAX: 8 ns + * 101: MAX: 2 ns + * 110: Reserved + * 111: Reserved + * + * This register field should not be programmed in I3C/DDR mode + */ +#define DRIVE_CONFIG1_I3C_SDR_SLEW_RATE_POS 0x00 +#define DRIVE_CONFIG1_I3C_SDR_SLEW_RATE_MASK 0x07 + + + +/* + * DRIVE_CONFIG2 + * Register Name : DRIVE_CONFIG2 + */ + +/* + * i2c_slew_rate + * Controls slew rate for output pin 14 in I2C mode. + * After device reset, the I2C_SLEW_RATE is used by default. If the 1st write operation from host is an SPI transaction, the device automatically switches to SPI_SLEW_RATE. If I3CSM feature is enabled, the device automatically switches to I3C_SDR_SLEW_RATE after receiving 0x7E+W message (an I3C broadcast message). + * + * 000: MIN: 20 ns; TYP: 40 ns; MAX: 60 ns + * 001: MIN: 12 ns; TYP: 24 ns; MAX: 36 ns + * 010: MIN: 6 ns; TYP: 12 ns; MAX: 19 ns + * 011: MIN: 4 ns; TYP: 8 ns; MAX: 14 ns + * 100: MIN: 2 ns; TYP: 4 ns; MAX: 8 ns + * 101: MAX: 2 ns + * 110: Reserved + * 111: Reserved + * + * This register field should not be programmed in I3C/DDR mode + */ +#define DRIVE_CONFIG2_I2C_SLEW_RATE_POS 0x03 +#define DRIVE_CONFIG2_I2C_SLEW_RATE_MASK (0x07 << DRIVE_CONFIG2_I2C_SLEW_RATE_POS) + +/* + * all_slew_rate + * Configure drive strength for all output pins in all modes (SPI3, SPI4, I2C, I3CSM) excluding pin 14. + * + * 000: MIN: 20 ns; TYP: 40 ns; MAX: 60 ns + * 001: MIN: 12 ns; TYP: 24 ns; MAX: 36 ns + * 010: MIN: 6 ns; TYP: 12 ns; MAX: 19 ns + * 011: MIN: 4 ns; TYP: 8 ns; MAX: 14 ns + * 100: MIN: 2 ns; TYP: 4 ns; MAX: 8 ns + * 101: MAX: 2 ns + * 110: Reserved + * 111: Reserved + * + * This register field should not be programmed in I3C/DDR mode + */ +#define DRIVE_CONFIG2_ALL_SLEW_RATE_POS 0x00 +#define DRIVE_CONFIG2_ALL_SLEW_RATE_MASK 0x07 + + + +/* + * DRIVE_CONFIG3 + * Register Name : DRIVE_CONFIG3 + */ + +/* + * spi_slew_rate + * Controls slew rate for output pin 14 in SPI 3-wire mode. In SPI 4-wire mode this register controls the slew rate of pin 1 as it is used as an output in SPI 4-wire mode only. After chip reset, the I2C_SLEW_RATE is used by default for pin 14 pin. If the 1st write operation from the host is an SPI3/4 transaction, the device automatically switches to SPI_SLEW_RATE. + * + * 000: MIN: 20 ns; TYP: 40 ns; MAX: 60 ns + * 001: MIN: 12 ns; TYP: 24 ns; MAX: 36 ns + * 010: MIN: 6 ns; TYP: 12 ns; MAX: 19 ns + * 011: MIN: 4 ns; TYP: 8 ns; MAX: 14 ns + * 100: MIN: 2 ns; TYP: 4 ns; MAX: 8 ns + * 101: MAX: 2 ns + * 110: Reserved + * 111: Reserved + * + * This register field should not be programmed in I3C/DDR mode + */ +#define DRIVE_CONFIG3_SPI_SLEW_RATE_POS 0x00 +#define DRIVE_CONFIG3_SPI_SLEW_RATE_MASK 0x07 + + + +/* + * INT_CONFIG + * Register Name : INT_CONFIG + */ + +/* + * int2_mode + * Interrupt mode and drive circuit shall be configurable by register. + * Interrupt Mode + * 1: Latched Mode + * 0: Pulsed Mode + */ +#define INT_CONFIG_INT2_MODE_POS 0x05 +#define INT_CONFIG_INT2_MODE_MASK (0x01 << INT_CONFIG_INT2_MODE_POS) + +/* + * int2_drive_circuit + * Interrupt mode and drive circuit shall be configurable by register. + * Drive Circuit + * 1: Push-Pull + * 0: Open drain + */ +#define INT_CONFIG_INT2_DRIVE_CIRCUIT_POS 0x04 +#define INT_CONFIG_INT2_DRIVE_CIRCUIT_MASK (0x01 << INT_CONFIG_INT2_DRIVE_CIRCUIT_POS) + +/* + * int2_polarity + * Interrupt mode and drive circuit shall be configurable by register. + * Interrupt Polarity + * 1: Active High + * 0: Active Low + */ +#define INT_CONFIG_INT2_POLARITY_POS 0x03 +#define INT_CONFIG_INT2_POLARITY_MASK (0x01 << INT_CONFIG_INT2_POLARITY_POS) + +/* + * int1_mode + * Interrupt mode and drive circuit shall be configurable by register. + * Interrupt Mode + * 1: Latched Mode + * 0: Pulsed Mode + */ +#define INT_CONFIG_INT1_MODE_POS 0x02 +#define INT_CONFIG_INT1_MODE_MASK (0x01 << INT_CONFIG_INT1_MODE_POS) + +/* + * int1_drive_circuit + * Interrupt mode and drive circuit shall be configurable by register. + * Drive Circuit + * 1: Push-Pull + * 0: Open drain + */ +#define INT_CONFIG_INT1_DRIVE_CIRCUIT_POS 0x01 +#define INT_CONFIG_INT1_DRIVE_CIRCUIT_MASK (0x01 << INT_CONFIG_INT1_DRIVE_CIRCUIT_POS) + +/* + * int1_polarity + * Interrupt mode and drive circuit shall be configurable by register. + * Interrupt Polarity + * 1: Active High + * 0: Active Low + */ +#define INT_CONFIG_INT1_POLARITY_POS 0x00 +#define INT_CONFIG_INT1_POLARITY_MASK 0x01 + + + +/* + * TEMP_DATA1 + * Register Name : TEMP_DATA1 + */ + +/* + * temp_data + * Temperature data + */ +#define TEMP_DATA1_TEMP_DATA_POS 0x00 +#define TEMP_DATA1_TEMP_DATA_MASK 0xff + + + +/* + * TEMP_DATA0 + * Register Name : TEMP_DATA0 + */ + +/* + * temp_data + * Temperature data + */ +#define TEMP_DATA0_TEMP_DATA_POS 0x00 +#define TEMP_DATA0_TEMP_DATA_MASK 0xff + + + +/* + * ACCEL_DATA_X1 + * Register Name : ACCEL_DATA_X1 + */ + +/* + * accel_data_x + * Accel X axis data + */ +#define ACCEL_DATA_X1_ACCEL_DATA_X_POS 0x00 +#define ACCEL_DATA_X1_ACCEL_DATA_X_MASK 0xff + + + +/* + * ACCEL_DATA_X0 + * Register Name : ACCEL_DATA_X0 + */ + +/* + * accel_data_x + * Accel X axis data + */ +#define ACCEL_DATA_X0_ACCEL_DATA_X_POS 0x00 +#define ACCEL_DATA_X0_ACCEL_DATA_X_MASK 0xff + + + +/* + * ACCEL_DATA_Y1 + * Register Name : ACCEL_DATA_Y1 + */ + +/* + * accel_data_y + * Accel Y axis data + */ +#define ACCEL_DATA_Y1_ACCEL_DATA_Y_POS 0x00 +#define ACCEL_DATA_Y1_ACCEL_DATA_Y_MASK 0xff + + + +/* + * ACCEL_DATA_Y0 + * Register Name : ACCEL_DATA_Y0 + */ + +/* + * accel_data_y + * Accel Y axis data + */ +#define ACCEL_DATA_Y0_ACCEL_DATA_Y_POS 0x00 +#define ACCEL_DATA_Y0_ACCEL_DATA_Y_MASK 0xff + + + +/* + * ACCEL_DATA_Z1 + * Register Name : ACCEL_DATA_Z1 + */ + +/* + * accel_data_z + * Accel Z axis data + */ +#define ACCEL_DATA_Z1_ACCEL_DATA_Z_POS 0x00 +#define ACCEL_DATA_Z1_ACCEL_DATA_Z_MASK 0xff + + + +/* + * ACCEL_DATA_Z0 + * Register Name : ACCEL_DATA_Z0 + */ + +/* + * accel_data_z + * Accel Z axis data + */ +#define ACCEL_DATA_Z0_ACCEL_DATA_Z_POS 0x00 +#define ACCEL_DATA_Z0_ACCEL_DATA_Z_MASK 0xff + + + +/* + * GYRO_DATA_X1 + * Register Name : GYRO_DATA_X1 + */ + +/* + * gyro_data_x + * Gyro X axis data + */ +#define GYRO_DATA_X1_GYRO_DATA_X_POS 0x00 +#define GYRO_DATA_X1_GYRO_DATA_X_MASK 0xff + + + +/* + * GYRO_DATA_X0 + * Register Name : GYRO_DATA_X0 + */ + +/* + * gyro_data_x + * Gyro X axis data + */ +#define GYRO_DATA_X0_GYRO_DATA_X_POS 0x00 +#define GYRO_DATA_X0_GYRO_DATA_X_MASK 0xff + + + +/* + * GYRO_DATA_Y1 + * Register Name : GYRO_DATA_Y1 + */ + +/* + * gyro_data_y + * Gyro Y axis data + */ +#define GYRO_DATA_Y1_GYRO_DATA_Y_POS 0x00 +#define GYRO_DATA_Y1_GYRO_DATA_Y_MASK 0xff + + + +/* + * GYRO_DATA_Y0 + * Register Name : GYRO_DATA_Y0 + */ + +/* + * gyro_data_y + * Gyro Y axis data + */ +#define GYRO_DATA_Y0_GYRO_DATA_Y_POS 0x00 +#define GYRO_DATA_Y0_GYRO_DATA_Y_MASK 0xff + + + +/* + * GYRO_DATA_Z1 + * Register Name : GYRO_DATA_Z1 + */ + +/* + * gyro_data_z + * Gyro Z axis data + */ +#define GYRO_DATA_Z1_GYRO_DATA_Z_POS 0x00 +#define GYRO_DATA_Z1_GYRO_DATA_Z_MASK 0xff + + + +/* + * GYRO_DATA_Z0 + * Register Name : GYRO_DATA_Z0 + */ + +/* + * gyro_data_z + * Gyro Z axis data + */ +#define GYRO_DATA_Z0_GYRO_DATA_Z_POS 0x00 +#define GYRO_DATA_Z0_GYRO_DATA_Z_MASK 0xff + + + +/* + * TMST_FSYNCH + * Register Name : TMST_FSYNCH + */ + +/* + * tmst_fsync_data + * Stores the time delta from the rising edge of FSYNC to the latest ODR until the UI Interface reads the FSYNC tag in the status register + */ +#define TMST_FSYNCH_TMST_FSYNC_DATA_POS 0x00 +#define TMST_FSYNCH_TMST_FSYNC_DATA_MASK 0xff + + + +/* + * TMST_FSYNCL + * Register Name : TMST_FSYNCL + */ + +/* + * tmst_fsync_data + * Stores the time delta from the rising edge of FSYNC to the latest ODR until the UI Interface reads the FSYNC tag in the status register + */ +#define TMST_FSYNCL_TMST_FSYNC_DATA_POS 0x00 +#define TMST_FSYNCL_TMST_FSYNC_DATA_MASK 0xff + + + +/* + * APEX_DATA4 + * Register Name : APEX_DATA4 + */ + +/* + * ff_dur + * Free Fall duration. The duration is given in number of samples and it can be converted to freefall distance by applying the following formula: + * ff_distance = 0.5*9.81*(ff_duration*dmp_odr_s)^2) + * Note: dmp_odr_s in the duration of DMP_ODR expressed in seconds. + */ +#define APEX_DATA4_FF_DUR_POS 0x00 +#define APEX_DATA4_FF_DUR_MASK 0xff + + + +/* + * APEX_DATA5 + * Register Name : APEX_DATA5 + */ + +/* + * ff_dur + * Free Fall duration. The duration is given in number of samples and it can be converted to freefall distance by applying the following formula: + * ff_distance = 0.5*9.81*(ff_duration*dmp_odr_s)^2) + * Note: dmp_odr_s in the duration of DMP_ODR expressed in seconds. + */ +#define APEX_DATA5_FF_DUR_POS 0x00 +#define APEX_DATA5_FF_DUR_MASK 0xff + + + +/* + * PWR_MGMT0 + * Register Name : PWR_MGMT0 + */ + +/* + * accel_lp_clk_sel + * 0: Accelerometer LP mode uses Wake Up oscillator clock. This is the lowest power consumption mode and it is the recommended setting. + * 1: Accelerometer LP mode uses RC oscillator clock. + * + * This field can be changed on-the-fly even if accel sensor is on. + */ +#define PWR_MGMT0_ACCEL_LP_CLK_SEL_POS 0x07 +#define PWR_MGMT0_ACCEL_LP_CLK_SEL_MASK (0x01 << PWR_MGMT0_ACCEL_LP_CLK_SEL_POS) + +/* + * idle + * If this bit is set to 1, the RC oscillator is powered on even if Accel and Gyro are powered off. + * Nominally this bit is set to 0, so when Accel and Gyro are powered off, + * the chip will go to OFF state , since the RC oscillator will also be powered off. + * + * This field can be changed on-the-fly even if a sensor is already on + */ +#define PWR_MGMT0_IDLE_POS 0x04 +#define PWR_MGMT0_IDLE_MASK (0x01 << PWR_MGMT0_IDLE_POS) + +/* + * gyro_mode + * 00: Turns gyroscope off + * 01: Places gyroscope in Standby Mode + * 10: Reserved + * 11: Places gyroscope in Low Noise (LN) Mode + * + * Gyroscope needs to be kept ON for a minimum of 45ms. When transitioning from OFF to any of the other modes, do not issue any register writes for 200 µs. + * + * This field can be changed on-the-fly even if gyro sensor is on + */ +#define PWR_MGMT0_GYRO_MODE_POS 0x02 +#define PWR_MGMT0_GYRO_MODE_MASK (0x03 << PWR_MGMT0_GYRO_MODE_POS) + +/* + * accel_mode + * 00: Turns accelerometer off + * 01: Turns accelerometer off + * 10: Places accelerometer in Low Power (LP) Mode + * 11: Places accelerometer in Low Noise (LN) Mode + * + * When selecting LP Mode please refer to ACCEL_LP_CLK_SEL setting, bit[7] of this register. + * + * Before entering LP mode and during LP Mode the following combinations of ODR and averaging are not permitted: + * 1) ODR=1600 Hz or ODR=800 Hz: any averaging. + * 2) ODR=400 Hz: averaging=16x, 32x or 64x. + * 3) ODR=200 Hz: averaging=64x. + * + * When transitioning from OFF to any of the other modes, do not issue any register writes for 200 µs. + * + * This field can be changed on-the-fly even if accel sensor is on + */ +#define PWR_MGMT0_ACCEL_MODE_POS 0x00 +#define PWR_MGMT0_ACCEL_MODE_MASK 0x03 + + + +/* + * GYRO_CONFIG0 + * Register Name : GYRO_CONFIG0 + */ + +/* + * gyro_ui_fs_sel + * Full scale select for gyroscope UI interface output + * + * 00: ±2000 dps + * 01: ±1000 dps + * 10: ±500 dps + * 11: ±250 dps + * + * This field can be changed on-the-fly even if gyro sensor is on + */ +#define GYRO_CONFIG0_GYRO_UI_FS_SEL_POS 0x05 +#define GYRO_CONFIG0_GYRO_UI_FS_SEL_MASK (0x03 << GYRO_CONFIG0_GYRO_UI_FS_SEL_POS) + +/* + * gyro_odr + * Gyroscope ODR selection for UI interface output + * + * 0000: Reserved + * 0001: Reserved + * 0010: Reserved + * 0011: Reserved + * 0100: Reserved + * 0101: 1.6k Hz + * 0110: 800 Hz + * 0111: 400 Hz + * 1000: 200 Hz + * 1001: 100 Hz + * 1010: 50 Hz + * 1011: 25 Hz + * 1100: 12.5 Hz + * 1101: Reserved + * 1110: Reserved + * 1111: Reserved + * + * This field can be changed on-the-fly even if gyro sensor is on + */ +#define GYRO_CONFIG0_GYRO_ODR_POS 0x00 +#define GYRO_CONFIG0_GYRO_ODR_MASK 0x0f + + + +/* + * ACCEL_CONFIG0 + * Register Name : ACCEL_CONFIG0 + */ + +/* + * accel_ui_fs_sel + * Full scale select for accelerometer UI interface output + * + * 00: ±16g + * 01: ±8g + * 10: ±4g + * 11: ±2g + * + * This field can be changed on-the-fly even if accel sensor is on + */ +#define ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS 0x05 +#define ACCEL_CONFIG0_ACCEL_UI_FS_SEL_MASK (0x03 << ACCEL_CONFIG0_ACCEL_UI_FS_SEL_POS) + +/* + * accel_odr + * Accelerometer ODR selection for UI interface output + * + * 0000: Reserved + * 0001: Reserved + * 0010: Reserved + * 0011: Reserved + * 0100: Reserved + * 0101: 1.6 kHz (LN mode) + * 0110: 800 Hz (LN mode) + * 0111: 400 Hz (LP or LN mode) + * 1000: 200 Hz (LP or LN mode) + * 1001: 100 Hz (LP or LN mode) + * 1010: 50 Hz (LP or LN mode) + * 1011: 25 Hz (LP or LN mode) + * 1100: 12.5 Hz (LP or LN mode) + * 1101: 6.25 Hz (LP mode) + * 1110: 3.125 Hz (LP mode) + * 1111: 1.5625 Hz (LP mode) + * + * This field can be changed on-the-fly when accel sensor is on + */ +#define ACCEL_CONFIG0_ACCEL_ODR_POS 0x00 +#define ACCEL_CONFIG0_ACCEL_ODR_MASK 0x0f + + + +/* + * TEMP_CONFIG0 + * Register Name : TEMP_CONFIG0 + */ + +/* + * temp_filt_bw + * Sets the bandwidth of the temperature signal DLPF + * + * 000: DLPF bypassed + * 001: DLPF BW = 180 Hz + * 010: DLPF BW = 72 Hz + * 011: DLPF BW = 34 Hz + * 100: DLPF BW = 16 Hz + * 101: DLPF BW = 8 Hz + * 110: DLPF BW = 4 Hz + * 111: DLPF BW = 4 Hz + * + * This field can be changed on-the-fly even if sensor is on + */ +#define TEMP_CONFIG0_TEMP_FILT_BW_POS 0x04 +#define TEMP_CONFIG0_TEMP_FILT_BW_MASK (0x07 << TEMP_CONFIG0_TEMP_FILT_BW_POS) + + + +/* + * GYRO_CONFIG1 + * Register Name : GYRO_CONFIG1 + */ + +/* + * gyro_ui_filt_bw + * Selects GYRO UI low pass filter bandwidth + * + * 000: Low pass filter bypassed + * 001: 180 Hz + * 010: 121 Hz + * 011: 73 Hz + * 100: 53 Hz + * 101: 34 Hz + * 110: 25 Hz + * 111: 16 Hz + * + * This field can be changed on-the-fly even if gyro sensor is on + */ +#define GYRO_CONFIG1_GYRO_UI_FILT_BW_POS 0x00 +#define GYRO_CONFIG1_GYRO_UI_FILT_BW_MASK 0x07 + + + +/* + * ACCEL_CONFIG1 + * Register Name : ACCEL_CONFIG1 + */ + +/* + * accel_ui_avg + * Selects averaging filter setting to create accelerometer output in accelerometer low power mode (LPM) + * + * 000: 2x average + * 001: 4x average + * 010: 8x average + * 011: 16x average + * 100: 32x average + * 101: 64x average + * 110: 64x average + * 111: 64x average + * + * This field cannot be changed when the accel sensor is in LPM + */ +#define ACCEL_CONFIG1_ACCEL_UI_AVG_POS 0x04 +#define ACCEL_CONFIG1_ACCEL_UI_AVG_MASK (0x07 << ACCEL_CONFIG1_ACCEL_UI_AVG_POS) + +/* + * accel_ui_filt_bw + * Selects ACCEL UI low pass filter bandwidth + * + * 000: Low pass filter bypassed + * 001: 180 Hz + * 010: 121 Hz + * 011: 73 Hz + * 100: 53 Hz + * 101: 34 Hz + * 110: 25 Hz + * 111: 16 Hz + * + * This field can be changed on-the-fly even if accel sensor is on + */ +#define ACCEL_CONFIG1_ACCEL_UI_FILT_BW_POS 0x00 +#define ACCEL_CONFIG1_ACCEL_UI_FILT_BW_MASK 0x07 + + + +/* + * APEX_CONFIG0 + * Register Name : APEX_CONFIG0 + */ + +/* + * dmp_power_save_en + * When this bit is set to 1, power saving is enabled for DMP algorithms + */ +#define APEX_CONFIG0_DMP_POWER_SAVE_EN_POS 0x03 +#define APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK (0x01 << APEX_CONFIG0_DMP_POWER_SAVE_EN_POS) + +/* + * dmp_init_en + * When this bit is set to 1, DMP runs DMP SW initialization procedure. Bit is reset by hardware when the procedure is finished. All other APEX features are ignored as long as DMP_INIT_EN is set. + * + * This field can be changed on-the-fly even if accel sensor is on. + */ +#define APEX_CONFIG0_DMP_INIT_EN_POS 0x02 +#define APEX_CONFIG0_DMP_INIT_EN_MASK (0x01 << APEX_CONFIG0_DMP_INIT_EN_POS) + +/* + * dmp_mem_reset_en + * When this bit is set to 1, it clears DMP SRAM for APEX operation or Self-test operation. + */ +#define APEX_CONFIG0_DMP_MEM_RESET_EN_POS 0x00 +#define APEX_CONFIG0_DMP_MEM_RESET_EN_MASK 0x03 + + + +/* + * APEX_CONFIG1 + * Register Name : APEX_CONFIG1 + */ + +/* + * smd_enable + * 0: Significant Motion Detection not enabled + * 1: Significant Motion Detection enabled + * + * This field can be changed on-the-fly even if accel sensor is on + */ +#define APEX_CONFIG1_SMD_ENABLE_POS 0x06 +#define APEX_CONFIG1_SMD_ENABLE_MASK (0x01 << APEX_CONFIG1_SMD_ENABLE_POS) + +/* + * ff_enable + * 0: Freefall Detection not enabled + * 1: Freefall Detection enabled + * + * This field can be changed on-the-fly even if accel sensor is on + */ +#define APEX_CONFIG1_FF_ENABLE_POS 0x05 +#define APEX_CONFIG1_FF_ENABLE_MASK (0x01 << APEX_CONFIG1_FF_ENABLE_POS) + +/* + * tilt_enable + * 0: Tilt Detection not enabled + * 1: Tilt Detection enabled + * + * This field can be changed on-the-fly even if accel sensor is on + */ +#define APEX_CONFIG1_TILT_ENABLE_POS 0x04 +#define APEX_CONFIG1_TILT_ENABLE_MASK (0x01 << APEX_CONFIG1_TILT_ENABLE_POS) + +/* + * ped_enable + * 0: Pedometer not enabled + * 1: Pedometer enabled + * + * This field can be changed on-the-fly even if accel sensor is on + */ +#define APEX_CONFIG1_PED_ENABLE_POS 0x03 +#define APEX_CONFIG1_PED_ENABLE_MASK (0x01 << APEX_CONFIG1_PED_ENABLE_POS) + +/* + * dmp_odr + * 00: 25 Hz + * 01: 400 Hz + * 10: 50 Hz + * 11: 100 Hz + * + * The ACCEL_ODR field must be configured to an ODR equal or greater to the DMP_ODR field, for correct device operation. + * + * This field can be changed on-the-fly even if accel sensor is on + */ +#define APEX_CONFIG1_DMP_ODR_POS 0x00 +#define APEX_CONFIG1_DMP_ODR_MASK 0x03 + + + +/* + * WOM_CONFIG + * Register Name : WOM_CONFIG + */ + +/* + * wom_int_dur + * Selects Wake on Motion interrupt assertion from among the following options + * + * 00: WoM interrupt asserted at first overthreshold event + * 01: WoM interrupt asserted at second overthreshold event + * 10: WoM interrupt asserted at third overthreshold event + * 11: WoM interrupt asserted at fourth overthreshold event + * + * This field can be changed on-the-fly even if accel sensor is on, but it cannot be changed if WOM_EN is already enabled + */ +#define WOM_CONFIG_WOM_INT_DUR_POS 0x03 +#define WOM_CONFIG_WOM_INT_DUR_MASK (0x03 << WOM_CONFIG_WOM_INT_DUR_POS) + +/* + * wom_int_mode + * 0: Set WoM interrupt on the OR of all enabled accelerometer thresholds + * 1: Set WoM interrupt on the AND of all enabled accelerometer thresholds + * + * This field can be changed on-the-fly even if accel sensor is on, but it cannot be changed if WOM_EN is already enabled + */ +#define WOM_CONFIG_WOM_INT_MODE_POS 0x02 +#define WOM_CONFIG_WOM_INT_MODE_MASK (0x01 << WOM_CONFIG_WOM_INT_MODE_POS) + +/* + * wom_mode + * 0 - Initial sample is stored. Future samples are compared to initial sample + * 1 - Compare current sample to previous sample + * + * This field can be changed on-the-fly even if accel sensor is already on, but it cannot be changed if wom_en is already enabled. + */ +#define WOM_CONFIG_WOM_MODE_POS 0x01 +#define WOM_CONFIG_WOM_MODE_MASK (0x01 << WOM_CONFIG_WOM_MODE_POS) + +/* + * wom_en + * 1: enable wake-on-motion detection. + * 0: disable wake-on-motion detection. + * + * This field can be changed on-the-fly even if accel sensor is already on. + */ +#define WOM_CONFIG_WOM_EN_POS 0x00 +#define WOM_CONFIG_WOM_EN_MASK 0x01 + + + +/* + * FIFO_CONFIG1 + * Register Name : FIFO_CONFIG1 + */ + +/* + * fifo_mode + * FIFO mode control + * + * 0: Stream-to-FIFO Mode + * 1: STOP-on-FULL Mode + */ +#define FIFO_CONFIG1_FIFO_MODE_POS 0x01 +#define FIFO_CONFIG1_FIFO_MODE_MASK (0x01 << FIFO_CONFIG1_FIFO_MODE_POS) + +/* + * fifo_bypass + * FIFO bypass control + * 0: FIFO is not bypassed + * 1: FIFO is bypassed + */ +#define FIFO_CONFIG1_FIFO_BYPASS_POS 0x00 +#define FIFO_CONFIG1_FIFO_BYPASS_MASK 0x01 + + + +/* + * FIFO_CONFIG2 + * Register Name : FIFO_CONFIG2 + */ + +/* + * fifo_wm + * FIFO watermark. Generate interrupt when the FIFO reaches or exceeds FIFO_WM size in bytes or records according to FIFO_COUNT_FORMAT setting. FIFO_WM_EN must be zero before writing this register. Interrupt only fires once. This register should be set to non-zero value, before choosing this interrupt source. + * + * This field should be changed when FIFO is empty to avoid spurious interrupts. + */ +#define FIFO_CONFIG2_FIFO_WM_POS 0x00 +#define FIFO_CONFIG2_FIFO_WM_MASK 0xff + + + +/* + * FIFO_CONFIG3 + * Register Name : FIFO_CONFIG3 + */ + +/* + * fifo_wm + * FIFO watermark. Generate interrupt when the FIFO reaches or exceeds FIFO_WM size in bytes or records according to FIFO_COUNT_FORMAT setting. FIFO_WM_EN must be zero before writing this register. Interrupt only fires once. This register should be set to non-zero value, before choosing this interrupt source. + * + * This field should be changed when FIFO is empty to avoid spurious interrupts. + */ +#define FIFO_CONFIG3_FIFO_WM_POS 0x00 +#define FIFO_CONFIG3_FIFO_WM_MASK 0x0f + + + +/* + * INT_SOURCE0 + * Register Name : INT_SOURCE0 + */ + +/* + * st_int1_en + * 0: Self-Test Done interrupt not routed to INT1 + * 1: Self-Test Done interrupt routed to INT1 + */ +#define INT_SOURCE0_ST_INT1_EN_POS 0x07 +#define INT_SOURCE0_ST_INT1_EN_MASK (0x01 << INT_SOURCE0_ST_INT1_EN_POS) + +/* + * fsync_int1_en + * 0: FSYNC interrupt not routed to INT1 + * 1: FSYNC interrupt routed to INT1 + */ +#define INT_SOURCE0_FSYNC_INT1_EN_POS 0x06 +#define INT_SOURCE0_FSYNC_INT1_EN_MASK (0x01 << INT_SOURCE0_FSYNC_INT1_EN_POS) + +/* + * pll_rdy_int1_en + * 0: PLL ready interrupt not routed to INT1 + * 1: PLL ready interrupt routed to INT1 + */ +#define INT_SOURCE0_PLL_RDY_INT1_EN_POS 0x05 +#define INT_SOURCE0_PLL_RDY_INT1_EN_MASK (0x01 << INT_SOURCE0_PLL_RDY_INT1_EN_POS) + +/* + * reset_done_int1_en + * 0: Reset done interrupt not routed to INT1 + * 1: Reset done interrupt routed to INT1 + */ +#define INT_SOURCE0_RESET_DONE_INT1_EN_POS 0x04 +#define INT_SOURCE0_RESET_DONE_INT1_EN_MASK (0x01 << INT_SOURCE0_RESET_DONE_INT1_EN_POS) + +/* + * drdy_int1_en + * 0: Data Ready interrupt not routed to INT1 + * 1: Data Ready interrupt routed to INT1 + */ +#define INT_SOURCE0_DRDY_INT1_EN_POS 0x03 +#define INT_SOURCE0_DRDY_INT1_EN_MASK (0x01 << INT_SOURCE0_DRDY_INT1_EN_POS) + +/* + * fifo_ths_int1_en + * 0: FIFO threshold interrupt not routed to INT1 + * 1: FIFO threshold interrupt routed to INT1 + */ +#define INT_SOURCE0_FIFO_THS_INT1_EN_POS 0x02 +#define INT_SOURCE0_FIFO_THS_INT1_EN_MASK (0x01 << INT_SOURCE0_FIFO_THS_INT1_EN_POS) + +/* + * fifo_full_int1_en + * 0: FIFO full interrupt not routed to INT1 + * 1: FIFO full interrupt routed to INT1 + * To avoid FIFO FULL interrupts while reading FIFO, this bit should be disabled while reading FIFO + */ +#define INT_SOURCE0_FIFO_FULL_INT1_EN_POS 0x01 +#define INT_SOURCE0_FIFO_FULL_INT1_EN_MASK (0x01 << INT_SOURCE0_FIFO_FULL_INT1_EN_POS) + +/* + * agc_rdy_int1_en + * 0: UI AGC ready interrupt not routed to INT1 + * 1: UI AGC ready interrupt routed to INT1 + */ +#define INT_SOURCE0_AGC_RDY_INT1_EN_POS 0x00 +#define INT_SOURCE0_AGC_RDY_INT1_EN_MASK 0x01 + + + +/* + * INT_SOURCE1 + * Register Name : INT_SOURCE1 + */ + +/* + * i3c_protocol_error_int1_en + * 0: I3CSM protocol error interrupt not routed to INT1 + * 1: I3CSM protocol error interrupt routed to INT1 + */ +#define INT_SOURCE1_I3C_PROTOCOL_ERROR_INT1_EN_POS 0x06 +#define INT_SOURCE1_I3C_PROTOCOL_ERROR_INT1_EN_MASK (0x01 << INT_SOURCE1_I3C_PROTOCOL_ERROR_INT1_EN_POS) + +/* + * smd_int1_en + * 0: SMD interrupt not routed to INT1 + * 1: SMD interrupt routed to INT1 + */ +#define INT_SOURCE1_SMD_INT1_EN_POS 0x03 +#define INT_SOURCE1_SMD_INT1_EN_MASK (0x01 << INT_SOURCE1_SMD_INT1_EN_POS) + +/* + * wom_z_int1_en + * 0: Z-axis WOM interrupt not routed to INT1 + * 1: Z-axis WOM interrupt routed to INT1 + */ +#define INT_SOURCE1_WOM_Z_INT1_EN_POS 0x02 +#define INT_SOURCE1_WOM_Z_INT1_EN_MASK (0x01 << INT_SOURCE1_WOM_Z_INT1_EN_POS) + +/* + * wom_y_int1_en + * 0: Y-axis WOM interrupt not routed to INT1 + * 1: Y-axis WOM interrupt routed to INT1 + */ +#define INT_SOURCE1_WOM_Y_INT1_EN_POS 0x01 +#define INT_SOURCE1_WOM_Y_INT1_EN_MASK (0x01 << INT_SOURCE1_WOM_Y_INT1_EN_POS) + +/* + * wom_x_int1_en + * 0: X-axis WOM interrupt not routed to INT1 + * 1: X-axis WOM interrupt routed to INT1 + */ +#define INT_SOURCE1_WOM_X_INT1_EN_POS 0x00 +#define INT_SOURCE1_WOM_X_INT1_EN_MASK 0x01 + + + +/* + * INT_SOURCE3 + * Register Name : INT_SOURCE3 + */ + +/* + * st_int2_en + * 0: Self-Test Done interrupt not routed to INT2 + * 1: Self-Test Done interrupt routed to INT2 + */ +#define INT_SOURCE3_ST_INT2_EN_POS 0x07 +#define INT_SOURCE3_ST_INT2_EN_MASK (0x01 << INT_SOURCE3_ST_INT2_EN_POS) + +/* + * fsync_int2_en + * 0: FSYNC interrupt not routed to INT2 + * 1: FSYNC interrupt routed to INT2 + */ +#define INT_SOURCE3_FSYNC_INT2_EN_POS 0x06 +#define INT_SOURCE3_FSYNC_INT2_EN_MASK (0x01 << INT_SOURCE3_FSYNC_INT2_EN_POS) + +/* + * pll_rdy_int2_en + * 0: PLL ready interrupt not routed to INT2 + * 1: PLL ready interrupt routed to INT2 + */ +#define INT_SOURCE3_PLL_RDY_INT2_EN_POS 0x05 +#define INT_SOURCE3_PLL_RDY_INT2_EN_MASK (0x01 << INT_SOURCE3_PLL_RDY_INT2_EN_POS) + +/* + * reset_done_int2_en + * 0: Reset done interrupt not routed to INT2 + * 1: Reset done interrupt routed to INT2 + */ +#define INT_SOURCE3_RESET_DONE_INT2_EN_POS 0x04 +#define INT_SOURCE3_RESET_DONE_INT2_EN_MASK (0x01 << INT_SOURCE3_RESET_DONE_INT2_EN_POS) + +/* + * drdy_int2_en + * 0: Data Ready interrupt not routed to INT2 + * 1: Data Ready interrupt routed to INT2 + */ +#define INT_SOURCE3_DRDY_INT2_EN_POS 0x03 +#define INT_SOURCE3_DRDY_INT2_EN_MASK (0x01 << INT_SOURCE3_DRDY_INT2_EN_POS) + +/* + * fifo_ths_int2_en + * 0: FIFO threshold interrupt not routed to INT2 + * 1: FIFO threshold interrupt routed to INT2 + */ +#define INT_SOURCE3_FIFO_THS_INT2_EN_POS 0x02 +#define INT_SOURCE3_FIFO_THS_INT2_EN_MASK (0x01 << INT_SOURCE3_FIFO_THS_INT2_EN_POS) + +/* + * fifo_full_int2_en + * 0: FIFO full interrupt not routed to INT2 + * 1: FIFO full interrupt routed to INT2 + */ +#define INT_SOURCE3_FIFO_FULL_INT2_EN_POS 0x01 +#define INT_SOURCE3_FIFO_FULL_INT2_EN_MASK (0x01 << INT_SOURCE3_FIFO_FULL_INT2_EN_POS) + +/* + * agc_rdy_int2_en + * 0: AGC ready interrupt not routed to INT2 + * 1: AGC ready interrupt routed to INT2 + */ +#define INT_SOURCE3_AGC_RDY_INT2_EN_POS 0x00 +#define INT_SOURCE3_AGC_RDY_INT2_EN_MASK 0x01 + + + +/* + * INT_SOURCE4 + * Register Name : INT_SOURCE4 + */ + +/* + * i3c_protocol_error_int2_en + * 0: I3CSM protocol error interrupt not routed to INT2 + * 1: I3CSM protocol error interrupt routed to INT2 + */ +#define INT_SOURCE4_I3C_PROTOCOL_ERROR_INT2_EN_POS 0x06 +#define INT_SOURCE4_I3C_PROTOCOL_ERROR_INT2_EN_MASK (0x01 << INT_SOURCE4_I3C_PROTOCOL_ERROR_INT2_EN_POS) + +/* + * smd_int2_en + * 0: SMD interrupt not routed to INT2 + * 1: SMD interrupt routed to INT2 + */ +#define INT_SOURCE4_SMD_INT2_EN_POS 0x03 +#define INT_SOURCE4_SMD_INT2_EN_MASK (0x01 << INT_SOURCE4_SMD_INT2_EN_POS) + +/* + * wom_z_int2_en + * 0: Z-axis WOM interrupt not routed to INT2 + * 1: Z-axis WOM interrupt routed to INT2 + */ +#define INT_SOURCE4_WOM_Z_INT2_EN_POS 0x02 +#define INT_SOURCE4_WOM_Z_INT2_EN_MASK (0x01 << INT_SOURCE4_WOM_Z_INT2_EN_POS) + +/* + * wom_y_int2_en + * 0: Y-axis WOM interrupt not routed to INT2 + * 1: Y-axis WOM interrupt routed to INT2 + */ +#define INT_SOURCE4_WOM_Y_INT2_EN_POS 0x01 +#define INT_SOURCE4_WOM_Y_INT2_EN_MASK (0x01 << INT_SOURCE4_WOM_Y_INT2_EN_POS) + +/* + * wom_x_int2_en + * 0: X-axis WOM interrupt not routed to INT2 + * 1: X-axis WOM interrupt routed to INT2 + */ +#define INT_SOURCE4_WOM_X_INT2_EN_POS 0x00 +#define INT_SOURCE4_WOM_X_INT2_EN_MASK 0x01 + + + +/* + * FIFO_LOST_PKT0 + * Register Name : FIFO_LOST_PKT0 + */ + +/* + * fifo_lost_pkt_cnt + * Stores the number of packets lost in the FIFO + */ +#define FIFO_LOST_PKT0_FIFO_LOST_PKT_CNT_POS 0x00 +#define FIFO_LOST_PKT0_FIFO_LOST_PKT_CNT_MASK 0xff + + + +/* + * FIFO_LOST_PKT1 + * Register Name : FIFO_LOST_PKT1 + */ + +/* + * fifo_lost_pkt_cnt + * Stores the number of packets lost in the FIFO + */ +#define FIFO_LOST_PKT1_FIFO_LOST_PKT_CNT_POS 0x00 +#define FIFO_LOST_PKT1_FIFO_LOST_PKT_CNT_MASK 0xff + + + +/* + * APEX_DATA0 + * Register Name : APEX_DATA0 + */ + +/* + * step_cnt + * This status register indicates number of step taken. + */ +#define APEX_DATA0_STEP_CNT_POS 0x00 +#define APEX_DATA0_STEP_CNT_MASK 0xff + + + +/* + * APEX_DATA1 + * Register Name : APEX_DATA1 + */ + +/* + * step_cnt + * This status register indicates number of step taken. + */ +#define APEX_DATA1_STEP_CNT_POS 0x00 +#define APEX_DATA1_STEP_CNT_MASK 0xff + + + +/* + * APEX_DATA2 + * Register Name : APEX_DATA2 + */ + +/* + * step_cadence + * Pedometer step cadence.Walk/run cadency in number of samples. Format is u6.2. + * E.g, At 50Hz and 2Hz walk frequency, the cadency is 25 samples. The register will output 100. + */ +#define APEX_DATA2_STEP_CADENCE_POS 0x00 +#define APEX_DATA2_STEP_CADENCE_MASK 0xff + + + +/* + * APEX_DATA3 + * Register Name : APEX_DATA3 + */ + +/* + * dmp_idle + * 0: Indicates DMP is running + * 1: Indicates DMP is idle + */ +#define APEX_DATA3_DMP_IDLE_POS 0x02 +#define APEX_DATA3_DMP_IDLE_MASK (0x01 << APEX_DATA3_DMP_IDLE_POS) + +/* + * activity_class + * Pedometer Output: Detected activity + * + * 00: Unknown + * 01: Walk + * 10: Run + * 11: Reserved + */ +#define APEX_DATA3_ACTIVITY_CLASS_POS 0x00 +#define APEX_DATA3_ACTIVITY_CLASS_MASK 0x03 + + + +/* + * INTF_CONFIG0 + * Register Name : INTF_CONFIG0 + */ + +/* + * fifo_count_format + * 0: FIFO count is reported in bytes + * 1: FIFO count is reported in records (1 record = 16 bytes for header + gyro + accel + temp sensor data + time stamp, or 8 bytes for header + gyro/accel + temp sensor data) + */ +#define INTF_CONFIG0_FIFO_COUNT_FORMAT_POS 0x06 +#define INTF_CONFIG0_FIFO_COUNT_FORMAT_MASK (0x01 << INTF_CONFIG0_FIFO_COUNT_FORMAT_POS) + +/* + * fifo_count_endian + * This bit applies to both fifo_count and lost_pkt_count + * 0 : Little Endian (The LSByte data is read first, followed by MSByte data). + * 1 : Big Endian (The MSByte data is read first, followed by LSByte data). + */ +#define INTF_CONFIG0_FIFO_COUNT_ENDIAN_POS 0x05 +#define INTF_CONFIG0_FIFO_COUNT_ENDIAN_MASK (0x01 << INTF_CONFIG0_FIFO_COUNT_ENDIAN_POS) + +/* + * sensor_data_endian + * This bit applies to sensor data to AP, and fifo data. + * 0 : Little Endian (The LSByte data is read first, followed by MSByte data). + * 1 : Big Endian (The MSByte data is read first, followed by LSByte data). + */ +#define INTF_CONFIG0_SENSOR_DATA_ENDIAN_POS 0x04 +#define INTF_CONFIG0_SENSOR_DATA_ENDIAN_MASK (0x01 << INTF_CONFIG0_SENSOR_DATA_ENDIAN_POS) + +/* + * INTF_CONFIG1 + * Register Name : INTF_CONFIG1 + */ + +/* + * i3c_sdr_en + * 0: I3CSM SDR mode not enabled + * 1: I3CSM SDR mode enabled + * + * Device will be in pure I2C mode if {I3C_SDR_EN, I3C_DDR_EN} = 00 + */ +#define INTF_CONFIG1_I3C_SDR_EN_POS 0x03 +#define INTF_CONFIG1_I3C_SDR_EN_MASK (0x01 << INTF_CONFIG1_I3C_SDR_EN_POS) + +/* + * i3c_ddr_en + * 0: I3CSM DDR mode not enabled + * 1: I3CSM DDR mode enabled + * + * This bit will not take effect unless I3C_SDR_EN = 1. + */ +#define INTF_CONFIG1_I3C_DDR_EN_POS 0x02 +#define INTF_CONFIG1_I3C_DDR_EN_MASK (0x01 << INTF_CONFIG1_I3C_DDR_EN_POS) + +/* + * clksel + * 00 Alway select internal RC oscillator + * 01 Select PLL when available, else select RC oscillator (default) + * 10 (Reserved) + * 11 Disable all clocks + */ +#define INTF_CONFIG1_CLKSEL_POS 0x00 +#define INTF_CONFIG1_CLKSEL_MASK 0x03 + + + +/* + * INT_STATUS_DRDY + * Register Name : INT_STATUS_DRDY + */ + +/* + * data_rdy_int + * This bit automatically sets to 1 when a Data Ready interrupt is generated. The bit clears to 0 after the register has been read. + */ +#define INT_STATUS_DRDY_DATA_RDY_INT_POS 0x00 +#define INT_STATUS_DRDY_DATA_RDY_INT_MASK 0x01 + + + +/* + * INT_STATUS + * Register Name : INT_STATUS + */ + +/* + * st_int + * This bit automatically sets to 1 when a Self Test done interrupt is generated. The bit clears to 0 after the register has been read. + */ +#define INT_STATUS_ST_INT_POS 0x07 +#define INT_STATUS_ST_INT_MASK (0x01 << INT_STATUS_ST_INT_POS) + +/* + * fsync_int + * This bit automatically sets to 1 when an FSYNC interrupt is generated. The bit clears to 0 after the register has been read. + */ +#define INT_STATUS_FSYNC_INT_POS 0x06 +#define INT_STATUS_FSYNC_INT_MASK (0x01 << INT_STATUS_FSYNC_INT_POS) + +/* + * pll_rdy_int + * This bit automatically sets to 1 when a PLL Ready interrupt is generated. The bit clears to 0 after the register has been read. + */ +#define INT_STATUS_PLL_RDY_INT_POS 0x05 +#define INT_STATUS_PLL_RDY_INT_MASK (0x01 << INT_STATUS_PLL_RDY_INT_POS) + +/* + * reset_done_int + * This bit automatically sets to 1 when software reset is complete. The bit clears to 0 after the register has been read. + */ +#define INT_STATUS_RESET_DONE_INT_POS 0x04 +#define INT_STATUS_RESET_DONE_INT_MASK (0x01 << INT_STATUS_RESET_DONE_INT_POS) + +/* + * fifo_ths_int + * This bit automatically sets to 1 when the FIFO buffer reaches the threshold value. The bit clears to 0 after the register has been read. + */ +#define INT_STATUS_FIFO_THS_INT_POS 0x02 +#define INT_STATUS_FIFO_THS_INT_MASK (0x01 << INT_STATUS_FIFO_THS_INT_POS) + +/* + * fifo_full_int + * This bit automatically sets to 1 when the FIFO buffer is full. The bit clears to 0 after the register has been read. + */ +#define INT_STATUS_FIFO_FULL_INT_POS 0x01 +#define INT_STATUS_FIFO_FULL_INT_MASK (0x01 << INT_STATUS_FIFO_FULL_INT_POS) + +/* + * agc_rdy_int + * This bit automatically sets to 1 when an AGC Ready interrupt is generated. The bit clears to 0 after the register has been read. + */ +#define INT_STATUS_AGC_RDY_INT_POS 0x00 +#define INT_STATUS_AGC_RDY_INT_MASK 0x01 + + + +/* + * INT_STATUS2 + * Register Name : INT_STATUS2 + */ + +/* + * smd_int + * Significant Motion Detection Interrupt, clears on read + */ +#define INT_STATUS2_SMD_INT_POS 0x03 +#define INT_STATUS2_SMD_INT_MASK (0x01 << INT_STATUS2_SMD_INT_POS) + +/* + * wom_x_int + * Wake on Motion Interrupt on X-axis, clears on read + */ +#define INT_STATUS2_WOM_X_INT_POS 0x02 +#define INT_STATUS2_WOM_X_INT_MASK (0x01 << INT_STATUS2_WOM_X_INT_POS) + +/* + * wom_y_int + * Wake on Motion Interrupt on Y-axis, clears on read + */ +#define INT_STATUS2_WOM_Y_INT_POS 0x01 +#define INT_STATUS2_WOM_Y_INT_MASK (0x01 << INT_STATUS2_WOM_Y_INT_POS) + +/* + * wom_z_int + * Wake on Motion Interrupt on Z-axis, clears on read + */ +#define INT_STATUS2_WOM_Z_INT_POS 0x00 +#define INT_STATUS2_WOM_Z_INT_MASK 0x01 + + + +/* + * INT_STATUS3 + * Register Name : INT_STATUS3 + */ + +/* + * step_det_int + * Step Detection Interrupt, clears on read + */ +#define INT_STATUS3_STEP_DET_INT_POS 0x05 +#define INT_STATUS3_STEP_DET_INT_MASK (0x01 << INT_STATUS3_STEP_DET_INT_POS) + +/* + * step_cnt_ovf_int + * Step Count Overflow Interrupt, clears on read + */ +#define INT_STATUS3_STEP_CNT_OVF_INT_POS 0x04 +#define INT_STATUS3_STEP_CNT_OVF_INT_MASK (0x01 << INT_STATUS3_STEP_CNT_OVF_INT_POS) + +/* + * tilt_det_int + * Tilt Detection Interrupt, clears on read + */ +#define INT_STATUS3_TILT_DET_INT_POS 0x03 +#define INT_STATUS3_TILT_DET_INT_MASK (0x01 << INT_STATUS3_TILT_DET_INT_POS) + +/* + * ff_det_int + * Freefall Interrupt, clears on read + */ +#define INT_STATUS3_FF_DET_INT_POS 0x02 +#define INT_STATUS3_FF_DET_INT_MASK (0x01 << INT_STATUS3_FF_DET_INT_POS) + +/* + * lowg_det_int + * LowG Interrupt, clears on read + */ +#define INT_STATUS3_LOWG_DET_INT_POS 0x01 +#define INT_STATUS3_LOWG_DET_INT_MASK (0x01 << INT_STATUS3_LOWG_DET_INT_POS) + + + +/* + * FIFO_COUNTH + * Register Name : FIFO_COUNTH + */ + +/* + * fifo_count + * Number of bytes in FIFO when fifo_count_format=0. + * Number of records in FIFO when fifo_count_format=1. + */ +#define FIFO_COUNTH_FIFO_COUNT_POS 0x00 +#define FIFO_COUNTH_FIFO_COUNT_MASK 0xff + + + +/* + * FIFO_COUNTL + * Register Name : FIFO_COUNTL + */ + +/* + * fifo_count + * Number of bytes in FIFO when fifo_count_format=0. + * Number of records in FIFO when fifo_count_format=1. + */ +#define FIFO_COUNTL_FIFO_COUNT_POS 0x00 +#define FIFO_COUNTL_FIFO_COUNT_MASK 0xff + + + +/* + * FIFO_DATA + * Register Name : FIFO_DATA + */ + +/* + * fifo_data + * FIFO data port + */ +#define FIFO_DATA_FIFO_DATA_POS 0x00 +#define FIFO_DATA_FIFO_DATA_MASK 0xff + + + +/* + * WHO_AM_I + * Register Name : WHO_AM_I + */ + +/* + * whoami + * Register to indicate to user which device is being accessed + */ +#define WHO_AM_I_WHOAMI_POS 0x00 +#define WHO_AM_I_WHOAMI_MASK 0xff + + + +/* + * BLK_SEL_W + * Register Name : BLK_SEL_W + */ + +/* + * blk_sel_w + * For write operation, select a 256-byte MCLK space, or 128-byte SCLK space. + * Automatically reset when OTP copy operation is triggered. + */ +#define BLK_SEL_W_BLK_SEL_W_POS 0x00 +#define BLK_SEL_W_BLK_SEL_W_MASK 0xff + + + +/* + * MADDR_W + * Register Name : MADDR_W + */ + +/* + * maddr_w + * For MREG write operation, the lower 8-bit address for accessing MCLK domain registers. + */ +#define MADDR_W_MADDR_W_POS 0x00 +#define MADDR_W_MADDR_W_MASK 0xff + + + +/* + * M_W + * Register Name : M_W + */ + +/* + * m_w + * For MREG write operation, the write port for accessing MCLK domain registers. + */ +#define M_W_M_W_POS 0x00 +#define M_W_M_W_MASK 0xff + + + +/* + * BLK_SEL_R + * Register Name : BLK_SEL_R + */ + +/* + * blk_sel_r + * For read operation, select a 256-byte MCLK space, or 128-byte SCLK space. + * Automatically reset when OTP copy operation is triggered. + */ +#define BLK_SEL_R_BLK_SEL_R_POS 0x00 +#define BLK_SEL_R_BLK_SEL_R_MASK 0xff + + + +/* + * MADDR_R + * Register Name : MADDR_R + */ + +/* + * maddr_r + * For MREG read operation, the lower 8-bit address for accessing MCLK domain registers. + */ +#define MADDR_R_MADDR_R_POS 0x00 +#define MADDR_R_MADDR_R_MASK 0xff + + + +/* + * M_R + * Register Name : M_R + */ + +/* + * m_r + * For MREG read operation, the read port for accessing MCLK domain registers. + */ +#define M_R_M_R_POS 0x00 +#define M_R_M_R_MASK 0xff + + +/* --------------------------------------------------------------------------- + * register MREG1 + * ---------------------------------------------------------------------------*/ + +/* + * TMST_CONFIG1 + * Register Name : TMST_CONFIG1 + */ + +/* + * tmst_res + * Time Stamp resolution; When set to 0 (default), time stamp resolution is 1 us. When set to 1, resolution is 16us + */ +#define TMST_CONFIG1_TMST_RES_POS 0x03 +#define TMST_CONFIG1_TMST_RES_MASK (0x01 << TMST_CONFIG1_TMST_RES_POS) + +/* + * tmst_delta_en + * Time Stamp delta Enable : When set to 1, the Time stamp field contains the measurement of time since the last occurrence of ODR. + */ +#define TMST_CONFIG1_TMST_DELTA_EN_POS 0x02 +#define TMST_CONFIG1_TMST_DELTA_EN_MASK (0x01 << TMST_CONFIG1_TMST_DELTA_EN_POS) + +/* + * tmst_fsync_en + * Time Stamp register Fsync Enable . When set to 1, the contents of the Timestamp feature of FSYNC is enabled. The user also needs to select fifo_tmst_fsync_en in order to propagate the timestamp value to the FIFO + */ +#define TMST_CONFIG1_TMST_FSYNC_EN_POS 0x01 +#define TMST_CONFIG1_TMST_FSYNC_EN_MASK (0x01 << TMST_CONFIG1_TMST_FSYNC_EN_POS) + +/* + * tmst_en + * Time Stamp register Enable + */ +#define TMST_CONFIG1_TMST_EN_POS 0x00 +#define TMST_CONFIG1_TMST_EN_MASK 0x01 + + + +/* + * FIFO_CONFIG5 + * Register Name : FIFO_CONFIG5 + */ + +/* + * fifo_wm_gt_th + * 1: trigger FIFO-Watermark interrupt on every ODR(DMA Write) if FIFO_COUNT: =FIFO_WM + * + * 0: Trigger FIFO-Watermark interrupt when FIFO_COUNT == FIFO_WM + */ +#define FIFO_CONFIG5_FIFO_WM_GT_TH_POS 0x05 +#define FIFO_CONFIG5_FIFO_WM_GT_TH_MASK (0x01 << FIFO_CONFIG5_FIFO_WM_GT_TH_POS) + +/* + * fifo_resume_partial_rd + * 0: FIFO is read in packets. If a partial packet is read, then the subsequent read will start from the beginning of the un-read packet. + * 1: FIFO can be read partially. When read is resumed, FIFO bytes will continue from last read point. The SW driver is responsible for cascading previous read and present read and maintain frame boundaries. + */ +#define FIFO_CONFIG5_FIFO_RESUME_PARTIAL_RD_POS 0x04 +#define FIFO_CONFIG5_FIFO_RESUME_PARTIAL_RD_MASK (0x01 << FIFO_CONFIG5_FIFO_RESUME_PARTIAL_RD_POS) + +/* + * fifo_hires_en + * Allows 20 bit resolution in the FIFO packet readout + */ +#define FIFO_CONFIG5_FIFO_HIRES_EN_POS 0x03 +#define FIFO_CONFIG5_FIFO_HIRES_EN_MASK (0x01 << FIFO_CONFIG5_FIFO_HIRES_EN_POS) + +/* + * fifo_tmst_fsync_en + * Allows the TMST in the FIFO to be replaced by the FSYNC timestamp + */ +#define FIFO_CONFIG5_FIFO_TMST_FSYNC_EN_POS 0x02 +#define FIFO_CONFIG5_FIFO_TMST_FSYNC_EN_MASK (0x01 << FIFO_CONFIG5_FIFO_TMST_FSYNC_EN_POS) + +/* + * fifo_gyro_en + * Enables Gyro Packets to go to FIFO + */ +#define FIFO_CONFIG5_FIFO_GYRO_EN_POS 0x01 +#define FIFO_CONFIG5_FIFO_GYRO_EN_MASK (0x01 << FIFO_CONFIG5_FIFO_GYRO_EN_POS) + +/* + * fifo_accel_en + * Enable Accel Packets to go to FIFO + */ +#define FIFO_CONFIG5_FIFO_ACCEL_EN_POS 0x00 +#define FIFO_CONFIG5_FIFO_ACCEL_EN_MASK 0x01 + + + +/* + * FIFO_CONFIG6 + * Register Name : FIFO_CONFIG6 + */ + +/* + * fifo_empty_indicator_dis + * 0: xFF is sent out as FIFO data when FIFO is empty. + * 1: The last FIFO data is sent out when FIFO is empty. + */ +#define FIFO_CONFIG6_FIFO_EMPTY_INDICATOR_DIS_POS 0x04 +#define FIFO_CONFIG6_FIFO_EMPTY_INDICATOR_DIS_MASK (0x01 << FIFO_CONFIG6_FIFO_EMPTY_INDICATOR_DIS_POS) + +/* + * rcosc_req_on_fifo_ths_dis + * 0: When the FIFO is operating in ALP+WUOSC mode and the watermark (WM) interrupt is enabled, the FIFO wakes up the system oscillator (RCOSC) as soon as the watermark level is reached. The system oscillator remains enabled until a Host FIFO read operation happens. This will temporarily cause a small increase in the power consumption due to the enabling of the system oscillator. + * 1: The system oscillator is not automatically woken-up by the FIFO/INT when the WM interrupt is triggered. The side effect is that the host can receive invalid packets until the system oscillator is off after it has been turned on for other reasons not related to a WM interrupt. + * + * The recommended setting of this bit is ‘1’ before entering and during all power modes excluding ALP with WUOSC. This is in order to avoid having to do a FIFO access/flush before entering sleep mode. During ALP with WUOSC it is recommended to set this bit to ‘0’. It is recommended to reset this bit back to ‘1’ before exiting ALP+WUOSC with a wait time of 1 ODR or higher. + */ +#define FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_POS 0x00 +#define FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS_MASK 0x01 + + + +/* + * FSYNC_CONFIG + * Register Name : FSYNC_CONFIG + */ + +/* + * fsync_ui_sel + * this register was called (ext_sync_sel) + * 0 Do not tag Fsync flag + * 1 Tag Fsync flag to TEMP_OUT’s LSB + * 2 Tag Fsync flag to GYRO_XOUT’s LSB + * 3 Tag Fsync flag to GYRO_YOUT’s LSB + * 4 Tag Fsync flag to GYRO_ZOUT’s LSB + * 5 Tag Fsync flag to ACCEL_XOUT’s LSB + * 6 Tag Fsync flag to ACCEL_YOUT’s LSB + * 7 Tag Fsync flag to ACCEL_ZOUT’s LSB + */ +#define FSYNC_CONFIG_FSYNC_UI_SEL_POS 0x04 +#define FSYNC_CONFIG_FSYNC_UI_SEL_MASK (0x07 << FSYNC_CONFIG_FSYNC_UI_SEL_POS) + +/* + * fsync_ui_flag_clear_sel + * 0 means the FSYNC flag is cleared when UI sensor reg is updated + * 1 means the FSYNC flag is cleared when UI interface reads the sensor register LSB of FSYNC tagged axis + */ +#define FSYNC_CONFIG_FSYNC_UI_FLAG_CLEAR_SEL_POS 0x01 +#define FSYNC_CONFIG_FSYNC_UI_FLAG_CLEAR_SEL_MASK (0x01 << FSYNC_CONFIG_FSYNC_UI_FLAG_CLEAR_SEL_POS) + +/* + * fsync_polarity + * 0: Start from Rising edge of FSYNC pulse to measure FSYNC interval + * 1: Start from Falling edge of FSYNC pulse to measure FSYNC interval + */ +#define FSYNC_CONFIG_FSYNC_POLARITY_POS 0x00 +#define FSYNC_CONFIG_FSYNC_POLARITY_MASK 0x01 + + + +/* + * INT_CONFIG0 + * Register Name : INT_CONFIG0 + */ + +/* + * ui_drdy_int_clear + * Data Ready Interrupt Clear Option (latched mode) + * 00: Clear on Status Bit Read + * 01: Clear on Status Bit Read + * 10: Clear on Sensor Register Read + * 11: Clear on Status Bit Read OR on Sensor Register read + */ +#define INT_CONFIG0_UI_DRDY_INT_CLEAR_POS 0x04 +#define INT_CONFIG0_UI_DRDY_INT_CLEAR_MASK (0x03 << INT_CONFIG0_UI_DRDY_INT_CLEAR_POS) + +/* + * fifo_ths_int_clear + * FIFO Threshold Interrupt Clear Option (latched mode) + * 00: Clear on Status Bit Read + * 01: Clear on Status Bit Read + * 10: Clear on FIFO data 1Byte Read + * 11: Clear on Status Bit Read OR on FIFO data 1 byte read + */ +#define INT_CONFIG0_FIFO_THS_INT_CLEAR_POS 0x02 +#define INT_CONFIG0_FIFO_THS_INT_CLEAR_MASK (0x03 << INT_CONFIG0_FIFO_THS_INT_CLEAR_POS) + +/* + * fifo_full_int_clear + * FIFO Full Interrupt Clear Option (latched mode) + * 00: Clear on Status Bit Read + * 01: Clear on Status Bit Read + * 10: Clear on FIFO data 1Byte Read + * 11: Clear on Status Bit Read OR on FIFO data 1 byte read + */ +#define INT_CONFIG0_FIFO_FULL_INT_CLEAR_POS 0x00 +#define INT_CONFIG0_FIFO_FULL_INT_CLEAR_MASK 0x03 + + + +/* + * INT_CONFIG1 + * Register Name : INT_CONFIG1 + */ + +/* + * int_tpulse_duration + * 0 - (Default) Interrupt pulse duration is 100us + * 1- Interrupt pulse duration is 8 us + */ +#define INT_CONFIG1_INT_TPULSE_DURATION_POS 0x06 +#define INT_CONFIG1_INT_TPULSE_DURATION_MASK (0x01 << INT_CONFIG1_INT_TPULSE_DURATION_POS) + +/* + * int_async_reset + * 0: The interrupt pulse is reset as soon as the interrupt status register is read if the pulse is still active. + * 1: The interrupt pulse remains high for the intended duration independent of when the interrupt status register is read. This is the default and recommended setting. In this case, when in ALP with the WUOSC clock, the clearing of the interrupt status register requires up to one ODR period after reading. + */ +#define INT_CONFIG1_INT_ASYNC_RESET_POS 0x04 +#define INT_CONFIG1_INT_ASYNC_RESET_MASK (0x01 << INT_CONFIG1_INT_ASYNC_RESET_POS) + + + +/* + * SENSOR_CONFIG3 + * Register Name : SENSOR_CONFIG3 + */ + +/* + * apex_disable + * 1: Disable APEX features to extend FIFO size to 2.25 Kbytes + */ +#define SENSOR_CONFIG3_APEX_DISABLE_POS 0x06 +#define SENSOR_CONFIG3_APEX_DISABLE_MASK (0x01 << SENSOR_CONFIG3_APEX_DISABLE_POS) + +/* + * ST_CONFIG + * Register Name : ST_CONFIG + */ + +/* + * accel_st_reg + * User must set this bit to 1 when enabling accelerometer self-test and clear it to 0 when self-test procedure has completed. + */ +#define ST_CONFIG_ACCEL_ST_REG_POS 0x07 +#define ST_CONFIG_ACCEL_ST_REG_MASK (0x01 << ST_CONFIG_ACCEL_ST_REG_POS) + +/* + * st_number_sample + * This bit selects the number of sensor samples that should be used to process self-test + * 0: 16 samples + * 1: 200 samples + */ +#define ST_CONFIG_ST_NUMBER_SAMPLE_POS 0x06 +#define ST_CONFIG_ST_NUMBER_SAMPLE_MASK (0x01 << ST_CONFIG_ST_NUMBER_SAMPLE_POS) + +/* + * accel_st_lim + * These bits control the tolerated ratio between self-test processed values and reference (fused) ones for accelerometer. + * 0 : 5% + * 1: 10% + * 2: 15% + * 3: 20% + * 4: 25% + * 5: 30% + * 6: 40% + * 7: 50% + */ +#define ST_CONFIG_ACCEL_ST_LIM_POS 0x03 +#define ST_CONFIG_ACCEL_ST_LIM_MASK (0x07 << ST_CONFIG_ACCEL_ST_LIM_POS) + +/* + * gyro_st_lim + * These bits control the tolerated ratio between self-test processed values and reference (fused) ones for gyro. + * 0 : 5% + * 1: 10% + * 2: 15% + * 3: 20% + * 4: 25% + * 5: 30% + * 6: 40% + * 7: 50% + */ +#define ST_CONFIG_GYRO_ST_LIM_POS 0x00 +#define ST_CONFIG_GYRO_ST_LIM_MASK 0x07 + + + +/* + * SELFTEST + * Register Name : SELFTEST + */ + +/* + * gyro_st_en + * 1: enable gyro self test operation. Host needs to program this bit to 0 to move chip out of self test mode. If host programs this bit to 0 while st_busy = 1 and st_done =0, the current running self-test operation is terminated by host. + */ +#define SELFTEST_GYRO_ST_EN_POS 0x07 +#define SELFTEST_GYRO_ST_EN_MASK (0x01 << SELFTEST_GYRO_ST_EN_POS) + +/* + * accel_st_en + * 1: enable accel self test operation. Host needs to program this bit to 0 to move chip out of self test mode. If host programs this bit to 0 while st_busy = 1 and st_done =0, the current running self-test operation is terminated by host. + */ +#define SELFTEST_ACCEL_ST_EN_POS 0x06 +#define SELFTEST_ACCEL_ST_EN_MASK (0x01 << SELFTEST_ACCEL_ST_EN_POS) + +/* + * en_gz_st + * Enable Gyro Z-axis self test + */ +#define SELFTEST_EN_GZ_ST_POS 0x05 +#define SELFTEST_EN_GZ_ST_MASK (0x01 << SELFTEST_EN_GZ_ST_POS) + +/* + * en_gy_st + * Enable Gyro Y-axis self test + */ +#define SELFTEST_EN_GY_ST_POS 0x04 +#define SELFTEST_EN_GY_ST_MASK (0x01 << SELFTEST_EN_GY_ST_POS) + +/* + * en_gx_st + * Enable Gyro X-axis self test + */ +#define SELFTEST_EN_GX_ST_POS 0x03 +#define SELFTEST_EN_GX_ST_MASK (0x01 << SELFTEST_EN_GX_ST_POS) + +/* + * en_az_st + * Enable Accel Z-axis self test + */ +#define SELFTEST_EN_AZ_ST_POS 0x02 +#define SELFTEST_EN_AZ_ST_MASK (0x01 << SELFTEST_EN_AZ_ST_POS) + +/* + * en_ay_st + * Enable Accel Y-axis self test + */ +#define SELFTEST_EN_AY_ST_POS 0x01 +#define SELFTEST_EN_AY_ST_MASK (0x01 << SELFTEST_EN_AY_ST_POS) + +/* + * en_ax_st + * Enable Accel X-axis self test + */ +#define SELFTEST_EN_AX_ST_POS 0x00 +#define SELFTEST_EN_AX_ST_MASK 0x01 + + + +/* + * INTF_CONFIG6 + * Register Name : INTF_CONFIG6 + */ + +/* + * i3c_timeout_en + * Value of 1 to enable i2c/i3c timeout function + */ +#define INTF_CONFIG6_I3C_TIMEOUT_EN_POS 0x04 +#define INTF_CONFIG6_I3C_TIMEOUT_EN_MASK (0x01 << INTF_CONFIG6_I3C_TIMEOUT_EN_POS) + +/* + * i3c_ibi_byte_en + * I3C Enable IBI-payload function. + */ +#define INTF_CONFIG6_I3C_IBI_BYTE_EN_POS 0x03 +#define INTF_CONFIG6_I3C_IBI_BYTE_EN_MASK (0x01 << INTF_CONFIG6_I3C_IBI_BYTE_EN_POS) + +/* + * i3c_ibi_en + * I3C Enable IBI function. + */ +#define INTF_CONFIG6_I3C_IBI_EN_POS 0x02 +#define INTF_CONFIG6_I3C_IBI_EN_MASK (0x01 << INTF_CONFIG6_I3C_IBI_EN_POS) + + + +/* + * INTF_CONFIG10 + * Register Name : INTF_CONFIG10 + */ + +/* + * asynctime0_dis + * 1: Disable asynchronous timing control mode 0 operation. + */ +#define INTF_CONFIG10_ASYNCTIME0_DIS_POS 0x07 +#define INTF_CONFIG10_ASYNCTIME0_DIS_MASK (0x01 << INTF_CONFIG10_ASYNCTIME0_DIS_POS) + +/* + * INTF_CONFIG7 + * Register Name : INTF_CONFIG7 + */ + +/* + * i3c_ddr_wr_mode + * This bit controls how I3C slave treats the 1st 2-byte data from + * host in a DDR write operation. + * + * 0: (a) The 1st-byte in DDR-WR configures the starting register + * address where the write operation should occur. + * (b) The 2nd-byte in DDR-WR is ignored and dropped. + * (c) The 3rd-byte in DDR-WR will be written into the register + * with address specified by the 1st-byte. + * Or, the next DDR-RD will be starting from the address + * specified by the 1st-byte of previous DDR-WR. + * + * 1: (a) The 1st-byte in DDR-WR configures the starting register + * address where the write operation should occur. + * (b) The 2nd-byte in DDR-WR will be written into the register + * with address specified by the 1st-byte. + */ +#define INTF_CONFIG7_I3C_DDR_WR_MODE_POS 0x03 +#define INTF_CONFIG7_I3C_DDR_WR_MODE_MASK (0x01 << INTF_CONFIG7_I3C_DDR_WR_MODE_POS) + +/* + * OTP_CONFIG + * Register Name : OTP_CONFIG + */ + +/* + * otp_copy_mode + * 00: Reserved + * 01: Enable copying OTP block to SRAM + * 10: Reserved + * 11: Enable copying self-test data from OTP memory to SRAM + */ +#define OTP_CONFIG_OTP_COPY_MODE_POS 0x02 +#define OTP_CONFIG_OTP_COPY_MODE_MASK (0x03 << OTP_CONFIG_OTP_COPY_MODE_POS) + +/* + * INT_SOURCE6 + * Register Name : INT_SOURCE6 + */ + +/* + * ff_int1_en + * 0: Freefall interrupt not routed to INT1 + * 1: Freefall interrupt routed to INT1 + */ +#define INT_SOURCE6_FF_INT1_EN_POS 0x07 +#define INT_SOURCE6_FF_INT1_EN_MASK (0x01 << INT_SOURCE6_FF_INT1_EN_POS) + +/* + * lowg_int1_en + * 0: Low-g interrupt not routed to INT1 + * 1: Low-g interrupt routed to INT1 + */ +#define INT_SOURCE6_LOWG_INT1_EN_POS 0x06 +#define INT_SOURCE6_LOWG_INT1_EN_MASK (0x01 << INT_SOURCE6_LOWG_INT1_EN_POS) + +/* + * step_det_int1_en + * 0: Step detect interrupt not routed to INT1 + * 1: Step detect interrupt routed to INT1 + */ +#define INT_SOURCE6_STEP_DET_INT1_EN_POS 0x05 +#define INT_SOURCE6_STEP_DET_INT1_EN_MASK (0x01 << INT_SOURCE6_STEP_DET_INT1_EN_POS) + +/* + * step_cnt_ofl_int1_en + * 0: Step count overflow interrupt not routed to INT1 + * 1: Step count overflow interrupt routed to INT1 + */ +#define INT_SOURCE6_STEP_CNT_OFL_INT1_EN_POS 0x04 +#define INT_SOURCE6_STEP_CNT_OFL_INT1_EN_MASK (0x01 << INT_SOURCE6_STEP_CNT_OFL_INT1_EN_POS) + +/* + * tilt_det_int1_en + * 0: Tilt detect interrupt not routed to INT1 + * 1: Tile detect interrupt routed to INT1 + */ +#define INT_SOURCE6_TILT_DET_INT1_EN_POS 0x03 +#define INT_SOURCE6_TILT_DET_INT1_EN_MASK (0x01 << INT_SOURCE6_TILT_DET_INT1_EN_POS) + + + +/* + * INT_SOURCE7 + * Register Name : INT_SOURCE7 + */ + +/* + * ff_int2_en + * 0: Freefall interrupt not routed to INT2 + * 1: Freefall interrupt routed to INT2 + */ +#define INT_SOURCE7_FF_INT2_EN_POS 0x07 +#define INT_SOURCE7_FF_INT2_EN_MASK (0x01 << INT_SOURCE7_FF_INT2_EN_POS) + +/* + * lowg_int2_en + * 0: Low-g interrupt not routed to INT2 + * 1: Low-g interrupt routed to INT2 + */ +#define INT_SOURCE7_LOWG_INT2_EN_POS 0x06 +#define INT_SOURCE7_LOWG_INT2_EN_MASK (0x01 << INT_SOURCE7_LOWG_INT2_EN_POS) + +/* + * step_det_int2_en + * 0: Step detect interrupt not routed to INT2 + * 1: Step detect interrupt routed to INT2 + */ +#define INT_SOURCE7_STEP_DET_INT2_EN_POS 0x05 +#define INT_SOURCE7_STEP_DET_INT2_EN_MASK (0x01 << INT_SOURCE7_STEP_DET_INT2_EN_POS) + +/* + * step_cnt_ofl_int2_en + * 0: Step count overflow interrupt not routed to INT2 + * 1: Step count overflow interrupt routed to INT2 + */ +#define INT_SOURCE7_STEP_CNT_OFL_INT2_EN_POS 0x04 +#define INT_SOURCE7_STEP_CNT_OFL_INT2_EN_MASK (0x01 << INT_SOURCE7_STEP_CNT_OFL_INT2_EN_POS) + +/* + * tilt_det_int2_en + * 0: Tilt detect interrupt not routed to INT2 + * 1: Tile detect interrupt routed to INT2 + */ +#define INT_SOURCE7_TILT_DET_INT2_EN_POS 0x03 +#define INT_SOURCE7_TILT_DET_INT2_EN_MASK (0x01 << INT_SOURCE7_TILT_DET_INT2_EN_POS) + + + +/* + * INT_SOURCE8 + * Register Name : INT_SOURCE8 + */ + +/* + * fsync_ibi_en + * 0: FSYNC interrupt not routed to IBI + * 1: FSYNC interrupt routed to IBI + */ +#define INT_SOURCE8_FSYNC_IBI_EN_POS 0x05 +#define INT_SOURCE8_FSYNC_IBI_EN_MASK (0x01 << INT_SOURCE8_FSYNC_IBI_EN_POS) + +/* + * pll_rdy_ibi_en + * 0: PLL ready interrupt not routed to IBI + * 1: PLL ready interrupt routed to IBI + */ +#define INT_SOURCE8_PLL_RDY_IBI_EN_POS 0x04 +#define INT_SOURCE8_PLL_RDY_IBI_EN_MASK (0x01 << INT_SOURCE8_PLL_RDY_IBI_EN_POS) + +/* + * ui_drdy_ibi_en + * 0: UI data ready interrupt not routed to IBI + * 1: UI data ready interrupt routed to IBI + */ +#define INT_SOURCE8_UI_DRDY_IBI_EN_POS 0x03 +#define INT_SOURCE8_UI_DRDY_IBI_EN_MASK (0x01 << INT_SOURCE8_UI_DRDY_IBI_EN_POS) + +/* + * fifo_ths_ibi_en + * 0: FIFO threshold interrupt not routed to IBI + * 1: FIFO threshold interrupt routed to IBI + */ +#define INT_SOURCE8_FIFO_THS_IBI_EN_POS 0x02 +#define INT_SOURCE8_FIFO_THS_IBI_EN_MASK (0x01 << INT_SOURCE8_FIFO_THS_IBI_EN_POS) + +/* + * fifo_full_ibi_en + * 0: FIFO full interrupt not routed to IBI + * 1: FIFO full interrupt routed to IBI + */ +#define INT_SOURCE8_FIFO_FULL_IBI_EN_POS 0x01 +#define INT_SOURCE8_FIFO_FULL_IBI_EN_MASK (0x01 << INT_SOURCE8_FIFO_FULL_IBI_EN_POS) + +/* + * agc_rdy_ibi_en + * 0: AGC ready interrupt not routed to IBI + * 1: AGC ready interrupt routed to IBI + */ +#define INT_SOURCE8_AGC_RDY_IBI_EN_POS 0x00 +#define INT_SOURCE8_AGC_RDY_IBI_EN_MASK 0x01 + + + +/* + * INT_SOURCE9 + * Register Name : INT_SOURCE9 + */ + +/* + * i3c_protocol_error_ibi_en + * 0: I3CSM protocol error interrupt not routed to IBI + * 1: I3CSM protocol error interrupt routed to IBI + */ +#define INT_SOURCE9_I3C_PROTOCOL_ERROR_IBI_EN_POS 0x07 +#define INT_SOURCE9_I3C_PROTOCOL_ERROR_IBI_EN_MASK (0x01 << INT_SOURCE9_I3C_PROTOCOL_ERROR_IBI_EN_POS) + +/* + * ff_ibi_en + * 0: Freefall interrupt not routed to IBI + * 1: Freefall interrupt routed to IBI + */ +#define INT_SOURCE9_FF_IBI_EN_POS 0x06 +#define INT_SOURCE9_FF_IBI_EN_MASK (0x01 << INT_SOURCE9_FF_IBI_EN_POS) + +/* + * lowg_ibi_en + * 0: Low-g interrupt not routed to IBI + * 1: Low-g interrupt routed to IBI + */ +#define INT_SOURCE9_LOWG_IBI_EN_POS 0x05 +#define INT_SOURCE9_LOWG_IBI_EN_MASK (0x01 << INT_SOURCE9_LOWG_IBI_EN_POS) + +/* + * smd_ibi_en + * 0: SMD interrupt not routed to IBI + * 1: SMD interrupt routed to IBI + */ +#define INT_SOURCE9_SMD_IBI_EN_POS 0x04 +#define INT_SOURCE9_SMD_IBI_EN_MASK (0x01 << INT_SOURCE9_SMD_IBI_EN_POS) + +/* + * wom_z_ibi_en + * 0: Z-axis WOM interrupt not routed to IBI + * 1: Z-axis WOM interrupt routed to IBI + */ +#define INT_SOURCE9_WOM_Z_IBI_EN_POS 0x03 +#define INT_SOURCE9_WOM_Z_IBI_EN_MASK (0x01 << INT_SOURCE9_WOM_Z_IBI_EN_POS) + +/* + * wom_y_ibi_en + * 0: Y-axis WOM interrupt not routed to IBI + * 1: Y-axis WOM interrupt routed to IBI + */ +#define INT_SOURCE9_WOM_Y_IBI_EN_POS 0x02 +#define INT_SOURCE9_WOM_Y_IBI_EN_MASK (0x01 << INT_SOURCE9_WOM_Y_IBI_EN_POS) + +/* + * wom_x_ibi_en + * 0: X-axis WOM interrupt not routed to IBI + * 1: X-axis WOM interrupt routed to IBI + */ +#define INT_SOURCE9_WOM_X_IBI_EN_POS 0x01 +#define INT_SOURCE9_WOM_X_IBI_EN_MASK (0x01 << INT_SOURCE9_WOM_X_IBI_EN_POS) + +/* + * st_done_ibi_en + * 0: Self-test done interrupt not routed to IBI + * 1: Self-test done interrupt routed to IBI + */ +#define INT_SOURCE9_ST_DONE_IBI_EN_POS 0x00 +#define INT_SOURCE9_ST_DONE_IBI_EN_MASK 0x01 + + + +/* + * INT_SOURCE10 + * Register Name : INT_SOURCE10 + */ + +/* + * step_det_ibi_en + * 0: Step detect interrupt not routed to IBI + * 1: Step detect interrupt routed to IBI + */ +#define INT_SOURCE10_STEP_DET_IBI_EN_POS 0x05 +#define INT_SOURCE10_STEP_DET_IBI_EN_MASK (0x01 << INT_SOURCE10_STEP_DET_IBI_EN_POS) + +/* + * step_cnt_ofl_ibi_en + * 0: Step count overflow interrupt not routed to IBI + * 1: Step count overflow interrupt routed to IBI + */ +#define INT_SOURCE10_STEP_CNT_OFL_IBI_EN_POS 0x04 +#define INT_SOURCE10_STEP_CNT_OFL_IBI_EN_MASK (0x01 << INT_SOURCE10_STEP_CNT_OFL_IBI_EN_POS) + +/* + * tilt_det_ibi_en + * 0: Tilt detect interrupt not routed to IBI + * 1: Tile detect interrupt routed to IBI + */ +#define INT_SOURCE10_TILT_DET_IBI_EN_POS 0x03 +#define INT_SOURCE10_TILT_DET_IBI_EN_MASK (0x01 << INT_SOURCE10_TILT_DET_IBI_EN_POS) + + + +/* + * APEX_CONFIG2 + * Register Name : APEX_CONFIG2 + */ + +/* + * low_energy_amp_th_sel + * Threshold to select a valid step. Used to increase step detection for slow walk use case. + * + * 0000: 30 mg + * 0001: 35 mg + * 0010: 40 mg + * 0011: 45 mg + * 0100: 50 mg + * 0101: 55 mg + * 0110: 60 mg + * 0111: 65 mg + * 1000: 70 mg + * 1001: 75 mg + * 1010: 80 mg (default) + * 1011: 85 mg + * 1100: 90 mg + * 1101: 95 mg + * 1110: 100 mg + * 1111: 105 mg + */ +#define APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS 0x04 +#define APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_MASK (0x0f << APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_POS) + +/* + * dmp_power_save_time_sel + * Duration of the period while the DMP stays awake after receiving a WOM event. + * + * 0000: 0 seconds + * 0001: 4 seconds + * 0010: 8 seconds (default) + * 0011: 12 seconds + * 0100: 16 seconds + * 0101: 20 seconds + * 0110: 24 seconds + * 0111: 28 seconds + * 1000: 32 seconds + * 1001: 36 seconds + * 1010: 40 seconds + * 1011: 44 seconds + * 1100: 48 seconds + * 1101: 52 seconds + * 1110: 56 seconds + * 1111: 60 seconds + */ +#define APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_POS 0x00 +#define APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_MASK 0x0f + + + +/* + * APEX_CONFIG3 + * Register Name : APEX_CONFIG3 + */ + +/* + * ped_amp_th_sel + * Threshold of step detection sensitivity. + * + * Low values increase detection sensitivity: reduce miss-detection. + * High values reduce detection sensitivity: reduce false-positive. + * + * 0000: 30 mg + * 0001: 34 mg + * 0010: 38 mg + * 0011: 42 mg + * 0100: 46 mg + * 0101: 50 mg + * 0110: 54 mg + * 0111: 58 mg + * 1000: 62 mg (default) + * 1001: 66 mg + * 1010: 70 mg + * 1011: 74 mg + * 1100: 78 mg + * 1101: 82 mg + * 1110: 86 mg + * 1111: 90 mg + */ +#define APEX_CONFIG3_PED_AMP_TH_SEL_POS 0x04 +#define APEX_CONFIG3_PED_AMP_TH_SEL_MASK (0x0f << APEX_CONFIG3_PED_AMP_TH_SEL_POS) + +/* + * ped_step_cnt_th_sel + * Minimum number of steps that must be detected before step count is incremented. + * + * Low values reduce latency but increase false positives. + * High values increase step count accuracy but increase latency. + * + * 0000: 0 steps + * 0001: 1 step + * 0010: 2 steps + * 0011: 3 steps + * 0100: 4 steps + * 0101: 5 steps (default) + * 0110: 6 steps + * 0111: 7 steps + * 1000: 8 steps + * 1001: 9 steps + * 1010: 10 steps + * 1011: 11 steps + * 1100: 12 steps + * 1101: 13 steps + * 1110: 14 steps + * 1111: 15 steps + */ +#define APEX_CONFIG3_PED_STEP_CNT_TH_SEL_POS 0x00 +#define APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK 0x0f + + + +/* + * APEX_CONFIG4 + * Register Name : APEX_CONFIG4 + */ + +/* + * ped_step_det_th_sel + * Minimum number of steps that must be detected before step event is signaled. + * + * Low values reduce latency but increase false positives. + * High values increase step event validity but increase latency. + * + * 000: 0 steps + * 001: 1 step + * 010: 2 steps (default) + * 011: 3 steps + * 100: 4 steps + * 101: 5 steps + * 110: 6 steps + * 111: 7 steps + */ +#define APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS 0x05 +#define APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK (0x07 << APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS) + +/* + * ped_sb_timer_th_sel + * Duration before algorithm considers that user has stopped taking steps. + * + * 000: 50 samples + * 001: 75 sample + * 010: 100 samples + * 011: 125 samples + * 100: 150 samples (default) + * 101: 175 samples + * 110: 200 samples + * 111: 225 samples + */ +#define APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS 0x02 +#define APEX_CONFIG4_PED_SB_TIMER_TH_SEL_MASK (0x07 << APEX_CONFIG4_PED_SB_TIMER_TH_SEL_POS) + +/* + * ped_hi_en_th_sel + * Threshold to classify acceleration signal as motion not due to steps. + * + * High values improve vibration rejection. + * Low values improve detection. + * + * 00: 87.89 mg + * 01: 104.49 mg (default) + * 10: 132.81 mg + * 11: 155.27 mg + */ +#define APEX_CONFIG4_PED_HI_EN_TH_SEL_POS 0x00 +#define APEX_CONFIG4_PED_HI_EN_TH_SEL_MASK 0x03 + + + +/* + * APEX_CONFIG5 + * Register Name : APEX_CONFIG5 + */ + +/* + * tilt_wait_time_sel + * Minimum duration for which the device should be tilted before signaling event. + * + * 00: 0s + * 01: 2s + * 10: 4s (default) + * 11: 6s + */ +#define APEX_CONFIG5_TILT_WAIT_TIME_SEL_POS 0x06 +#define APEX_CONFIG5_TILT_WAIT_TIME_SEL_MASK (0x03 << APEX_CONFIG5_TILT_WAIT_TIME_SEL_POS) + +/* + * lowg_peak_th_hyst_sel + * Hysteresis value added to the low-g threshold after exceeding it. + * + * 000: 31 mg (default) + * 001: 63 mg + * 010: 94 mg + * 011: 125 mg + * 100: 156 mg + * 101: 188 mg + * 110: 219 mg + * 111: 250 mg + */ +#define APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS 0x03 +#define APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_MASK (0x07 << APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_POS) + +/* + * highg_peak_th_hyst_sel + * Hysteresis value subtracted from the high-g threshold after exceeding it. + * + * 000: 31 mg (default) + * 001: 63 mg + * 010: 94 mg + * 011: 125 mg + * 100: 156 mg + * 101: 188 mg + * 110: 219 mg + * 111: 250 mg + */ +#define APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_POS 0x00 +#define APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_MASK 0x07 + + + +/* + * APEX_CONFIG9 + * Register Name : APEX_CONFIG9 + */ + +/* + * ff_debounce_duration_sel + * Period after a freefall is signaled during which a new freefall will not be detected. Prevents false detection due to bounces. + * + * 0000: 0 ms + * 0001: 1250 ms + * 0010: 1375 ms + * 0011: 1500 ms + * 0100: 1625 ms + * 0101: 1750 ms + * 0110: 1875 ms + * 0111: 2000 ms + * 1000: 2125 ms (default) + * 1001: 2250 ms + * 1010: 2375 ms + * 1011: 2500 ms + * 1100: 2625 ms + * 1101: 2750 ms + * 1110: 2875 ms + * 1111: 3000 ms + */ +#define APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS 0x04 +#define APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_MASK (0x0f << APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_POS) + +/* + * smd_sensitivity_sel + * Parameter to tune SMD algorithm robustness to rejection, ranging from 0 to 4 (values higher than 4 are reserved). + * + * Low values increase detection rate but increase false positives. + * High values reduce false positives but reduce detection rate (especially for transport use cases). + * + * Default value is 0. + */ +#define APEX_CONFIG9_SMD_SENSITIVITY_SEL_POS 0x01 +#define APEX_CONFIG9_SMD_SENSITIVITY_SEL_MASK (0x07 << APEX_CONFIG9_SMD_SENSITIVITY_SEL_POS) + +/* + * sensitivity_mode + * Pedometer sensitivity mode + * 0: Normal (default) + * 1: Slow walk + * + * Slow walk mode improves slow walk detection (<1Hz) but the number of false positives may increase. + */ +#define APEX_CONFIG9_SENSITIVITY_MODE_POS 0x00 +#define APEX_CONFIG9_SENSITIVITY_MODE_MASK 0x01 + + + +/* + * APEX_CONFIG10 + * Register Name : APEX_CONFIG10 + */ + +/* + * lowg_peak_th_sel + * Threshold for accel values below which low-g state is detected. + * + * 00000: 31 mg (default) + * 00001: 63 mg + * 00010: 94 mg + * 00011: 125 mg + * 00100: 156 mg + * 00101: 188 mg + * 00110: 219 mg + * 00111: 250 mg + * 01000: 281 mg + * 01001: 313 mg + * 01010: 344 mg + * 01011: 375 mg + * 01100: 406 mg + * 01101: 438 mg + * 01110: 469 mg + * 01111: 500 mg + * 10000: 531 mg + * 10001: 563 mg + * 10010: 594 mg + * 10011: 625 mg + * 10100: 656 mg + * 10101: 688 mg + * 10110: 719 mg + * 10111: 750 mg + * 11000: 781 mg + * 11001: 813 mg + * 11010: 844 mg + * 11011: 875 mg + * 11100: 906 mg + * 11101: 938 mg + * 11110: 969 mg + * 11111: 1000 mg + */ +#define APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS 0x03 +#define APEX_CONFIG10_LOWG_PEAK_TH_SEL_MASK (0x1f << APEX_CONFIG10_LOWG_PEAK_TH_SEL_POS) + +/* + * lowg_time_th_sel + * Number of samples required to enter low-g state. + * + * 000: 1 sample (default) + * 001: 2 samples + * 010: 3 samples + * 011: 4 samples + * 100: 5 samples + * 101: 6 samples + * 110: 7 samples + * 111: 8 samples + */ +#define APEX_CONFIG10_LOWG_TIME_TH_SEL_POS 0x00 +#define APEX_CONFIG10_LOWG_TIME_TH_SEL_MASK 0x07 + + + +/* + * APEX_CONFIG11 + * Register Name : APEX_CONFIG11 + */ + +/* + * highg_peak_th_sel + * Threshold for accel values above which high-g state is detected. + * + * 00000: 250 mg (default) + * 00001: 500 mg + * 00010: 750 mg + * 00011: 1000 mg + * 00100: 1250 mg + * 00101: 1500 mg + * 00110: 1750 mg + * 00111: 2000 mg + * 01000: 2250 mg + * 01001: 2500 mg + * 01010: 2750 mg + * 01011: 3000 mg + * 01100: 3250 mg + * 01101: 3500 mg + * 01110: 3750 mg + * 01111: 4000 mg + * 10000: 4250 mg + * 10001: 4500 mg + * 10010: 4750 mg + * 10011: 5000 mg + * 10100: 5250 mg + * 10101: 5500 mg + * 10110: 5750 mg + * 10111: 6000 mg + * 11000: 6250 mg + * 11001: 6500 mg + * 11010: 6750 mg + * 11011: 7000 mg + * 11100: 7250 mg + * 11101: 7500 mg + * 11110: 7750 mg + * 11111: 8000 mg + */ +#define APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS 0x03 +#define APEX_CONFIG11_HIGHG_PEAK_TH_SEL_MASK (0x1f << APEX_CONFIG11_HIGHG_PEAK_TH_SEL_POS) + +/* + * highg_time_th_sel + * Number of samples required to enter high-g state. + * + * 000: 1 sample (default) + * 001: 2 samples + * 010: 3 samples + * 011: 4 samples + * 100: 5 samples + * 101: 6 samples + * 110: 7 samples + * 111: 8 samples + */ +#define APEX_CONFIG11_HIGHG_TIME_TH_SEL_POS 0x00 +#define APEX_CONFIG11_HIGHG_TIME_TH_SEL_MASK 0x07 + + + +/* + * ACCEL_WOM_X_THR + * Register Name : ACCEL_WOM_X_THR + */ + +/* + * wom_x_th + * Threshold value for the Wake on Motion Interrupt for X-axis accelerometer + * WoM thresholds are expressed in fixed “mg” independent of the selected Range [0g : 1g]; Resolution 1g/256=~3.9mg + */ +#define ACCEL_WOM_X_THR_WOM_X_TH_POS 0x00 +#define ACCEL_WOM_X_THR_WOM_X_TH_MASK 0xff + + + +/* + * ACCEL_WOM_Y_THR + * Register Name : ACCEL_WOM_Y_THR + */ + +/* + * wom_y_th + * Threshold value for the Wake on Motion Interrupt for Y-axis accelerometer + * WoM thresholds are expressed in fixed “mg” independent of the selected Range [0g : 1g]; Resolution 1g/256=~3.9mg + */ +#define ACCEL_WOM_Y_THR_WOM_Y_TH_POS 0x00 +#define ACCEL_WOM_Y_THR_WOM_Y_TH_MASK 0xff + + + +/* + * ACCEL_WOM_Z_THR + * Register Name : ACCEL_WOM_Z_THR + */ + +/* + * wom_z_th + * Threshold value for the Wake on Motion Interrupt for Z-axis accelerometer + * WoM thresholds are expressed in fixed “mg” independent of the selected Range [0g : 1g]; Resolution 1g/256=~3.9mg + */ +#define ACCEL_WOM_Z_THR_WOM_Z_TH_POS 0x00 +#define ACCEL_WOM_Z_THR_WOM_Z_TH_MASK 0xff + + + +/* + * OFFSET_USER0 + * Register Name : OFFSET_USER0 + */ + +/* + * gyro_x_offuser + * Gyro offset programmed by user. Max value is +/-64 dps, resolution is 1/32 dps + */ +#define OFFSET_USER0_GYRO_X_OFFUSER_POS 0x00 +#define OFFSET_USER0_GYRO_X_OFFUSER_MASK 0xff + + + +/* + * OFFSET_USER1 + * Register Name : OFFSET_USER1 + */ + +/* + * gyro_x_offuser + * Gyro offset programmed by user. Max value is +/-64 dps, resolution is 1/32 dps + */ +#define OFFSET_USER1_GYRO_X_OFFUSER_POS 0x00 +#define OFFSET_USER1_GYRO_X_OFFUSER_MASK 0x0f + +/* + * gyro_y_offuser + * Gyro offset programmed by user. Max value is +/-64 dps, resolution is 1/32 dps + */ +#define OFFSET_USER1_GYRO_Y_OFFUSER_POS 0x04 +#define OFFSET_USER1_GYRO_Y_OFFUSER_MASK (0x0f << OFFSET_USER1_GYRO_Y_OFFUSER_POS) + + + +/* + * OFFSET_USER2 + * Register Name : OFFSET_USER2 + */ + +/* + * gyro_y_offuser + * Gyro offset programmed by user. Max value is +/-64 dps, resolution is 1/32 dps + */ +#define OFFSET_USER2_GYRO_Y_OFFUSER_POS 0x00 +#define OFFSET_USER2_GYRO_Y_OFFUSER_MASK 0xff + + + +/* + * OFFSET_USER3 + * Register Name : OFFSET_USER3 + */ + +/* + * gyro_z_offuser + * Gyro offset programmed by user. Max value is +/-64 dps, resolution is 1/32 dps + */ +#define OFFSET_USER3_GYRO_Z_OFFUSER_POS 0x00 +#define OFFSET_USER3_GYRO_Z_OFFUSER_MASK 0xff + + + +/* + * OFFSET_USER4 + * Register Name : OFFSET_USER4 + */ + +/* + * gyro_z_offuser + * Gyro offset programmed by user. Max value is +/-64 dps, resolution is 1/32 dps + */ +#define OFFSET_USER4_GYRO_Z_OFFUSER_POS 0x00 +#define OFFSET_USER4_GYRO_Z_OFFUSER_MASK 0x0f + +/* + * accel_x_offuser + * Accel offset programmed by user. Max value is +/-1 gee, resolution is 0.5 mgee + */ +#define OFFSET_USER4_ACCEL_X_OFFUSER_POS 0x04 +#define OFFSET_USER4_ACCEL_X_OFFUSER_MASK (0x0f << OFFSET_USER4_ACCEL_X_OFFUSER_POS) + + + +/* + * OFFSET_USER5 + * Register Name : OFFSET_USER5 + */ + +/* + * accel_x_offuser + * Accel offset programmed by user. Max value is +/-1 gee, resolution is 0.5 mgee + */ +#define OFFSET_USER5_ACCEL_X_OFFUSER_POS 0x00 +#define OFFSET_USER5_ACCEL_X_OFFUSER_MASK 0xff + + + +/* + * OFFSET_USER6 + * Register Name : OFFSET_USER6 + */ + +/* + * accel_y_offuser + * Accel offset programmed by user. Max value is +/-1 gee, resolution is 0.5 mgee + */ +#define OFFSET_USER6_ACCEL_Y_OFFUSER_POS 0x00 +#define OFFSET_USER6_ACCEL_Y_OFFUSER_MASK 0xff + + + +/* + * OFFSET_USER7 + * Register Name : OFFSET_USER7 + */ + +/* + * accel_y_offuser + * Accel offset programmed by user. Max value is +/-1 gee, resolution is 0.5 mgee + */ +#define OFFSET_USER7_ACCEL_Y_OFFUSER_POS 0x00 +#define OFFSET_USER7_ACCEL_Y_OFFUSER_MASK 0x0f + +/* + * accel_z_offuser + * Accel offset programmed by user. Max value is +/-1 gee, resolution is 0.5 mgee + */ +#define OFFSET_USER7_ACCEL_Z_OFFUSER_POS 0x04 +#define OFFSET_USER7_ACCEL_Z_OFFUSER_MASK (0x0f << OFFSET_USER7_ACCEL_Z_OFFUSER_POS) + + + +/* + * OFFSET_USER8 + * Register Name : OFFSET_USER8 + */ + +/* + * accel_z_offuser + * Accel offset programmed by user. Max value is +/-1 gee, resolution is 0.5 mgee + */ +#define OFFSET_USER8_ACCEL_Z_OFFUSER_POS 0x00 +#define OFFSET_USER8_ACCEL_Z_OFFUSER_MASK 0xff + + + +/* + * ST_STATUS1 + * Register Name : ST_STATUS1 + */ + +/* + * accel_st_pass + * 1: Accel self-test passed for all the 3 axes + */ +#define ST_STATUS1_ACCEL_ST_PASS_POS 0x05 +#define ST_STATUS1_ACCEL_ST_PASS_MASK (0x01 << ST_STATUS1_ACCEL_ST_PASS_POS) + +/* + * accel_st_done + * 1: Accel self-test done for all the 3 axes + */ +#define ST_STATUS1_ACCEL_ST_DONE_POS 0x04 +#define ST_STATUS1_ACCEL_ST_DONE_MASK (0x01 << ST_STATUS1_ACCEL_ST_DONE_POS) + +/* + * az_st_pass + * 1: Accel Z-axis self-test passed + */ +#define ST_STATUS1_AZ_ST_PASS_POS 0x03 +#define ST_STATUS1_AZ_ST_PASS_MASK (0x01 << ST_STATUS1_AZ_ST_PASS_POS) + +/* + * ay_st_pass + * 1: Accel Y-axis self-test passed + */ +#define ST_STATUS1_AY_ST_PASS_POS 0x02 +#define ST_STATUS1_AY_ST_PASS_MASK (0x01 << ST_STATUS1_AY_ST_PASS_POS) + +/* + * ax_st_pass + * 1: Accel X-axis self-test passed + */ +#define ST_STATUS1_AX_ST_PASS_POS 0x01 +#define ST_STATUS1_AX_ST_PASS_MASK (0x01 << ST_STATUS1_AX_ST_PASS_POS) + + + +/* + * ST_STATUS2 + * Register Name : ST_STATUS2 + */ + +/* + * st_incomplete + * 1: Self-test is incomplete. + * This bit is set to 1 if the self-test was aborted. + * One possible cause of aborting the self-test may be the detection of significant movement in the gyro when the self-test for gyro and/or accel is being executed. + */ +#define ST_STATUS2_ST_INCOMPLETE_POS 0x06 +#define ST_STATUS2_ST_INCOMPLETE_MASK (0x01 << ST_STATUS2_ST_INCOMPLETE_POS) + +/* + * gyro_st_pass + * 1: Gyro self-test passed for all the 3 axes + */ +#define ST_STATUS2_GYRO_ST_PASS_POS 0x05 +#define ST_STATUS2_GYRO_ST_PASS_MASK (0x01 << ST_STATUS2_GYRO_ST_PASS_POS) + +/* + * gyro_st_done + * 1: Gyro self-test done for all the 3 axes + */ +#define ST_STATUS2_GYRO_ST_DONE_POS 0x04 +#define ST_STATUS2_GYRO_ST_DONE_MASK (0x01 << ST_STATUS2_GYRO_ST_DONE_POS) + +/* + * gz_st_pass + * 1: Gyro Z-axis self-test passed + */ +#define ST_STATUS2_GZ_ST_PASS_POS 0x03 +#define ST_STATUS2_GZ_ST_PASS_MASK (0x01 << ST_STATUS2_GZ_ST_PASS_POS) + +/* + * gy_st_pass + * 1: Gyro Y-axis self-test passed + */ +#define ST_STATUS2_GY_ST_PASS_POS 0x02 +#define ST_STATUS2_GY_ST_PASS_MASK (0x01 << ST_STATUS2_GY_ST_PASS_POS) + +/* + * gx_st_pass + * 1: Gyro X-axis self-test passed + */ +#define ST_STATUS2_GX_ST_PASS_POS 0x01 +#define ST_STATUS2_GX_ST_PASS_MASK (0x01 << ST_STATUS2_GX_ST_PASS_POS) + + + +/* + * FDR_CONFIG + * Register Name : FDR_CONFIG + */ + +/* + * fdr_sel + * [7:4] Reserved + * [3:0] FIFO packet rate decimation factor. Sets the number of discarded FIFO packets. Valid range is 0 to 127. User must disable sensors when initializing FDR_SEL value or making changes to it. + * + * 0xxx: Decimation is disabled, all packets are sent to FIFO + * 1000: 1 packet out of 2 is sent to FIFO + * 1001: 1 packet out of 4 is sent to FIFO + * 1010: 1 packet out of 8 is sent to FIFO + * 1011: 1 packet out of 16 is sent to FIFO + * 1100: 1 packet out of 32 is sent to FIFO + * 1101: 1 packet out of 64 is sent to FIFO + * 1110: 1 packet out of 128 is sent to FIFO + * 1111: 1 packet out of 256 is sent to FIFO + */ +#define FDR_CONFIG_FDR_SEL_POS 0x00 +#define FDR_CONFIG_FDR_SEL_MASK 0xff + + + +/* + * APEX_CONFIG12 + * Register Name : APEX_CONFIG12 + */ + +/* + * ff_max_duration_sel + * Maximum freefall length. Longer freefalls are ignored. + * + * 0000: 102 cm (default) + * 0001: 120 cm + * 0010: 139 cm + * 0011: 159 cm + * 0100: 181 cm + * 0101: 204 cm + * 0110: 228 cm + * 0111: 254 cm + * 1000: 281 cm + * 1001: 310 cm + * 1010: 339 cm + * 1011: 371 cm + * 1100: 403 cm + * 1101: 438 cm + * 1110: 473 cm + * 1111: 510 cm + */ +#define APEX_CONFIG12_FF_MAX_DURATION_SEL_POS 0x04 +#define APEX_CONFIG12_FF_MAX_DURATION_SEL_MASK (0x0f << APEX_CONFIG12_FF_MAX_DURATION_SEL_POS) + +/* + * ff_min_duration_sel + * Minimum freefall length. Shorter freefalls are ignored. + * + * 0000: 10 cm (default) + * 0001: 12 cm + * 0010: 13 cm + * 0011: 16 cm + * 0100: 18 cm + * 0101: 20 cm + * 0110: 23 cm + * 0111: 25 cm + * 1000: 28 cm + * 1001: 31 cm + * 1010: 34 cm + * 1011: 38 cm + * 1100: 41 cm + * 1101: 45 cm + * 1110: 48 cm + * 1111: 52 cm + */ +#define APEX_CONFIG12_FF_MIN_DURATION_SEL_POS 0x00 +#define APEX_CONFIG12_FF_MIN_DURATION_SEL_MASK 0x0f + + +/* --------------------------------------------------------------------------- + * register MREG3 + * ---------------------------------------------------------------------------*/ + +/* + * XA_ST_DATA + * Register Name : XA_ST_DATA + */ + +/* + * xa_st_data + * Accel X-axis self test data converted to 8 bit code. + */ +#define XA_ST_DATA_XA_ST_DATA_POS 0x00 +#define XA_ST_DATA_XA_ST_DATA_MASK 0xff + + + +/* + * YA_ST_DATA + * Register Name : YA_ST_DATA + */ + +/* + * ya_st_data + * Accel Y-axis self test data converted to 8 bit code. + */ +#define YA_ST_DATA_YA_ST_DATA_POS 0x00 +#define YA_ST_DATA_YA_ST_DATA_MASK 0xff + + + +/* + * ZA_ST_DATA + * Register Name : ZA_ST_DATA + */ + +/* + * za_st_data + * Accel Z-axis self test data converted to 8 bit code. + */ +#define ZA_ST_DATA_ZA_ST_DATA_POS 0x00 +#define ZA_ST_DATA_ZA_ST_DATA_MASK 0xff + + + +/* + * XG_ST_DATA + * Register Name : XG_ST_DATA + */ + +/* + * xg_st_data + * Gyro X-axis self test data converted to 8 bit code. + */ +#define XG_ST_DATA_XG_ST_DATA_POS 0x00 +#define XG_ST_DATA_XG_ST_DATA_MASK 0xff + + + +/* + * YG_ST_DATA + * Register Name : YG_ST_DATA + */ + +/* + * yg_st_data + * Gyro Y-axis self test data converted to 8 bit code. + */ +#define YG_ST_DATA_YG_ST_DATA_POS 0x00 +#define YG_ST_DATA_YG_ST_DATA_MASK 0xff + + + +/* + * ZG_ST_DATA + * Register Name : ZG_ST_DATA + */ + +/* + * zg_st_data + * Gyro Z-axis self test data converted to 8 bit code. + */ +#define ZG_ST_DATA_ZG_ST_DATA_POS 0x00 +#define ZG_ST_DATA_ZG_ST_DATA_MASK 0xff + + +/* --------------------------------------------------------------------------- + * register MREG2 + * ---------------------------------------------------------------------------*/ + +/* + * OTP_CTRL7 + * Register Name : OTP_CTRL7 + */ + +/* + * otp_reload + * 1: to trigger OTP copy operation. This bit is cleared to 0 after OTP copy is done. + * + * With otp_copy_mode[1:0] = 2'b01, it takes 280us to complete the OTP reloading operation. + * With otp_copy_mode[1:0] = 2'b11, it takes 20us to complete the OTP reloading operation. + */ +#define OTP_CTRL7_OTP_RELOAD_POS 0x03 +#define OTP_CTRL7_OTP_RELOAD_MASK (0x01 << OTP_CTRL7_OTP_RELOAD_POS) + +/* + * otp_pwr_down + * 0: Power up OTP to copy from OTP to SRAM + * 1: Power down OTP + * + * This bit is automatically set to 1 when OTP copy operation is complete. + */ +#define OTP_CTRL7_OTP_PWR_DOWN_POS 0x01 +#define OTP_CTRL7_OTP_PWR_DOWN_MASK (0x01 << OTP_CTRL7_OTP_PWR_DOWN_POS) + +#ifdef __cplusplus +} +#endif + +#endif /*#ifndef _INV_IMU_REGMAP_REV_A_H_*/ \ No newline at end of file diff --git a/icm42670p/imu/inv_imu_selftest.c b/icm42670p/imu/inv_imu_selftest.c new file mode 100644 index 0000000..264edda --- /dev/null +++ b/icm42670p/imu/inv_imu_selftest.c @@ -0,0 +1,208 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#include "imu/inv_imu_selftest.h" +#include "imu/inv_imu_extfunc.h" + +static int configure_selftest_parameters(inv_imu_device_t * s, + const inv_imu_selftest_parameters_t st_params); + +int inv_imu_run_selftest(inv_imu_device_t *s, const inv_imu_selftest_parameters_t st_params, + inv_imu_selftest_output_t *st_output) +{ + int status = 0; + uint8_t value; + uint8_t data = 0; + uint8_t st_done = 0; + int polling_timeout_ms = 1000; + +#ifdef ICM42680 + /* Enable self-test */ + status |= inv_imu_read_reg(s, SENSOR_CONFIG3_MREG1, 1, &value); + value &= ~SENSOR_CONFIG3_APEX_DISABLE_MASK; + status |= inv_imu_write_reg(s, SENSOR_CONFIG3_MREG1, 1, &value); +#endif + + /* Disables Gyro/Accel sensors */ + status |= inv_imu_read_reg(s, PWR_MGMT0, 1, &value); + value &= ~PWR_MGMT0_ACCEL_MODE_MASK; +#if INV_IMU_IS_GYRO_SUPPORTED + value &= ~PWR_MGMT0_GYRO_MODE_MASK; +#endif + status |= inv_imu_write_reg(s, PWR_MGMT0, 1, &value); + + /* Enable RC oscillator */ + status |= inv_imu_switch_on_mclk(s); + + /* Clear DMP SRAM (1 ms wait included in `inv_imu_reset_dmp()`) */ + status |= inv_imu_reset_dmp(s, APEX_CONFIG0_DMP_MEM_RESET_APEX_ST_EN); + /* Update `dmp_is_on` since APEX features will have to restart from scratch */ + s->dmp_is_on = 0; + + /* Load self-test data */ + status |= inv_imu_load_selftest_data(s); + + /* Set self-test parameters */ + status |= configure_selftest_parameters(s, st_params); + + /* + * Enable accel and/or gyro self-test. + * If both accel and gyro self-test are enabled, + * they should be set simultaneously in the same write access + */ + status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value); + value &= ~SELFTEST_ACCEL_ST_EN_MASK; +#if INV_IMU_IS_GYRO_SUPPORTED + value &= ~SELFTEST_GYRO_ST_EN_MASK; +#endif + value |= (uint8_t)st_params.st_control; + status |= inv_imu_write_reg(s, SELFTEST_MREG1, 1, &value); + + /* Poll int_status_st_done bit */ + do { + inv_imu_sleep_us(1000); + status |= inv_imu_read_reg(s, INT_STATUS, 1, &st_done); + st_done &= INT_STATUS_ST_INT_MASK; + + if (0 == --polling_timeout_ms) + return (status | -1); /* Return error if timeout is reached */ + + } while (!st_done /* Exit if ST_DONE */ + && !status /* Or if error is detected */); + + /* Read self-test results (must start with ST_STATUS2) */ + status |= inv_imu_read_reg(s, ST_STATUS2_MREG1, 1, &data); +#if INV_IMU_IS_GYRO_SUPPORTED + st_output->gyro_status = (data & ST_STATUS2_GYRO_ST_PASS_MASK) >> ST_STATUS2_GYRO_ST_PASS_POS; + st_output->gyro_status |= + ((data & ST_STATUS2_ST_INCOMPLETE_MASK) >> ST_STATUS2_ST_INCOMPLETE_POS) << 1; + st_output->gx_status = (data & ST_STATUS2_GX_ST_PASS_MASK) >> ST_STATUS2_GX_ST_PASS_POS; + st_output->gy_status = (data & ST_STATUS2_GY_ST_PASS_MASK) >> ST_STATUS2_GY_ST_PASS_POS; + st_output->gz_status = (data & ST_STATUS2_GZ_ST_PASS_MASK) >> ST_STATUS2_GZ_ST_PASS_POS; +#endif + status |= inv_imu_read_reg(s, ST_STATUS1_MREG1, 1, &data); + st_output->accel_status = + (data & ST_STATUS1_ACCEL_ST_PASS_MASK) >> ST_STATUS1_ACCEL_ST_PASS_POS; + st_output->ax_status = (data & ST_STATUS1_AX_ST_PASS_MASK) >> ST_STATUS1_AX_ST_PASS_POS; + st_output->ay_status = (data & ST_STATUS1_AY_ST_PASS_MASK) >> ST_STATUS1_AY_ST_PASS_POS; + st_output->az_status = (data & ST_STATUS1_AZ_ST_PASS_MASK) >> ST_STATUS1_AZ_ST_PASS_POS; + + /* Disable self-test */ + status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value); + value &= ~SELFTEST_ACCEL_ST_EN_MASK; +#if INV_IMU_IS_GYRO_SUPPORTED + value &= ~SELFTEST_GYRO_ST_EN_MASK; +#endif + value |= (uint8_t)SELFTEST_DIS; + status |= inv_imu_write_reg(s, SELFTEST_MREG1, 1, &value); + + /* Reset FIFO because ST data may have been pushed to it */ + status |= inv_imu_reset_fifo(s); + + /* Restore idle bit */ + status |= inv_imu_switch_off_mclk(s); + +#ifdef ICM42680 + /* Disable self-test */ + status |= inv_imu_read_reg(s, SENSOR_CONFIG3_MREG1, 1, &value); + value &= ~SENSOR_CONFIG3_APEX_DISABLE_MASK; + value |= 1 << SENSOR_CONFIG3_APEX_DISABLE_POS; + status |= inv_imu_write_reg(s, SENSOR_CONFIG3_MREG1, 1, &value); +#endif + + return status; +} + +int inv_imu_init_selftest_parameters_struct(inv_imu_device_t * s, + inv_imu_selftest_parameters_t *st_params) +{ + (void)s; + st_params->st_num_samples = ST_CONFIG_16_SAMPLES; +#if INV_IMU_IS_GYRO_SUPPORTED + st_params->st_control = (SELFTEST_ACCEL_GYRO_ST_EN_t)SELFTEST_EN; +#else + st_params->st_control = (SELFTEST_ACCEL_GYRO_ST_EN_t)SELFTEST_ACCEL_EN; +#endif + return 0; +} + +int inv_imu_load_selftest_data(inv_imu_device_t *s) +{ + int status = 0; + uint8_t value; + + /* Enable RC oscillator */ + status |= inv_imu_switch_on_mclk(s); + + /* Set up OTP controller to reload factory-trimmed self-test response into SRAM */ + status |= inv_imu_read_reg(s, OTP_CONFIG_MREG1, 1, &value); + value &= ~OTP_CONFIG_OTP_COPY_MODE_MASK; + value |= (uint8_t)OTP_CONFIG_OTP_COPY_ST_DATA; + status |= inv_imu_write_reg(s, OTP_CONFIG_MREG1, 1, &value); + + /* Take the OTP macro out of power-down mode */ + status |= inv_imu_read_reg(s, OTP_CTRL7_MREG2, 1, &value); + value &= ~OTP_CTRL7_OTP_PWR_DOWN_MASK; + value |= (uint8_t)OTP_CTRL7_PWR_DOWN_DIS; + status |= inv_imu_write_reg(s, OTP_CTRL7_MREG2, 1, &value); + + /* Wait for voltage generator to power on */ + inv_imu_sleep_us(100); + + /* Host should disable INT function first before kicking off OTP copy operation */ + + /* Trigger OTP to reload data (this time in self-test mode) */ + status |= inv_imu_read_reg(s, OTP_CTRL7_MREG2, 1, &value); + value &= ~OTP_CTRL7_OTP_RELOAD_MASK; + value |= (uint8_t)OTP_CTRL7_OTP_RELOAD_EN; + status |= inv_imu_write_reg(s, OTP_CTRL7_MREG2, 1, &value); + + /* Wait for OTP reload */ + inv_imu_sleep_us(20); + + /* Disable RC oscillator */ + status |= inv_imu_switch_off_mclk(s); + + return status; +} + +static int configure_selftest_parameters(inv_imu_device_t * s, + const inv_imu_selftest_parameters_t st_params) +{ + int status = 0; + uint8_t value; + + /* Self-test configuration cannot be updated if it already running */ + status |= inv_imu_read_reg(s, SELFTEST_MREG1, 1, &value); + if (value & (SELFTEST_ACCEL_ST_EN_MASK | SELFTEST_GYRO_ST_EN_MASK)) + return INV_ERROR_UNEXPECTED; + + status |= inv_imu_read_reg(s, ST_CONFIG_MREG1, 1, &value); + value &= ~((uint8_t)ST_CONFIG_ST_NUMBER_SAMPLE_MASK); + value &= ~((uint8_t)ST_CONFIG_ACCEL_ST_LIM_MASK); +#if INV_IMU_IS_GYRO_SUPPORTED + value &= ~((uint8_t)ST_CONFIG_GYRO_ST_LIM_MASK); +#endif + value |= (uint8_t)st_params.st_num_samples; + value |= (uint8_t)ST_CONFIG_ACCEL_ST_LIM_50; +#if INV_IMU_IS_GYRO_SUPPORTED + value |= (uint8_t)ST_CONFIG_GYRO_ST_LIM_50; +#endif + status |= inv_imu_write_reg(s, ST_CONFIG_MREG1, 1, &value); + + return status; +} diff --git a/icm42670p/imu/inv_imu_selftest.h b/icm42670p/imu/inv_imu_selftest.h new file mode 100644 index 0000000..4019b6b --- /dev/null +++ b/icm42670p/imu/inv_imu_selftest.h @@ -0,0 +1,83 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/** @defgroup selftest Self-Test + * @brief High-level functions for Self-Test procedures + * @{ + */ + +/** @file inv_imu_selftest.h */ + +#ifndef _INV_IMU_SELFTEST_H_ +#define _INV_IMU_SELFTEST_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "imu/inv_imu_driver.h" + +/** Self-test input parameters */ +typedef struct { + ST_CONFIG_NUM_SAMPLES_t st_num_samples; /**< Number of samples used to perform self-test */ + SELFTEST_ACCEL_GYRO_ST_EN_t st_control; /**< Define which sensor is under self-test */ +} inv_imu_selftest_parameters_t; + +/** Self-test routine outputs */ +typedef struct { + int8_t accel_status; /**< global accel self-test passed */ + int8_t ax_status; /**< AX self-test status */ + int8_t ay_status; /**< AY self-test status */ + int8_t az_status; /**< AZ self-test status */ +#if INV_IMU_IS_GYRO_SUPPORTED + int8_t gyro_status; /**< global gyro self-test status: st_pass (bit0), st_incomplete (bit1) */ + int8_t gx_status; /**< GX self-test status */ + int8_t gy_status; /**< GY self-test status */ + int8_t gz_status; /**< GZ self-test status */ +#endif +} inv_imu_selftest_output_t; + +/** @brief Execute self-test. + * @param[in] s Pointer to device. + * @param[in] st_params Self-test parameters. + * @param[out] st_output Self-test results. + * @return 0 on success, negative value on error. + */ +int inv_imu_run_selftest(inv_imu_device_t *s, const inv_imu_selftest_parameters_t st_params, + inv_imu_selftest_output_t *st_output); + +/** @brief Fill the self-test configuration structure with default configuration. + * @param[in] s Pointer to device. + * @param[in] selftest_params Self-test parameters to be initialized. + * @return 0 on success, negative value on error. + */ +int inv_imu_init_selftest_parameters_struct(inv_imu_device_t * s, + inv_imu_selftest_parameters_t *selftest_params); + +/** @brief Load self-test data. + * @param[in] s Pointer to device. + * @return 0 on success, negative value on error. + */ +int inv_imu_load_selftest_data(inv_imu_device_t *s); + +#ifdef __cplusplus +} +#endif + +#endif /* _INV_IMU_SELFTEST_H_ */ + +/** @} */ diff --git a/icm42670p/imu/inv_imu_transport.c b/icm42670p/imu/inv_imu_transport.c new file mode 100644 index 0000000..cf4d698 --- /dev/null +++ b/icm42670p/imu/inv_imu_transport.c @@ -0,0 +1,296 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#include "imu/inv_imu_transport.h" +#include "imu/inv_imu_extfunc.h" + +#include /* NULL */ + +#define TIMEOUT_US 1000000 /* 1 sec */ + +/* Function definition */ +static uint8_t *get_register_cache_addr(void *t, const uint32_t reg); +static int write_sreg(void *t, uint8_t reg, uint32_t len, const uint8_t *buf); +static int read_sreg(void *t, uint8_t reg, uint32_t len, uint8_t *buf); +static int write_mclk_reg(void *t, uint16_t regaddr, uint8_t wr_cnt, const uint8_t *buf); +static int read_mclk_reg(void *t, uint16_t regaddr, uint8_t rd_cnt, uint8_t *buf); + +int inv_imu_init_transport(void *t) +{ + int status = 0; + struct inv_imu_transport *tr = (struct inv_imu_transport *)t; + + if (tr == NULL) + return INV_ERROR_BAD_ARG; + + status |= read_sreg(t, (uint8_t)PWR_MGMT0, 1, &(tr->register_cache.pwr_mgmt0_reg)); +#if INV_IMU_IS_GYRO_SUPPORTED + status |= read_sreg(t, (uint8_t)GYRO_CONFIG0, 1, &(tr->register_cache.gyro_config0_reg)); +#endif + status |= read_sreg(t, (uint8_t)ACCEL_CONFIG0, 1, &(tr->register_cache.accel_config0_reg)); + + status |= + read_mclk_reg(t, (TMST_CONFIG1_MREG1 & 0xFFFF), 1, &(tr->register_cache.tmst_config1_reg)); + + tr->need_mclk_cnt = 0; + + return status; +} + +int inv_imu_read_reg(void *t, uint32_t reg, uint32_t len, uint8_t *buf) +{ + int rc = 0; + + if (t == NULL) + return INV_ERROR_BAD_ARG; + + for (uint32_t i = 0; i < len; i++) { + const uint8_t *cache_addr = get_register_cache_addr(t, reg + i); + + if (cache_addr) { + buf[i] = *cache_addr; + } else { + if (!(reg & 0x10000)) { + rc |= read_mclk_reg(t, ((reg + i) & 0xFFFF), 1, &buf[i]); + } else { + rc |= read_sreg(t, (uint8_t)(reg + i), len - i, &buf[i]); + break; + } + } + } + + return rc; +} + +int inv_imu_write_reg(void *t, uint32_t reg, uint32_t len, const uint8_t *buf) +{ + int rc = 0; + + if (t == NULL) + return INV_ERROR_BAD_ARG; + + for (uint32_t i = 0; i < len; i++) { + uint8_t *cache_addr = get_register_cache_addr(t, reg + i); + + if (cache_addr) + *cache_addr = buf[i]; + + if (!(reg & 0x10000)) + rc |= write_mclk_reg(t, ((reg + i) & 0xFFFF), 1, &buf[i]); + } + + if (reg & 0x10000) + rc |= write_sreg(t, (uint8_t)reg, len, buf); + + return rc; +} + +int inv_imu_switch_on_mclk(void *t) +{ + int status = 0; + uint8_t data; + struct inv_imu_transport *tr = (struct inv_imu_transport *)t; + + if (tr == NULL) + return INV_ERROR_BAD_ARG; + + /* set IDLE bit only if it is not set yet */ + if (tr->need_mclk_cnt == 0) { + uint64_t start; + + status |= inv_imu_read_reg(t, PWR_MGMT0, 1, &data); + data |= PWR_MGMT0_IDLE_MASK; + status |= inv_imu_write_reg(t, PWR_MGMT0, 1, &data); + + if (status) + return status; + + /* Check if MCLK is ready */ + start = inv_imu_get_time_us(); + do { + status = inv_imu_read_reg(t, MCLK_RDY, 1, &data); + + if (status) + return status; + + /* Timeout */ + if (inv_imu_get_time_us() - start > TIMEOUT_US) + return INV_ERROR_TIMEOUT; + + } while (!(data & MCLK_RDY_MCLK_RDY_MASK)); + } else { + /* Make sure it is already on */ + status |= inv_imu_read_reg(t, PWR_MGMT0, 1, &data); + if (0 == (data &= PWR_MGMT0_IDLE_MASK)) + status |= INV_ERROR; + } + + /* Increment the counter to keep track of number of MCLK requesters */ + tr->need_mclk_cnt++; + + return status; +} + +int inv_imu_switch_off_mclk(void *t) +{ + int status = 0; + uint8_t data; + struct inv_imu_transport *tr = (struct inv_imu_transport *)t; + + if (tr == NULL) + return INV_ERROR_BAD_ARG; + + /* Reset the IDLE but only if there is one requester left */ + if (tr->need_mclk_cnt == 1) { + status |= inv_imu_read_reg(t, PWR_MGMT0, 1, &data); + data &= ~PWR_MGMT0_IDLE_MASK; + status |= inv_imu_write_reg(t, PWR_MGMT0, 1, &data); + } else { + /* Make sure it is still on */ + status |= inv_imu_read_reg(t, PWR_MGMT0, 1, &data); + if (0 == (data &= PWR_MGMT0_IDLE_MASK)) + status |= INV_ERROR; + } + + /* Decrement the counter */ + tr->need_mclk_cnt--; + + return status; +} + +/* Static function */ + +static uint8_t *get_register_cache_addr(void *t, const uint32_t reg) +{ + struct inv_imu_transport *tr = (struct inv_imu_transport *)t; + + if (tr == NULL) + return (uint8_t *)0; /* error */ + + switch (reg) { + case PWR_MGMT0: + return &(tr->register_cache.pwr_mgmt0_reg); +#if INV_IMU_IS_GYRO_SUPPORTED + case GYRO_CONFIG0: + return &(tr->register_cache.gyro_config0_reg); +#endif + case ACCEL_CONFIG0: + return &(tr->register_cache.accel_config0_reg); + case TMST_CONFIG1_MREG1: + return &(tr->register_cache.tmst_config1_reg); + default: + return (uint8_t *)0; // Not found + } +} + +static int read_sreg(void *t, uint8_t reg, uint32_t len, uint8_t *buf) +{ + struct inv_imu_serif *serif = (struct inv_imu_serif *)t; + + if (serif == NULL) + return INV_ERROR_BAD_ARG; + + if (len > serif->max_read) + return INV_ERROR_SIZE; + + if (serif->read_reg(serif, reg, buf, len) != 0) + return INV_ERROR_TRANSPORT; + + return 0; +} + +static int write_sreg(void *t, uint8_t reg, uint32_t len, const uint8_t *buf) +{ + struct inv_imu_serif *serif = (struct inv_imu_serif *)t; + + if (serif == NULL) + return INV_ERROR_BAD_ARG; + + if (len > serif->max_write) + return INV_ERROR_SIZE; + + if (serif->write_reg(serif, reg, buf, len) != 0) + return INV_ERROR_TRANSPORT; + + return 0; +} + +static int read_mclk_reg(void *t, uint16_t regaddr, uint8_t rd_cnt, uint8_t *buf) +{ + uint8_t data; + uint8_t blk_sel = (regaddr & 0xFF00) >> 8; + int status = 0; + + if (t == NULL) + return INV_ERROR_BAD_ARG; + + /* Have IMU not in IDLE mode to access MCLK domain */ + status |= inv_imu_switch_on_mclk(t); + + /* optimize by changing BLK_SEL only if not NULL */ + if (blk_sel) + status |= write_sreg(t, (uint8_t)BLK_SEL_R & 0xff, 1, &blk_sel); + + data = (regaddr & 0x00FF); + status |= write_sreg(t, (uint8_t)MADDR_R, 1, &data); + inv_imu_sleep_us(10); + status |= read_sreg(t, (uint8_t)M_R, rd_cnt, buf); + inv_imu_sleep_us(10); + + if (blk_sel) { + data = 0; + status |= write_sreg(t, (uint8_t)BLK_SEL_R, 1, &data); + } + + /* switch OFF MCLK if needed */ + status |= inv_imu_switch_off_mclk(t); + + return status; +} + +static int write_mclk_reg(void *t, uint16_t regaddr, uint8_t wr_cnt, const uint8_t *buf) +{ + uint8_t data; + uint8_t blk_sel = (regaddr & 0xFF00) >> 8; + int status = 0; + + if (t == NULL) + return INV_ERROR_BAD_ARG; + + /* Have IMU not in IDLE mode to access MCLK domain */ + status |= inv_imu_switch_on_mclk(t); + + /* optimize by changing BLK_SEL only if not NULL */ + if (blk_sel) + status |= write_sreg(t, (uint8_t)BLK_SEL_W, 1, &blk_sel); + + data = (regaddr & 0x00FF); + status |= write_sreg(t, (uint8_t)MADDR_W, 1, &data); + for (uint8_t i = 0; i < wr_cnt; i++) { + status |= write_sreg(t, (uint8_t)M_W, 1, &buf[i]); + inv_imu_sleep_us(10); + } + + if (blk_sel) { + data = 0; + status = write_sreg(t, (uint8_t)BLK_SEL_W, 1, &data); + } + + status |= inv_imu_switch_off_mclk(t); + + return status; +} diff --git a/icm42670p/imu/inv_imu_transport.h b/icm42670p/imu/inv_imu_transport.h new file mode 100644 index 0000000..59425f9 --- /dev/null +++ b/icm42670p/imu/inv_imu_transport.h @@ -0,0 +1,120 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +/** @defgroup Transport Transport + * @brief Abstraction layer to access device's registers + * @{ + */ + +/** @file inv_imu_transport.h */ + +#ifndef _INV_IMU_TRANSPORT_H_ +#define _INV_IMU_TRANSPORT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "imu/inv_imu_defs.h" + +#include "Invn/InvError.h" + +/* Available serial interface type. */ +#define UI_I2C 0 /**< identifies I2C interface. */ +#define UI_SPI4 1 /**< identifies 4-wire SPI interface. */ +#define UI_SPI3 2 /**< identifies 3-wire SPI interface. */ + +/** @brief Serif type definition. + * @deprecated Kept for retro-compatibility. Replaced with `uint32_t` type + * in `struct inv_imu_serif` struct. + */ +typedef uint32_t SERIAL_IF_TYPE_t; + +/** Serial interface definition */ +typedef struct inv_imu_serif { + void *context; + int (*read_reg)(struct inv_imu_serif *serif, uint8_t reg, uint8_t *buf, uint32_t len); + int (*write_reg)(struct inv_imu_serif *serif, uint8_t reg, const uint8_t *buf, uint32_t len); + uint32_t max_read; + uint32_t max_write; + uint32_t serif_type; +} inv_imu_serif_t; + +/** Transport interface definition. */ +typedef struct inv_imu_transport { + /** Serial interface object. + * @warning Must be the first object in this structure. + */ + inv_imu_serif_t serif; + + /** Contains mirrored values of some IP registers. */ + struct register_cache { + uint8_t pwr_mgmt0_reg; +#if INV_IMU_IS_GYRO_SUPPORTED + uint8_t gyro_config0_reg; +#endif + uint8_t accel_config0_reg; + uint8_t tmst_config1_reg; + } register_cache; + + /** Internal counter for MCLK requests. */ + uint8_t need_mclk_cnt; +} inv_imu_transport_t; + +/** @brief Init cache variable. + * @param[in] t Pointer to transport (as void * so it can be called from any module). + * @return 0 on success, negative value on error. + */ +int inv_imu_init_transport(void *t); + +/** @brief Reads data from a register on IMU. + * @param[in] t Pointer to transport (as void * so it can be called from any module). + * @param[in] reg Register address to be read. + * @param[in] len Number of byte to be read. + * @param[out] buf Output data from the register. + * @return 0 on success, negative value on error. + */ +int inv_imu_read_reg(void *t, uint32_t reg, uint32_t len, uint8_t *buf); + +/** @brief Writes data to a register on IMU. + * @param[in] t Pointer to transport (as void * so it can be called from any module). + * @param[in] reg Register address to be written. + * @param[in] len Number of byte to be written. + * @param[in] buf Input data to write. + * @return 0 on success, negative value on error. + */ +int inv_imu_write_reg(void *t, uint32_t reg, uint32_t len, const uint8_t *buf); + +/** @brief Enable MCLK. + * @param[in] t Pointer to transport (as void * so it can be called from any module). + * @return 0 on success, negative value on error. + */ +int inv_imu_switch_on_mclk(void *t); + +/** @brief Disable MCLK. + * @param[in] t Pointer to transport (as void * so it can be called from any module). + * @return 0 on success, negative value on error. + */ +int inv_imu_switch_off_mclk(void *t); + +#ifdef __cplusplus +} +#endif + +#endif /* _INV_IMU_TRANSPORT_H_ */ + +/** @} */ diff --git a/icm42670p/imu/inv_imu_version.h b/icm42670p/imu/inv_imu_version.h new file mode 100644 index 0000000..57757ee --- /dev/null +++ b/icm42670p/imu/inv_imu_version.h @@ -0,0 +1,31 @@ +/* + * + * Copyright (c) [2018] by InvenSense, Inc. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + * + */ + +#ifndef _INV_IMU_VERSION_H_ +#define _INV_IMU_VERSION_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#define INV_IMU_VERSION_STRING "3.0.0" + +#ifdef __cplusplus +} +#endif + +#endif /* _INV_IMU_VERSION_H_ */