Drop 3.0.0 release
This commit is contained in:
Loic Michallon
geoffroy.jabouley
parent
a7fb6c310a
commit
8512856713
1
.gitignore
vendored
Normal file
1
.gitignore
vendored
Normal file
@ -0,0 +1 @@
|
||||
*.tar.gz
|
||||
41
CMakeLists.txt
Normal file
41
CMakeLists.txt
Normal file
@ -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)
|
||||
|
||||
28
LICENSE.txt
Normal file
28
LICENSE.txt
Normal file
@ -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.
|
||||
26
README.md
Normal file
26
README.md
Normal file
@ -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/
|
||||
```
|
||||
51
examples/Invn/InvError.h
Normal file
51
examples/Invn/InvError.h
Normal file
@ -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_ */
|
||||
|
||||
/** @} */
|
||||
64
examples/eis/README.md
Normal file
64
examples/eis/README.md
Normal file
@ -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:
|
||||
```
|
||||
<timestamp> us Gyro: <gyr_x> <gyr_y> <gyr_z> dps [FSYNC occurred <fsync_delay> us ago]
|
||||
```
|
||||
|
||||
With:
|
||||
* `<timestamp>`: Time in microsecond read from MCU clock when latest INT1 was fired.
|
||||
* `<gyr_x|y|z>`: Raw gyroscope value converted in dps.
|
||||
* `<fsync_count>`: 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
|
||||
357
examples/eis/eis.c
Normal file
357
examples/eis/eis.c
Normal file
@ -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 <stdio.h>
|
||||
|
||||
/*
|
||||
* 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);
|
||||
}
|
||||
97
examples/freefall/README.md
Normal file
97
examples/freefall/README.md
Normal file
@ -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:
|
||||
|
||||
```
|
||||
<timestamp> us LOW_G
|
||||
<timestamp> us FREEFALL length: <ff_length> cm (<ff_duration> ms)
|
||||
```
|
||||
|
||||
With:
|
||||
* `<timestamp>`: Time in microsecond read from MCU clock when latest INT1 was fired.
|
||||
* `<ff_length>`: Length of the FreeFall in centimeters, computed based on the number of samples.
|
||||
* `<ff_duration>`: 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)
|
||||
```
|
||||
|
||||
240
examples/freefall/freefall.c
Normal file
240
examples/freefall/freefall.c
Normal file
@ -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 <stddef.h> /* 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);
|
||||
}
|
||||
51
examples/pedometer/README.md
Normal file
51
examples/pedometer/README.md
Normal file
@ -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:
|
||||
|
||||
```
|
||||
<timestamp> us STEP_DET count: <step_count> steps cadence: <cadence> steps/s activity: <activity_class>
|
||||
```
|
||||
|
||||
With:
|
||||
* `<timestamp>`: 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. `<activity_class>` 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
|
||||
```
|
||||
|
||||
364
examples/pedometer/pedometer.c
Normal file
364
examples/pedometer/pedometer.c
Normal file
@ -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 <stddef.h> /* 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);
|
||||
}
|
||||
72
examples/raw/README.md
Normal file
72
examples/raw/README.md
Normal file
@ -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 <timestamp> us Accel: <acc_x> <acc_y> <acc_z> g Gyro: <gyr_x> <gyr_y> <gyr_z> dps Temp: <temp> degC FIFO time: <fifo_time> us
|
||||
```
|
||||
* When print in LSB is enabled:
|
||||
```
|
||||
LSB <timestamp> us Accel: <raw_acc_x> <raw_acc_y> <raw_acc_z> Gyro: <raw_gyr_x> <raw_gyr_y> <raw_gyr_z> Temp: <raw_temp> FIFO time: <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
|
||||
```
|
||||
|
||||
484
examples/raw/raw.c
Normal file
484
examples/raw/raw.c
Normal file
@ -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 <stdio.h>
|
||||
|
||||
/*
|
||||
* 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);
|
||||
}
|
||||
27
examples/selftest/README.md
Normal file
27
examples/selftest/README.md
Normal file
@ -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
|
||||
```
|
||||
|
||||
173
examples/selftest/selftest.c
Normal file
173
examples/selftest/selftest.c
Normal file
@ -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 <stddef.h> /* 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);
|
||||
}
|
||||
34
examples/smd/README.md
Normal file
34
examples/smd/README.md
Normal file
@ -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:
|
||||
|
||||
```
|
||||
<timestamp> us SMD
|
||||
```
|
||||
|
||||
With:
|
||||
* `<timestamp>`: 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
|
||||
```
|
||||
|
||||
316
examples/smd/smd.c
Normal file
316
examples/smd/smd.c
Normal file
@ -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 <stddef.h> /* 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);
|
||||
}
|
||||
202
examples/system_interface.c
Normal file
202
examples/system_interface.c
Normal file
@ -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 <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
/* 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;
|
||||
}
|
||||
216
examples/system_interface.h
Normal file
216
examples/system_interface.h
Normal file
@ -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_ */
|
||||
37
examples/tilt/README.md
Normal file
37
examples/tilt/README.md
Normal file
@ -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:
|
||||
|
||||
```
|
||||
<timestamp> us TILT
|
||||
```
|
||||
|
||||
With:
|
||||
* `<timestamp>`: 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
|
||||
```
|
||||
|
||||
330
examples/tilt/tilt.c
Normal file
330
examples/tilt/tilt.c
Normal file
@ -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 <stddef.h> /* 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);
|
||||
}
|
||||
41
examples/wom/README.md
Normal file
41
examples/wom/README.md
Normal file
@ -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:
|
||||
|
||||
```
|
||||
<timestamp> us WOM X=<x> Y=<y> Z=<z>
|
||||
```
|
||||
|
||||
With:
|
||||
* `<timestamp>`: Time in microsecond read from MCU clock when latest INT1 was fired.
|
||||
* `<x>`: 1 when motion through X axis exceeds configured threshold, 0 otherwise.
|
||||
* `<y>`: 1 when motion through Y axis exceeds configured threshold, 0 otherwise.
|
||||
* `<z>`: 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
|
||||
```
|
||||
|
||||
218
examples/wom/wom.c
Normal file
218
examples/wom/wom.c
Normal file
@ -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 <stddef.h> /* 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);
|
||||
}
|
||||
84
icm42670p/imu/README.md
Normal file
84
icm42670p/imu/README.md
Normal file
@ -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);
|
||||
```
|
||||
50
icm42670p/imu/inv_imu.h
Normal file
50
icm42670p/imu/inv_imu.h
Normal file
@ -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_ */
|
||||
|
||||
/** @} */
|
||||
354
icm42670p/imu/inv_imu_apex.c
Normal file
354
icm42670p/imu/inv_imu_apex.c
Normal file
@ -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;
|
||||
}
|
||||
179
icm42670p/imu/inv_imu_apex.h
Normal file
179
icm42670p/imu/inv_imu_apex.h
Normal file
@ -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_ */
|
||||
|
||||
/** @} */
|
||||
1002
icm42670p/imu/inv_imu_defs.h
Normal file
1002
icm42670p/imu/inv_imu_defs.h
Normal file
File diff suppressed because it is too large
Load Diff
1685
icm42670p/imu/inv_imu_driver.c
Normal file
1685
icm42670p/imu/inv_imu_driver.c
Normal file
File diff suppressed because it is too large
Load Diff
543
icm42670p/imu/inv_imu_driver.h
Normal file
543
icm42670p/imu/inv_imu_driver.h
Normal file
@ -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_ */
|
||||
|
||||
/** @} */
|
||||
50
icm42670p/imu/inv_imu_extfunc.h
Normal file
50
icm42670p/imu/inv_imu_extfunc.h
Normal file
@ -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 <stdint.h>
|
||||
|
||||
#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_ */
|
||||
|
||||
/** @} */
|
||||
3369
icm42670p/imu/inv_imu_regmap_rev_a.h
Normal file
3369
icm42670p/imu/inv_imu_regmap_rev_a.h
Normal file
File diff suppressed because it is too large
Load Diff
208
icm42670p/imu/inv_imu_selftest.c
Normal file
208
icm42670p/imu/inv_imu_selftest.c
Normal file
@ -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;
|
||||
}
|
||||
83
icm42670p/imu/inv_imu_selftest.h
Normal file
83
icm42670p/imu/inv_imu_selftest.h
Normal file
@ -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_ */
|
||||
|
||||
/** @} */
|
||||
296
icm42670p/imu/inv_imu_transport.c
Normal file
296
icm42670p/imu/inv_imu_transport.c
Normal file
@ -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 <stddef.h> /* 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;
|
||||
}
|
||||
120
icm42670p/imu/inv_imu_transport.h
Normal file
120
icm42670p/imu/inv_imu_transport.h
Normal file
@ -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_ */
|
||||
|
||||
/** @} */
|
||||
31
icm42670p/imu/inv_imu_version.h
Normal file
31
icm42670p/imu/inv_imu_version.h
Normal file
@ -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_ */
|
||||
Loading…
x
Reference in New Issue
Block a user