diff --git a/.gitmodules b/.gitmodules index f5cf07a..e568e4d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -37,6 +37,3 @@ [submodule "examples/freetype/main/lib/freetype"] path = examples/freetype/main/lib/freetype url = https://gitlab.freedesktop.org/freetype/freetype.git -[submodule "components/lvgl/lv_lib/lv_lib_qrcode"] - path = components/lvgl/lv_lib/lv_lib_qrcode - url = https://github.com/lvgl/lv_lib_qrcode.git diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 681b635..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -# CMakeLists.txt -# -# Copyright 2021 Espressif Systems (Shanghai) Co. Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# diff --git a/components/i2c_devices/CMakeLists.txt b/components/i2c_devices/CMakeLists.txt index 7d374f7..213f453 100644 --- a/components/i2c_devices/CMakeLists.txt +++ b/components/i2c_devices/CMakeLists.txt @@ -1,9 +1,11 @@ -file(GLOB_RECURSE CSRC_FILES *.c) + idf_component_register( - SRCS - ${CSRC_FILES} + SRC_DIRS + "icm42670" + "touch_panel" INCLUDE_DIRS - "include" + "icm42670/include" + "touch_panel/include" REQUIRES bsp) diff --git a/components/i2c_devices/icm42670/Message.c b/components/i2c_devices/icm42670/Message.c new file mode 100644 index 0000000..f6ee93b --- /dev/null +++ b/components/i2c_devices/icm42670/Message.c @@ -0,0 +1,75 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively “Software”) is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +#include "Message.h" + +#include +#include + +static int msg_level; +static inv_msg_printer_t msg_printer; + +void inv_msg_printer_default(int level, const char * str, va_list ap) +{ +#if !defined(__ICCARM__) + const char * s[INV_MSG_LEVEL_MAX] = { + "", // INV_MSG_LEVEL_OFF + " [E] ", // INV_MSG_LEVEL_ERROR + " [W] ", // INV_MSG_LEVEL_WARNING + " [I] ", // INV_MSG_LEVEL_INFO + " [V] ", // INV_MSG_LEVEL_VERBOSE + " [D] ", // INV_MSG_LEVEL_DEBUG + }; + + fprintf(stderr, "%s", s[level]); + vfprintf(stderr, str, ap); + fprintf(stderr, "\n"); +#else + (void)level, (void)str, (void)ap; +#endif +} + +void inv_msg_setup(int level, inv_msg_printer_t printer) +{ + msg_level = level; + if (level < INV_MSG_LEVEL_OFF) + msg_level = INV_MSG_LEVEL_OFF; + else if (level > INV_MSG_LEVEL_MAX) + msg_level = INV_MSG_LEVEL_MAX; + msg_printer = printer; +} + +void inv_msg(int level, const char * str, ...) +{ + if(level && level <= msg_level && msg_printer) { + va_list ap; + va_start(ap, str); + msg_printer(level, str, ap); + va_end(ap); + } +} + +int inv_msg_get_level(void) +{ + return msg_level; +} diff --git a/components/i2c_devices/icm42670/icm42670.c b/components/i2c_devices/icm42670/icm42670.c new file mode 100644 index 0000000..798fcf2 --- /dev/null +++ b/components/i2c_devices/icm42670/icm42670.c @@ -0,0 +1,169 @@ +// Copyright 2020 Espressif Systems (Shanghai) Co. Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "stdio.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "esp_log.h" +#include "bsp_i2c.h" +#include "Message.h" +#include "inv_imu_driver.h" + +static const char *TAG = "icm42670"; + + +/* + * Set of timers used throughout standalone applications + */ + +#define SENSOR_ODR_1600HZ 1600 +#define SENSOR_ODR_800HZ 800 +#define SENSOR_ODR_400HZ 400 +#define SENSOR_ODR_200HZ 200 +#define SENSOR_ODR_100HZ 100 +#define SENSOR_ODR_50HZ 50 +#define SENSOR_ODR_25HZ 25 +#define SENSOR_ODR_12_5HZ 12.5 + + +static i2c_bus_device_handle_t g_i2c_dev_handle; + +static int platform_io_hal_read_reg(void *context, uint8_t reg, uint8_t *buf, uint32_t len) +{ + return i2c_bus_read_bytes(g_i2c_dev_handle, reg, len, buf); +} + +static int platform_io_hal_write_reg(void *context, uint8_t reg, const uint8_t *buf, uint32_t len) +{ + return i2c_bus_write_bytes(g_i2c_dev_handle, reg, len, buf); +} + +static void platform_delay_ms(uint32_t ms) +{ + vTaskDelay(pdMS_TO_TICKS(ms)); +} + +static void platform_delay_us(uint32_t us) +{ + ets_delay_us(us); +} + +/* + * Printer function for message facility + */ +static void msg_printer(int level, const char *str, va_list ap) +{ + static char out_str[2560]; /* static to limit stack usage */ + uint32_t idx = 0; +#define LOG_COLOR_BLACK "30" +#define LOG_COLOR_RED "31" +#define LOG_COLOR_GREEN "32" +#define LOG_COLOR_BROWN "33" +#define LOG_COLOR_BLUE "34" +#define LOG_COLOR_PURPLE "35" +#define LOG_COLOR_CYAN "36" +#define LOG_COLOR(COLOR) "\033[0;" COLOR "m" +#define LOG_BOLD(COLOR) "\033[1;" COLOR "m" +#define LOG_RESET_COLOR "\033[0m" +#define LOG_COLOR_E LOG_COLOR(LOG_COLOR_RED) +#define LOG_COLOR_W LOG_COLOR(LOG_COLOR_BROWN) +#define LOG_COLOR_I LOG_COLOR(LOG_COLOR_GREEN) + const char *s[INV_MSG_LEVEL_MAX] = { + "", // INV_MSG_LEVEL_OFF + LOG_COLOR_E "[E]", // INV_MSG_LEVEL_ERROR + LOG_COLOR_W "[W]", // INV_MSG_LEVEL_WARNING + LOG_COLOR_I "[I]", // INV_MSG_LEVEL_INFO + LOG_COLOR_BLUE "[V]", // INV_MSG_LEVEL_VERBOSE + LOG_COLOR_BLUE "[D]", // INV_MSG_LEVEL_DEBUG + }; + if (level >= INV_MSG_LEVEL_MAX) { + printf("message error!\r\n"); + return; + } + + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "%s %s: ", s[level], TAG); + if (idx >= (sizeof(out_str))) { + return; + } + idx += vsnprintf(&out_str[idx], sizeof(out_str) - idx, str, ap); + if (idx >= (sizeof(out_str))) { + return; + } + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, LOG_RESET_COLOR "\r\n"); + if (idx >= (sizeof(out_str))) { + return; + } + printf(out_str); +} + +esp_err_t icm42670_init(void) +{ + esp_err_t ret = ESP_OK; + if (NULL != g_i2c_dev_handle) { + return ESP_FAIL; + } + + bsp_i2c_add_device(&g_i2c_dev_handle, 0x68); + + if (NULL == g_i2c_dev_handle) { + return ESP_FAIL; + } + + /* Setup message facility to see internal traces from FW */ + inv_msg_setup(INV_MSG_LEVEL_INFO, msg_printer); + + inv_imu_set_serif(platform_io_hal_read_reg, platform_io_hal_write_reg); + inv_imu_set_delay(platform_delay_ms, platform_delay_us); + + float hw_rate = 0.0; + + /* Init device */ + ret = inv_imu_initialize(); + + if (ret != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize INV concise driver!"); + return ret; + } + + INV_MSG(INV_MSG_LEVEL_INFO, "IMU device successfully initialized"); + + // We may customize full scale range here. + ret |= inv_imu_set_accel_fsr(ACCEL_CONFIG0_FS_SEL_4g); + ret |= inv_imu_set_gyro_fsr(GYRO_CONFIG0_FS_SEL_2000dps); + + // Below settings are required to configure and enable sensors. + ret |= inv_imu_acc_enable(); + ret |= inv_imu_acc_set_rate(SENSOR_ODR_400HZ, 2, &hw_rate); //200Hz ODR, watermark: 2 packets. + ret |= inv_imu_gyro_enable(); + ret |= inv_imu_gyro_set_rate(SENSOR_ODR_400HZ, 4, &hw_rate); //50Hz ODR, watermark 4. + + if (ret != 0) { + INV_LOG(SENSOR_LOG_LEVEL, "Feature enable Failed. Do nothing %d", ret); + return ret; + } + INV_LOG(SENSOR_LOG_LEVEL, "HW odr setting %f ", hw_rate); + + return ESP_OK; +} + +esp_err_t icm42670_get_raw_data(AccDataPacket *accData, + GyroDataPacket *gyroData, + chip_temperature *imu_chip_temperature) +{ + int ret = inv_data_handler(accData, gyroData, imu_chip_temperature, 1); + if (0 != ret) { + return ESP_FAIL; + } + return ESP_OK; +} diff --git a/components/i2c_devices/icm42670/include/InvExport.h b/components/i2c_devices/icm42670/include/InvExport.h new file mode 100644 index 0000000..a1a48c8 --- /dev/null +++ b/components/i2c_devices/icm42670/include/InvExport.h @@ -0,0 +1,39 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively “Software”) is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +#ifndef _INV_IDD_EXPORT_H_ +#define _INV_IDD_EXPORT_H_ + +#if defined(_WIN32) + #if !defined(INV_EXPORT) && defined(INV_DO_DLL_EXPORT) + #define INV_EXPORT __declspec(dllexport) + #elif !defined(INV_EXPORT) && defined(INV_DO_DLL_IMPORT) + #define INV_EXPORT __declspec(dllimport) + #endif +#endif + +#if !defined(INV_EXPORT) + #define INV_EXPORT +#endif + +#endif /* _INV_IDD_EXPORT_H_ */ diff --git a/components/i2c_devices/icm42670/include/Message.h b/components/i2c_devices/icm42670/include/Message.h new file mode 100644 index 0000000..4315ec1 --- /dev/null +++ b/components/i2c_devices/icm42670/include/Message.h @@ -0,0 +1,181 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively “Software”) is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +/** @defgroup Message Message + * @brief Utility functions to display and redirect diagnostic messages + * + * Use INV_MSG_DISABLE or INV_MSG_ENABLE define before including + * this header to enable/disable messages for a compilation unit. + * + * Under Linux, Windows or Arduino, messages are enabled by default. + * Use INV_MSG_DISABLE to disable them. + * + * Under orther environmment, message are disabled by default. + * Use INV_MSG_ENABLE to disable them. + * + * @ingroup EmbUtils + * @{ + */ + +#ifndef _INV_MESSAGE_H_ +#define _INV_MESSAGE_H_ + +#include "InvExport.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/** @brief For eMD target, disable log by default + * If compile switch is set for a compilation unit + * messages will be totally disabled by default + */ +#if !defined(__linux) && !defined(_WIN32) && !defined(ARDUINO) + #define INV_MSG_DISABLE 1 +#endif + +#define INV_MSG_ENABLE + +/** @brief Allow to force enabling messaging using INV_MSG_ENABLE define */ +#ifdef INV_MSG_ENABLE + #undef INV_MSG_DISABLE +#endif + + +/** @brief Helper macro for calling inv_msg() + * If INV_MSG_DISABLE compile switch is set for a compilation unit + * messages will be totally disabled + */ +#define INV_MSG(level, ...) _INV_MSG(level, __VA_ARGS__) + +/** @brief Helper macro for calling inv_msg_setup() + * If INV_MSG_DISABLE compile switch is set for a compilation unit + * messages will be totally disabled + */ +#define INV_MSG_SETUP(level, printer) _INV_MSG_SETUP(level, printer) + +/** @brief Helper macro for calling inv_msg_setup_level() + * If INV_MSG_DISABLE compile switch is set for a compilation unit + * messages will be totally disabled + */ +#define INV_MSG_SETUP_LEVEL(level) _INV_MSG_SETUP_LEVEL(level) + +/** @brief Helper macro for calling inv_msg_setup_default() + * If INV_MSG_DISABLE compile switch is set for a compilation unit + * messages will be totally disabled + */ +#define INV_MSG_SETUP_DEFAULT() _INV_MSG_SETUP_DEFAULT() + +/** @brief Return current level + * @warning This macro may expand as a function call + */ +#define INV_MSG_LEVEL _INV_MSG_LEVEL + +#if defined(INV_MSG_DISABLE) + #define _INV_MSG(level, ...) (void)0 + #define _INV_MSG_SETUP(level, printer) (void)0 + #define _INV_MSG_SETUP_LEVEL(level) (void)0 + #define _INV_MSG_LEVEL INV_MSG_LEVEL_OFF +#else + #define _INV_MSG(level, ...) inv_msg(level, __VA_ARGS__) + #define _INV_MSG_SETUP(level, printer) inv_msg_setup(level, printer) + #define _INV_MSG_SETUP_LEVEL(level) inv_msg_setup(level, inv_msg_printer_default) + #define _INV_MSG_SETUP_DEFAULT() inv_msg_setup_default() + #define _INV_MSG_LEVEL inv_msg_get_level() +#endif + +/** @brief message level definition + */ +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 +}; + + +/** @brief Prototype for print routine function + */ +typedef void (*inv_msg_printer_t)(int level, const char * str, va_list ap); + + +/** @brief Set message level and printer function + * @param[in] level only message above level will be passed to printer function + * @param[in] printer user provided function in charge printing message + * @return none + */ +void INV_EXPORT inv_msg_setup(int level, inv_msg_printer_t printer); + + +/** @brief Default printer function that display messages to stderr + * Function uses stdio. Care must be taken on embeded platfrom. + * Function does nothing with IAR compiler. + * @return none + */ +void INV_EXPORT inv_msg_printer_default(int level, const char * str, va_list ap); + +/** @brief Set message level + * Default printer function will be used. + * @param[in] level only message above level will be passed to printer function + * @return none + */ +static inline void inv_msg_setup_level(int level) +{ + inv_msg_setup(level, inv_msg_printer_default); +} + + +/** @brief Set default message level and printer + * @return none + */ +static inline void inv_msg_setup_default(void) +{ + inv_msg_setup(INV_MSG_LEVEL_INFO, inv_msg_printer_default); +} + +/** @brief Return current message level + * @return current message level + */ +int INV_EXPORT inv_msg_get_level(void); + +/** @brief Display a message (through means of printer function) + * @param[in] level for the message + * @param[in] str message string + * @param[in] ... optional arguments + * @return none + */ +void INV_EXPORT inv_msg(int level, const char * str, ...); + + +#ifdef __cplusplus +} +#endif + +#endif /* INV_MESSAGE_H_ */ + +/** @} */ diff --git a/components/i2c_devices/icm42670/include/icm42670.h b/components/i2c_devices/icm42670/include/icm42670.h new file mode 100644 index 0000000..22f1664 --- /dev/null +++ b/components/i2c_devices/icm42670/include/icm42670.h @@ -0,0 +1,41 @@ + +#ifndef _ICM_42670_H_ +#define _ICM_42670_H_ + +#include "esp_err.h" +#include "inv_imu_driver.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Init ICM-42670 IMU + * + * @return + * - ESP_OK: Success + * - Others: Fail + */ +esp_err_t icm42670_init(void); + +/** + * @brief Get raw data from ICM-42670 + * + * @param accData Pointer to accelerometer data + * @param gyroData Pointor to gyroscope data + * @param imu_chip_temperature Pointer to temperature data + * @return + * - ESP_OK: Success + * - Others: Fail + */ +esp_err_t icm42670_get_raw_data(AccDataPacket *accData, + GyroDataPacket *gyroData, + chip_temperature *imu_chip_temperature); + + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/components/i2c_devices/icm42670/include/inv_imu_cust_config.h b/components/i2c_devices/icm42670/include/inv_imu_cust_config.h new file mode 100644 index 0000000..da32ccd --- /dev/null +++ b/components/i2c_devices/icm42670/include/inv_imu_cust_config.h @@ -0,0 +1,104 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively Software is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ +#ifndef _INV_ICM4N607_CONFIG_H_ +#define _INV_ICM4N607_CONFIG_H_ + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif // __cplusplus + +#define FIFO_WM_MODE_EN 0 //1:fifo mode 0:dri mode +#define IS_HIGH_RES_MODE 0 // 1: FIFO high resolution mode (20bits data format); 0: 16 bits data format; +#define SPI_MODE_EN 1 //1:spi 0:i2c + +#define DATA_FORMAT_DPS_G 1 // output sensor data with unit in default accel: m/s2 gyro: rad/s + +#define SUPPORT_FIFO_TS 1 // support fifo timestamp by default +#define DMP_POWERSAVE_FF 0 // disable apex power save mode for apex freefall for better performance +#define DMP_POWERSAVE_PEDO 1 // setting apex power save mode for apex pedometer by default + +#define SUPPORT_DELAY_US 1 // 1: platform supports delay in us; 0: platform does not support delay in ms. + +#define SENSOR_REG_DUMP 0 // register dump control +#define SENSOR_LOG_LEVEL 3 //INV_LOG_LEVEL_INFO +#define SENSOR_LOG_TS_ONLY 1 //only print log sensor data TS , FIFO Timestampe is only available in fifo mode + + +#define SENSOR_DIRECTION 0 // 0~7 sensorConvert map index below +/* { { 1, 1, 1}, {0, 1, 2} }, + { { -1, 1, 1}, {1, 0, 2} }, + { { -1, -1, 1}, {0, 1, 2} }, + { { 1, -1, 1}, {1, 0, 2} }, + { { -1, 1, -1}, {0, 1, 2} }, + { { 1, 1, -1}, {1, 0, 2} }, + { { 1, -1, -1}, {0, 1, 2} }, + { { -1, -1, -1}, {1, 0, 2} }, */ + +// For Xian the max FIFO size is 2048 Bytes; common FIFO package is 16 Bytes; so the max number of FIFO pakage is ~128. +#define MAX_RECV_PACKET 128 + + +/* Initial WOM threshold to be applied to IMU in mg */ +#define WOM_THRESHOLD_INITIAL_MG 200 + +/* WOM threshold to be applied to IMU, ranges from 1 to 255, in 4mg unit */ +#define WOM_THRESHOLD WOM_THRESHOLD_INITIAL_MG/3 +#define WOM_THRESHOLD_X WOM_THRESHOLD +#define WOM_THRESHOLD_Y WOM_THRESHOLD +#define WOM_THRESHOLD_Z WOM_THRESHOLD + + +#if 0 +/* customer board, need invoke system marco*/ +#define INV_LOG //define INV_LOG(loglevel,fmt,arg...) printf("ICM4n607: "fmt"\n",##arg) +//#define INV_LOG(loglevel,fmt, args...) pr_err("INV icm4n607 "fmt"\n",##args) + +//#define INV_MSG(loglevel,fmt, args...) pr_err("INV icm4n607 "fmt"\n",##args) +#define EXIT(a) ; +#else +/* smart motion board*/ +#include "Message.h" +#define INV_LOG INV_MSG +#define EXIT(a) exit(a) +#endif + +/** @brief enumeration of serial interfaces available on IMU */ +typedef enum +{ + UI_I2C, + UI_SPI4, + UI_SPI3 +} SERIAL_IF_TYPE_t; + +#ifdef __cplusplus +} +#endif // __cplusplus + +#endif diff --git a/components/i2c_devices/icm42670/include/inv_imu_driver.h b/components/i2c_devices/icm42670/include/inv_imu_driver.h new file mode 100644 index 0000000..6694502 --- /dev/null +++ b/components/i2c_devices/icm42670/include/inv_imu_driver.h @@ -0,0 +1,2097 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively Software is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ +#ifndef _INV_ICM4N607_H_ +#define _INV_ICM4N607_H_ + +#include +#include +#include +#include +#include "inv_imu_cust_config.h" + + +#ifdef __cplusplus +extern "C" { +#endif // __cplusplus + + +/* List whoami values for all icm436xx variants*/ +#define ICM43607_WHOAMI 0x3F +#define ICM42607_WHOAMI 0x60 +#define ICM42670_WHOAMI 0x67 + +#define T1000_WHOAMI 0x30 + +/* Register Map BANK0 */ +#define MCLK_RDY 0x0000 + +typedef struct{ + uint8_t otp_done : 1; + uint8_t resv_02 : 2; + uint8_t mclk_rdy : 1; + uint8_t resv_01 : 4; +} mclk_rdy_t; + +#define DEVICE_CONFIG_REG 0x0001 + +typedef struct{ + uint8_t spi_mode : 1; + uint8_t resv_01 : 1; + uint8_t spi_ap_4wire : 1; + uint8_t resv_02 : 5; +} device_config_t; + +#define SIGNAL_PATH_RESET 0x0002 + +typedef struct{ + uint8_t resv_01 : 2; + uint8_t fifo_flush : 1; + uint8_t resv_02 : 1; + uint8_t soft_reset_device_config : 1; + uint8_t resv_03 : 3; +} signal_path_reset_t; + +#define DRIVE_CONFIG_REG1 0x0003 + +typedef struct{ + uint8_t i3c_sdr_slew_rate : 3; + uint8_t i3c_ddr_slew_rate : 3; + uint8_t resv_01 : 2; +} drive_config1_t; + +#define DRIVE_CONFIG_REG2 0x0004 + +typedef struct{ + uint8_t all_slew_rate : 3; + uint8_t i2c_slew_rate : 3; + uint8_t resv_01 : 2; +} drive_config2_t; + + +#define DRIVE_CONFIG_REG3 0x0005 + +typedef struct{ + uint8_t SPI_slew_rate : 3; + uint8_t resv_01 : 5; +} drive_config3_t; + +#define INT_CONFIG_REG 0x0006 + +typedef struct{ + uint8_t int1_polarity : 1; + uint8_t int1_drive_circuit : 1; + uint8_t int1_mode : 1; + uint8_t int2_polarity : 1; + uint8_t int2_drive_circuit : 1; + uint8_t int2_mode : 1; + uint8_t resv_01 : 2; +} int_config_t; + +#define TEMP_DATA1 0x0009 +#define TEMP_DATA0 0x000a +#define ACCEL_DATA_X1 0x000b +#define ACCEL_DATA_X0 0x000c +#define ACCEL_DATA_Y1 0x000d +#define ACCEL_DATA_Y0 0x000e +#define ACCEL_DATA_Z1 0x000f +#define ACCEL_DATA_Z0 0x0010 +#define GYRO_DATA_X1 0x0011 +#define GYRO_DATA_X0 0x0012 +#define GYRO_DATA_Y1 0x0013 +#define GYRO_DATA_Y0 0x0014 +#define GYRO_DATA_Z1 0x0015 +#define GYRO_DATA_Z0 0x0016 +#define TMST_FSYNCH 0x0017 +#define TMST_FSYNCL 0x0018 +#define APEX_DATA4 0x001d +#define APEX_DATA5 0x001e + +#define PWR_MGMT_0 0x001f + +typedef struct{ + uint8_t accel_mode : 2; + uint8_t gyro_mode : 2; + uint8_t idle : 1; + uint8_t resv_01 : 2; + uint8_t accel_lp_clk_sel : 1; +} pwr_mgmt0_t; + +typedef union{ + uint8_t data; + pwr_mgmt0_t byte; +}PWR_MGMT0_T; + +#define GYRO_CONFIG0 0x0020 + +typedef struct{ + uint8_t gyro_odr : 4; + uint8_t resv01 : 1; + uint8_t gyro_ui_fs_sel : 2; + uint8_t resv02 : 1; +} gyro_config0_t; + +#define ACCEL_CONFIG0 0x0021 + +typedef struct{ + uint8_t accel_odr : 4; + uint8_t bit4_resv : 1; + uint8_t accel_ui_fs_sel : 2; + uint8_t bit7_resv : 1; +} accel_config0_t; + +#define TEMP_CONFIG0 0x0022 + +typedef struct{ + uint8_t resv_01 : 4; + uint8_t temp_filt_bw : 3; + uint8_t resv_02 : 1; +} temp_config0_t; + + +#define GYRO_CONFIG1 0x0023 + +typedef struct{ + uint8_t gyro_ui_filt_bw : 3; + uint8_t resv_01 : 5; +} gyro_config1_t; + + +#define ACCEL_CONFIG1 0x0024 + +typedef struct{ + uint8_t accel_ui_filt_bw : 3; + uint8_t resv_01 : 1; + uint8_t accel_ui_avg : 3; + uint8_t resv_02 : 1; +} accel_config1_t; + + +#define APEX_CONFIG0 0x0025 + +typedef struct{ + uint8_t dmp_mem_reset_en : 1; + uint8_t resv_01 : 1; + uint8_t dmp_init_en : 1; + uint8_t dmp_power_save_en : 1; + uint8_t resv_02 : 4; +} apex_config0_t; + +#define APEX_CONFIG1 0x0026 + +typedef struct{ + uint8_t dmp_odr : 2; + uint8_t resv_01 : 1; + uint8_t ped_en : 1; + uint8_t tilt_en : 1; + uint8_t ff_en : 1; + uint8_t smd_en : 1; + uint8_t resv_02 : 1; +} apex_config1_t; + +#define WOM_CONFIG 0x0027 + +typedef struct{ + uint8_t wom_en : 1; + uint8_t wom_mode : 1; + uint8_t wom_int_mode : 1; + uint8_t wom_int_dur : 2; + uint8_t resv_01 : 3; +} wom_config_t; + +#define FIFO_CONFIG1 0x0028 + +typedef struct{ + uint8_t fifo_bypass : 1; + uint8_t fifo_mode : 1; + uint8_t resv_01 : 6; +} fifo_config1_t; + + +#define FIFO_CONFIG2 0x0029 + +typedef struct{ + uint8_t fifo_wm7_0 : 8; +} fifo_config2_t; + +#define FIFO_CONFIG3 0x002a + +typedef struct{ + uint8_t fifo_wm11_8 : 4; + uint8_t resv_01 : 4; +} fifo_config3_t; + +#define INT_SOURCE0 0x002b + +typedef struct{ + uint8_t agc_rdy_int1_en : 1; + uint8_t fifo_full_int1_en : 1; + uint8_t fifo_ths_int1_en : 1; + uint8_t drdy_int1_en : 1; + uint8_t reset_done_int1_en : 1; + uint8_t pll_rdy_int1_en : 1; + uint8_t fsync_int1_en : 1; + uint8_t st_int1_en : 1; +} int_source0_t; + +#define INT_SOURCE1 0x002c + +typedef struct{ + uint8_t wom_x_int1_en : 1; + uint8_t wom_y_int1_en : 1; + uint8_t wom_z_int1_en : 1; + uint8_t smd_int1_en : 1; + uint8_t resv_01 : 2; + uint8_t i3c_protocol_error_int1_en : 1; + uint8_t resv_02 : 1; +} int_source1_t; + +#define INT_SOURCE3 0x002d + +typedef struct{ + uint8_t agc_rdy_int2_en : 1; + uint8_t fifo_full_int2_en : 1; + uint8_t fifo_ths_int2_en : 1; + uint8_t drdy_int2_en : 1; + uint8_t reset_done_int2_en : 1; + uint8_t pll_rdy_int2_en : 1; + uint8_t fsync_int2_en : 1; + uint8_t st_int2_en : 1; +} int_source3_t; + +#define INT_SOURCE4 0x002e + +typedef struct{ + uint8_t wom_x_int2_en : 1; + uint8_t wom_y_int2_en : 1; + uint8_t wom_z_int2_en : 1; + uint8_t smd_int2_en : 1; + uint8_t resv_01 : 2; + uint8_t i3c_protocol_error_int2_en: 1; + uint8_t resv_02 : 1; +} int_sourc4_t; + +#define FIFO_LOST_PKT0 0x002f +#define FIFO_LOST_PKT1 0x0030 +#define APEX_DATA0 0x0031 +#define APEX_DATA1 0x0032 +#define APEX_DATA2 0x0033 + +#define APEX_DATA3 0x0034 + +typedef struct{ + uint8_t activity_class : 2; + uint8_t dmp_idle : 1; + uint8_t resv_01 : 5; +} apex_data3_t; + +#define INTF_CONFIG0 0x0035 + +typedef struct{ + uint8_t ui_sifs_cfg : 2; + uint8_t resv_01 : 2; + uint8_t sensor_data_endian : 1; + uint8_t fifo_count_endian : 1; + uint8_t fifo_count_format : 1; + uint8_t resv_02 : 1; +} intf_config0_t; + +#define INTF_CONFIG1 0x0036 + +typedef struct{ + uint8_t clksel : 2; + uint8_t i3c_ddr_en : 1; + uint8_t i3c_sdr_en : 1; + uint8_t resv_01 : 4; +} intf_config1_t; + +#define INT_STATUS_DRDY 0x0039 + +typedef struct{ + uint8_t data_rdy_int : 1; + uint8_t resv_01 : 7; +} int_status_drdy_t; + + +#define INT_STATUS 0x003a + +typedef struct{ + uint8_t agc_rdy_int : 1; + uint8_t fifo_full_int : 1; + uint8_t fifo_ths_int : 1; + uint8_t resv_01 : 1; + uint8_t reset_done_int : 1; + uint8_t pll_rdy_int : 1; + uint8_t fsync_int : 1; + uint8_t st_int : 1; +} int_status_t; + +#define INT_STATUS2 0x003b + +typedef struct{ + uint8_t wom_z_int : 1; + uint8_t wom_y_int : 1; + uint8_t wom_x_int : 1; + uint8_t smd_int : 1; + uint8_t resv_01 : 4; +} int_status2_t; + +#define INT_STATUS3 0x003c + +typedef struct{ + uint8_t resv_01 : 1; + uint8_t lg_det_int : 1; + uint8_t ff_det_int : 1; + uint8_t tilt_det_int : 1; + uint8_t step_cnt_ovf_int : 1; + uint8_t step_det_int : 1; + uint8_t resv_02 : 2; +} int_status3_t; + +#define FIFO_COUNTH 0x003d +#define FIFO_COUNTL 0x003e +#define FIFO_DATA 0x003f +#define WHO_AM_I 0x0075 +#define BLK_SEL_W 0x0079 +#define MADDR_W 0x007a +#define M_W 0x007b +#define BLK_SEL_R 0x007c +#define MADDR_R 0x007d +#define M_R 0x007e + +/*end of common reg */ + +/* MMEM_SIGP */ + +/* MREG_TOP1 */ +#define TMST_CONFIG1_MREG 0x1000 + +typedef struct{ + uint8_t tmst_en : 1; + uint8_t tmst_fsyn_en : 1; + uint8_t tmst_delta_en : 1; + uint8_t tmst_res : 1; + uint8_t tmst_on_sreg_en : 1; + uint8_t resv_01 : 3; +} tmst_config1_t; + +#define FIFO_CONFIG5_MREG 0x1001 + +typedef struct{ + uint8_t fifo_accel_en : 1; + uint8_t fifo_gyro_en : 1; + uint8_t fifo_tmst_fsync_en : 1; + uint8_t fifo_hires_en : 1; + uint8_t fifo_resume_partial_rd : 1; + uint8_t fifo_wm_gt_th : 1; + uint8_t resv_01 : 2; +} fifo_config5_t; + +#define FIFO_CONFIG6_MREG 0x1002 +typedef struct{ + uint8_t rcosc_req_on_fifo_ths_dis : 1; + uint8_t resv_01 : 3; + uint8_t fifo_empty_indicator_dis : 1; + uint8_t resv_02 : 3; +}fifo_config6_t; + +#define FSYNC_CONFIG_MREG 0x1003 +typedef struct{ + uint8_t fsync_polarity : 1; + uint8_t fsync_ui_flag_clear_sel : 1; + uint8_t resv_01 : 2; + uint8_t fsync_ui_sel : 3; + uint8_t resv_02 : 1; +}fsync_config_t; + + +#define INT_CONFIG0_MREG 0x1004 + +#define INT_CONFIG1_MREG 0x1005 +typedef struct{ + uint8_t resv_01 : 4; + uint8_t int_async_reset : 1; + uint8_t resv_02 : 1; + uint8_t int_tpulse_duration : 1; + uint8_t resv_03 : 1; +}int_config1_t; + +#define SENSOR_CONFIG3 0x1006 +typedef struct{ + uint8_t resv_01 : 6; + uint8_t apex_disable : 1; + uint8_t resv_02 : 1; +}sensor_config3_t; + +#define ST_CONFIG_MREG 0x1013 + +typedef struct{ + uint8_t gyro_st_lim : 3; + uint8_t accel_st_lim : 3; + uint8_t st_num_sample : 1; + uint8_t resv_01 : 1; +} st_config_t; + +#define SELFTEST_MREG 0x1014 + +typedef struct{ + uint8_t resv_01 : 6; + uint8_t accel_st_en : 1; + uint8_t gyro_st_en : 1; +} selftest_t; + +#define INTF_CONFIG6_MREG 0x1023 +#define INTF_CONFIG10_MREG 0x1025 +#define INTF_CONFIG7_MREG 0x1028 + +#define OTP_CONFIG 0x102b + +typedef struct{ + uint8_t resv_01 : 2; + uint8_t otp_copy_mode : 2; + uint8_t resv_02 : 4; +} otp_config_t; + + +#define INT_SOURCE6_MREG 0x102f + +typedef struct{ + uint8_t resv_01 : 3; + uint8_t tilt_det_int1_en : 1; + uint8_t step_cnt_ovfl_int1_en : 1; + uint8_t step_det_int1_en : 1; + uint8_t lowg_int1_en : 1; + uint8_t ff_int1_en : 1; +} int_source6_t; + +#define INT_SOURCE7_MREG 0x1030 + +typedef struct{ + uint8_t resv_01 : 3; + uint8_t tilt_det_int2_en : 1; + uint8_t step_cnt_ovfl_int2_en : 1; + uint8_t step_det_int2_en : 1; + uint8_t lowg_int2_en : 1; + uint8_t ff_int2_en : 1; +} int_source7_t; + +#define INT_SOURCE8_MREG 0x1031 +#define INT_SOURCE9_MREG 0x1032 +#define INT_SOURCE10_MREG 0x1033 + +#define APEX_CONFIG2_MREG 0x1044 + +typedef struct{ + uint8_t dmp_power_save_time_sel : 4; + uint8_t low_energy_amp_th_sel : 4; +} apex_config2_t; + +#define APEX_CONFIG3_MREG 0x1045 + +typedef struct{ + uint8_t pedo_step_cnt_th_sel : 4; + uint8_t pedo_amp_th_sel : 4; +} apex_config3_t; + +#define APEX_CONFIG4_MREG 0x1046 + +typedef struct{ + uint8_t pedo_hi_enrgy_th_sel : 2; + uint8_t pedo_sb_timer_th_sel : 3; + uint8_t pedo_step_det_th_sel : 3; +} apex_config4_t; + +#define APEX_CONFIG5_MREG 0x1047 + +typedef struct{ + uint8_t highg_peak_th_hyst_sel : 3; + uint8_t lowg_peak_th_hyst_sel : 3; + uint8_t tilt_wait_time_sel : 2; +} apex_config5_t; + +#define APEX_CONFIG9_MREG 0x1048 + +typedef struct{ + uint8_t sensitivity_mode : 1; + uint8_t smd_sensitivity_sel : 3; + uint8_t ff_debounce_duration_sel : 4; +} apex_config9_t; + +#define APEX_CONFIG10_MREG 0x1049 + +typedef struct{ + uint8_t lowg_time_th_sel : 3; + uint8_t lowg_peak_th_sel : 5; +} apex_config10_t; + +#define APEX_CONFIG11_MREG 0x104a + +typedef struct{ + uint8_t highg_time_th_sel : 3; + uint8_t highg_peak_th_sel : 5; +} apex_config11_t; + +#define ACCEL_WOM_X_THR_MREG 0x104b +#define ACCEL_WOM_Y_THR_MREG 0x104c +#define ACCEL_WOM_Z_THR_MREG 0x104d +#define OFFSET_USER0 0x104e +#define OFFSET_USER1 0x104f +#define OFFSET_USER2 0x1050 +#define OFFSET_USER3 0x1051 +#define OFFSET_USER4 0x1052 +#define OFFSET_USER5 0x1053 +#define OFFSET_USER6 0x1054 +#define OFFSET_USER7 0x1055 +#define OFFSET_USER8 0x1056 + +#define ST_STATUS1_MREG 0x1063 +#define FDR_CONFIG_MREG 0x1066 + + +typedef struct{ + uint8_t resv_01 : 1; + uint8_t ax_st_pass : 1; + uint8_t ay_st_pass : 1; + uint8_t az_st_pass : 1; + uint8_t accel_st_done : 1; + uint8_t accel_st_pass : 1; + uint8_t resv_02 : 2; +} st_status1_t; + +#define ST_STATUS2_MREG 0x1064 + +typedef struct{ + uint8_t resv_01 : 1; + uint8_t gx_st_pass : 1; + uint8_t gy_st_pass : 1; + uint8_t gz_st_pass : 1; + uint8_t gyro_st_done : 1; + uint8_t gyro_st_pass : 1; + uint8_t st_incomplete : 1; + uint8_t resv_02 : 1; +} st_status2_t; + + +#define APEX_CONFIG12_MREG 0x1067 + +typedef struct{ + uint8_t ff_min_duration_sel : 4; + uint8_t ff_max_duration_sel : 4; +}apex_config12_t; + + +/* MMEM_TOP */ +#define XA_ST_DATA_MMEM 0x5000 +#define YA_ST_DATA_MMEM 0x5001 +#define ZA_ST_DATA_MMEM 0x5002 +#define XG_ST_DATA_MMEM 0x5003 +#define YG_ST_DATA_MMEM 0x5004 +#define ZG_ST_DATA_MMEM 0x5005 + +/* MREG_OTP */ +/* Bank MREG2 */ +#define OTP_CTRL7 0x2806 + +typedef struct{ + uint8_t otp_soak : 1; + uint8_t otp_pwr_down : 1; + uint8_t otp_low_pwr_mode : 1; + uint8_t otp_reload : 1; + uint8_t otp_boot : 1; + uint8_t otp_stress : 1; + uint8_t otp_reset : 1; + uint8_t pgm_dly_cfg : 1; +} otp_ctrl7_t; + + + +/* --------------------------------------------------------------------------- + * register bank 0 + * ---------------------------------------------------------------------------- */ + +/* + * MISC_1 (MCLK_RDY register in datasheet) + * Register Name : MISC_1 (MCLK_RDY) + */ + +/* + * mclk_rdy + * 1 to indicate internal clock is currently running. + * 0 to indicate internal clock is not currently running. + */ + typedef enum { + MCLK_RDY_YES = (1 ), + MCLK_RDY_NO = (0 ), +} MCLK_RDY_t; + +/* + * otp_done + * 1 to indicate OTP_copy operation is done. + */ + typedef enum { + OTP_DONE_YES = (1 ), + OTP_DONE_NO = (0 ), +} OTP_DONE_t; + + +/* + * DEVICE_CONFIG + * Register Name : DEVICE_CONFIG + */ + +/* SPI_MODE */ +typedef enum +{ + DEVICE_CONFIG_SPI_MODE_1_2 = (0x1 ), + DEVICE_CONFIG_SPI_MODE_0_3 = (0x0 ), +} DEVICE_CONFIG_SPI_MODE_t; + +/* SPI_AP_4WIRE */ +typedef enum +{ + DEVICE_CONFIG_SPI_AP_4WIRE = (0x1 ), + DEVICE_CONFIG_SPI_AP_3WIRE = (0x0 ), +} DEVICE_CONFIG_SPI_AP_4WIRE_t; + + +/* + * SIGNAL_PATH_RESET + * Register Name: SIGNAL_PATH_RESET + */ +/* SOFT_RESET_DEVICE_CONFIG */ +typedef enum +{ + SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_EN = (0x01 ), + SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_DIS = (0x00 ), +} SIGNAL_PATH_RESET_SOFT_RESET_DEVICE_CONFIG_t; + +/* + * FIFO_FLUSH + * When set to 1, FIFO will get flushed. FIFO flush requires special sequence of programming the registers. Please see use notes for details + * Host can only program this register bit to 1. */ +typedef enum +{ + SIGNAL_PATH_RESET_FIFO_FLUSH_EN = (0x01 ), + SIGNAL_PATH_RESET_FIFO_FLUSH_DIS = (0x00 ), +} SIGNAL_PATH_RESET_FIFO_FLUSH_t; + + +/* + * FIFO_CONFIG1 + * Register Name: FIFO_CONFIG + */ + +/* FIFO_BYPASS */ +typedef enum +{ + FIFO_CONFIG1_FIFO_BYPASS_ON = (0x01 ), + FIFO_CONFIG1_FIFO_BYPASS_OFF = (0x00 ), +} FIFO_CONFIG1_FIFO_BYPASS_t; + +typedef enum +{ + FIFO_CONFIG1_FIFO_MODE_SNAPSHOT = (0x01 ), + FIFO_CONFIG1_FIFO_MODE_STREAM = (0x00 ), +} FIFO_CONFIG1_FIFO_MODE_t; + +/** Contains Pedometer output */ +typedef struct APEX_DATA_STEP_ACTIVITY { + uint16_t step_cnt; /**< Number of steps taken */ + uint8_t step_cadence; /**< Walk/run cadency in number of samples. + Format is u6.2. E.g, At 50Hz and 2Hz walk frequency, if the cadency is 25 samples. The register will output 100. */ + uint8_t activity_class; /**< Detected activity unknown (0), walk (1) or run (2) */ +} APEX_DATA_STEP_ACTIVITY_t; + +/* ACTIVITY_CLASS */ +typedef enum +{ + APEX_DATA3_ACTIVITY_CLASS_OTHER = 0x0, + APEX_DATA3_ACTIVITY_CLASS_WALK = 0x1, + APEX_DATA3_ACTIVITY_CLASS_RUN = 0x2, +} APEX_DATA3_ACTIVITY_CLASS_t; + +/* DMP_IDLE */ +typedef enum +{ + APEX_DATA3_DMP_IDLE_ON = (0x01 ), + APEX_DATA3_DMP_IDLE_OFF = (0x00 ), +} APEX_DATA3_DMP_IDLE_OFF_t; + +/* + * INTF_CONFIG0 + * Register Name: INTF_CONFIG0 + */ + +/* FIFO_SREG_INVALID_IND */ +typedef enum +{ + INTF_CONFIG0_FIFO_SREG_INVALID_IND_DIS = (0x01 ), + INTF_CONFIG0_FIFO_SREG_INVALID_IND_EN = (0x00 ), +} INTF_CONFIG0_FIFO_SREG_INVALID_IND_t; + +/* FIFO_COUNT_FORMAT */ +typedef enum +{ + INTF_CONFIG0_FIFO_COUNT_FORMAT_RECORD = (0x01 ), + INTF_CONFIG0_FIFO_COUNT_FORMAT_BYTE = (0x00 ), +} INTF_CONFIG0_FIFO_COUNT_FORMAT_t; + +/* FIFO_COUNT_ENDIAN */ +typedef enum +{ + INTF_CONFIG0_FIFO_COUNT_BIG_ENDIAN = (0x01 ), + INTF_CONFIG0_FIFO_COUNT_LITTLE_ENDIAN = (0x00 ), +} INTF_CONFIG0_FIFO_COUNT_ENDIAN_t; + +/* SENSOR_DATA_ENDIAN */ +typedef enum +{ + INTF_CONFIG0_SENSOR_DATA_BIG_ENDIAN = (0x01 ), + INTF_CONFIG0_SENSOR_DATA_LITTLE_ENDIAN = (0x00 ), +} INTF_CONFIG0_SENSOR_DATA_ENDIAN_t; + +/* UI_SIFS_CFG */ +typedef enum +{ + INTF_CONFIG0_UI_SIFS_CFG_DISABLE_SPI = (0x02 ), + INTF_CONFIG0_UI_SIFS_CFG_DISABLE_I2C = (0x03 ), +} INTF_CONFIG0_UI_SIFS_CFG_t; + + +/* + * INTF_CONFIG1 + * Register Name: INTF_CONFIG1 + */ + +/* I3C_SDR_EN + * Enable I3C SDR mode. Chip will be in pure I2C mode if {i3c_sdr_en, i3c_ddr_en} = 00. + */ +typedef enum +{ + INTF_CONFIG1_I3C_SDR_EN = (0x01 ), + INTF_CONFIG1_I3C_SDR_DIS = (0x00 ), +} INTF_CONFIG1_I3C_SDR_EN_t; + +/* I3C_DDR_EN + * Enable I3C DDR mode. This bit won't take effect unless i3c_sdr_en = 1. + */ +typedef enum +{ + INTF_CONFIG1_I3C_DDR_EN = (0x01 ), + INTF_CONFIG1_I3C_DDR_DIS = (0x00 ), +} INTF_CONFIG1_I3C_DDR_EN_t; + +/* I3C_CLKSEL */ +typedef enum +{ + INTF_CONFIG1_CLKSEL_RCOSC = (0x00 ), + INTF_CONFIG1_CLKSEL_PLL_RCOSC = (0x01 ), + INTF_CONFIG1_CLKSEL_DIS_ALL = (0x03 ), +} INTF_CONFIG1_CLKSEL_t; + + +/* + * INT_STATUS_DRDY + * Register Name : INT_STATUS_DRDY + * This is a clear on read register. + * DRDY_DATA_REG (Accel / Gyro) + */ +typedef enum +{ + INT_STATUS_DRDY_DATA_RDY_INT_GENERATED = (0x01 ), + INT_STATUS_DRDY_DATA_RDY_INT_CLEARED = (0x00 ), +} INT_STATUS_DRDY_DATA_RDY_INT_t; + + +/* + * INT_STATUS + * Register Name : INT_STATUS + */ + +/* + * int_status_st_done + * Self-Test Done + */ +typedef enum +{ + INT_STATUS_ST_INT_GENERATED = (0x01 ), + INT_STATUS_ST_INT_CLEARED = (0x00 ), +} INT_STATUS_ST_INT_t; + +/* + * int_status_fsync_int + * FSYNC_INT + */ +typedef enum +{ + INT_STATUS_FSYNC_INT_GENERATED = (0x01 ), + INT_STATUS_FSYNC_INT_CLEARED = (0x00 ), +} INT_STATUS_FSYNC_INT_t; + +/* + * int_status_pll_rdy_int + * PLL_RDY_INT + */ +typedef enum +{ + INT_STATUS_PLL_RDY_INT_GENERATED = (0x01 ), + INT_STATUS_PLL_RDY_INT_CLEARED = (0x00 ), +} INT_STATUS_PLL_RDY_INT_t; + +/* + * int_status_reset_done_int + * This is a clear on read register. + * POR complete and Software reset complete + */ +typedef enum +{ + INT_STATUS_RESET_DONE_INT_GENERATED = (0x01 ), + INT_STATUS_RESET_DONE_INT_CLEARED = (0x00 ), +} INT_STATUS_RESET_DONE_INT_t; + +/* + * int_status_fifo_ths_int + * This is a clear on read register. + * FIFO Threshold + */ +typedef enum +{ + INT_STATUS_FIFO_THS_INT_GENERATED = (0x01 ), + INT_STATUS_FIFO_THS_INT_CLEARED = (0x00 ), +} INT_STATUS_FIFO_THS_INT_t; + +/* + * int_status_fifo_full_int + * This is a clear on read register. + * FIFO Full + */ +typedef enum +{ + INT_STATUS_FIFO_FULL_INT_GENERATED = (0x01 ), + INT_STATUS_FIFO_FULL_INT_CLEARED = (0x00 ), +} INT_STATUS_FIFO_FULL_INT_t; + +/* + * int_status_agc_rdy_int + * This is a clear on read register. + * Gyro drive is ready + */ +typedef enum +{ + INT_STATUS_AGC_RDY_INT_GENERATED = (0x01 ), + INT_STATUS_AGC_RDY_INT_CLEARED = (0x00 ), +} INT_STATUS_AGC_RDY_INT_t; + + +/* INT_STATUS2 */ + +/* + * int_status_smd_int + * SMD Interrupt clears on read + */ +typedef enum +{ + INT_STATUS2_SMD_INT_GENERATED = (0x01 ), + INT_STATUS2_SMD_INT_CLEARED = (0x00 ), +} INT_STATUS2_SMD_INT_t; + +/* + * int_status_wom_x + * WOM Interrupt on X axis, clears on read + */ +typedef enum +{ + INT_STATUS2_WOM_X_INT_GENERATED = (0x01 ), + INT_STATUS2_WOM_X_INT_CLEARED = (0x00 ), +} INT_STATUS2_WOM_X_INT_t; + +/* + * int_status_wom_y + * WOM Interrupt on Y axis, clears on read + */ +typedef enum +{ + INT_STATUS2_WOM_Y_INT_GENERATED = (0x01 ), + INT_STATUS2_WOM_Y_INT_CLEARED = (0x00 ), +} INT_STATUS2_WOM_Y_INT_t; + +/* + * int_status_wom_z + * WOM interrupt on Z axis , clears on read + */ +typedef enum +{ + INT_STATUS2_WOM_Z_INT_GENERATED = (0x01 ), + INT_STATUS2_WOM_Z_INT_CLEARED = (0x00 ), +} INT_STATUS2_WOM_Z_INT_t; + + +/* + * PWR_MGMT_0 + * Register Name: PWR_MGMT_0 + */ + +/* ACCEL_LP_CLK_SEL */ +typedef enum +{ + PWR_MGMT_0_ACCEL_LP_CLK_WUOSC = (0x00 ), + PWR_MGMT_0_ACCEL_LP_CLK_RCOSC = (0x01 ), +} PWR_MGMT_0_ACCEL_LP_CLK_t; + +/* IDLE */ +typedef enum +{ + PWR_MGMT_0_IDLE_RCOSC_ON = (0x01 ), + PWR_MGMT_0_IDLE_RCOSC_OFF = (0x00 ), +} PWR_MGMT_0_IDLE_t; + +/* GYRO_MODE */ +typedef enum +{ + PWR_MGMT_0_GYRO_MODE_LN = (0x03 ), + PWR_MGMT_0_GYRO_MODE_LP = (0x02 ), + PWR_MGMT_0_GYRO_MODE_STANDBY = (0x01 ), + PWR_MGMT_0_GYRO_MODE_OFF = (0x00 ), +} PWR_MGMT_0_GYRO_MODE_t; + +/* ACCEL_MODE */ +typedef enum +{ + PWR_MGMT_0_ACCEL_MODE_LN = 0x03, + PWR_MGMT_0_ACCEL_MODE_LP = 0x02, + PWR_MGMT_0_ACCEL_MODE_OFF = 0x00, +} PWR_MGMT_0_ACCEL_MODE_t; + + +/* + * GYRO_CONFIG0 + * Register Name: GYRO_CONFIG0 + */ + +/* GYRO_FS_SEL*/ + +/** @brief Gyroscope FSR selection + */ +typedef enum +{ +#if defined(ICM43688) + GYRO_CONFIG0_FS_SEL_500dps = (3 ), /*!< 500dps*/ + GYRO_CONFIG0_FS_SEL_1000dps = (2 ), /*!< 1000dps*/ + GYRO_CONFIG0_FS_SEL_2000dps = (1 ), /*!< 2000dps*/ + GYRO_CONFIG0_FS_SEL_4000dps = (0 ), /*!< 4000dps*/ +#else + GYRO_CONFIG0_FS_SEL_250dps = (3 ), /*!< 250dps*/ + GYRO_CONFIG0_FS_SEL_500dps = (2 ), /*!< 500dps*/ + GYRO_CONFIG0_FS_SEL_1000dps = (1 ), /*!< 1000dps*/ + GYRO_CONFIG0_FS_SEL_2000dps = (0 ), /*!< 2000dps*/ +#endif +} GYRO_CONFIG0_FS_SEL_t; + +/* GYRO_ODR */ + +/** @brief Gyroscope ODR selection + */ +typedef enum +{ + GYRO_CONFIG0_ODR_1_5625_HZ = 0xF, /*!< 1.5625 Hz (640 ms)*/ + GYRO_CONFIG0_ODR_3_125_HZ = 0xE, /*!< 3.125 Hz (320 ms)*/ + GYRO_CONFIG0_ODR_6_25_HZ = 0xD, /*!< 6.25 Hz (160 ms)*/ + GYRO_CONFIG0_ODR_12_5_HZ = 0xC, /*!< 12.5 Hz (80 ms)*/ + GYRO_CONFIG0_ODR_25_HZ = 0xB, /*!< 25 Hz (40 ms)*/ + GYRO_CONFIG0_ODR_50_HZ = 0xA, /*!< 50 Hz (20 ms)*/ + GYRO_CONFIG0_ODR_100_HZ = 0x9, /*!< 100 Hz (10 ms)*/ + GYRO_CONFIG0_ODR_200_HZ = 0x8, /*!< 200 Hz (5 ms)*/ + GYRO_CONFIG0_ODR_400_HZ = 0x7, /*!< 400 Hz (2.5 ms)*/ + GYRO_CONFIG0_ODR_800_HZ = 0x6, /*!< 800 Hz (1.25 ms)*/ + GYRO_CONFIG0_ODR_1600_HZ = 0x5, /*!< 1.6 KHz (625 us)*/ +} GYRO_CONFIG0_ODR_t; + +/* + * ACCEL_CONFIG0 + * Register Name: ACCEL_CONFIG0 + */ + +/* ACCEL_FS_SEL */ + +/** @brief Accelerometer FSR selection + */ +typedef enum +{ +#if defined(ICM43688) + ACCEL_CONFIG0_FS_SEL_4g = (0x3 ), /*!< 4g*/ + ACCEL_CONFIG0_FS_SEL_8g = (0x2 ), /*!< 8g*/ + ACCEL_CONFIG0_FS_SEL_16g = (0x1 ), /*!< 16g*/ + ACCEL_CONFIG0_FS_SEL_32g = (0x0 ), /*!< 32g*/ +#else + ACCEL_CONFIG0_FS_SEL_2g = (0x3 ), /*!< 2g*/ + ACCEL_CONFIG0_FS_SEL_4g = (0x2 ), /*!< 4g*/ + ACCEL_CONFIG0_FS_SEL_8g = (0x1 ), /*!< 8g*/ + ACCEL_CONFIG0_FS_SEL_16g = (0x0 ), /*!< 16g*/ +#endif +} ACCEL_CONFIG0_FS_SEL_t; + +/* ACCEL_ODR */ + +/** @brief Accelerometer ODR selection + */ +typedef enum +{ + ACCEL_CONFIG0_ODR_1_5625_HZ = 0xF, /*!< 1.5625 Hz (640 ms)*/ + ACCEL_CONFIG0_ODR_3_125_HZ = 0xE, /*!< 3.125 Hz (320 ms)*/ + ACCEL_CONFIG0_ODR_6_25_HZ = 0xD, /*!< 6.25 Hz (160 ms)*/ + ACCEL_CONFIG0_ODR_12_5_HZ = 0xC, /*!< 12.5 Hz (80 ms)*/ + ACCEL_CONFIG0_ODR_25_HZ = 0xB, /*!< 25 Hz (40 ms)*/ + ACCEL_CONFIG0_ODR_50_HZ = 0xA, /*!< 50 Hz (20 ms)*/ + ACCEL_CONFIG0_ODR_100_HZ = 0x9, /*!< 100 Hz (10 ms)*/ + ACCEL_CONFIG0_ODR_200_HZ = 0x8, /*!< 200 Hz (5 ms)*/ + ACCEL_CONFIG0_ODR_400_HZ = 0x7, /*!< 400 Hz (2.5 ms)*/ + ACCEL_CONFIG0_ODR_800_HZ = 0x6, /*!< 800 Hz (1.25 ms)*/ + ACCEL_CONFIG0_ODR_1600_HZ = 0x5, /*!< 1.6 KHz (625 us)*/ +} ACCEL_CONFIG0_ODR_t; + +/* + * GYRO_CONFIG1 + * Register Name: GYRO_CONFIG1 + */ + +/* GYRO_UI_FILT_BW_IND */ +typedef enum +{ + GYRO_CONFIG1_GYRO_FILT_BW_16 = (0x07 ), + GYRO_CONFIG1_GYRO_FILT_BW_25 = (0x06 ), + GYRO_CONFIG1_GYRO_FILT_BW_34 = (0x05 ), + GYRO_CONFIG1_GYRO_FILT_BW_53 = (0x04 ), + GYRO_CONFIG1_GYRO_FILT_BW_73 = (0x03 ), + GYRO_CONFIG1_GYRO_FILT_BW_121 = (0x02 ), + GYRO_CONFIG1_GYRO_FILT_BW_180 = (0x01 ), + GYRO_CONFIG1_GYRO_FILT_BW_NO_FILTER = (0x00 ), +} GYRO_CONFIG1_GYRO_FILT_BW_t; + +/* GYRO_UI_AVG_IND */ +typedef enum +{ + GYRO_CONFIG1_GYRO_FILT_AVG_64 = (0x05 ), + GYRO_CONFIG1_GYRO_FILT_AVG_32 = (0x04 ), + GYRO_CONFIG1_GYRO_FILT_AVG_16 = (0x03 ), + GYRO_CONFIG1_GYRO_FILT_AVG_8 = (0x02 ), + GYRO_CONFIG1_GYRO_FILT_AVG_4 = (0x01 ), + GYRO_CONFIG1_GYRO_FILT_AVG_2 = (0x00 ), +} GYRO_CONFIG1_GYRO_FILT_AVG_t; + +/* + * ACCEL_CONFIG1 + * Register Name: ACCEL_CONFIG1 + */ + +/* ACCEL_UI_FILT_BW_IND */ +typedef enum +{ + ACCEL_CONFIG1_ACCEL_FILT_BW_16 = (0x7 ), + ACCEL_CONFIG1_ACCEL_FILT_BW_25 = (0x6 ), + ACCEL_CONFIG1_ACCEL_FILT_BW_34 = (0x5 ), + ACCEL_CONFIG1_ACCEL_FILT_BW_53 = (0x4 ), + ACCEL_CONFIG1_ACCEL_FILT_BW_73 = (0x3 ), + ACCEL_CONFIG1_ACCEL_FILT_BW_121 = (0x2 ), + ACCEL_CONFIG1_ACCEL_FILT_BW_180 = (0x1 ), + ACCEL_CONFIG1_ACCEL_FILT_BW_NO_FILTER = (0x0 ), +} ACCEL_CONFIG1_ACCEL_FILT_BW_t; + +/* ACCEL_UI_AVG_IND */ +typedef enum +{ + ACCEL_CONFIG1_ACCEL_FILT_AVG_64 = (0x5 ), + ACCEL_CONFIG1_ACCEL_FILT_AVG_32 = (0x4 ), + ACCEL_CONFIG1_ACCEL_FILT_AVG_16 = (0x3 ), + ACCEL_CONFIG1_ACCEL_FILT_AVG_8 = (0x2 ), + ACCEL_CONFIG1_ACCEL_FILT_AVG_4 = (0x1 ), + ACCEL_CONFIG1_ACCEL_FILT_AVG_2 = (0x0 ), +} ACCEL_CONFIG1_ACCEL_FILT_AVG_t; + +/* + * GYRO_CONFIG1 + * Register Name: GYRO_CONFIG1 + */ + + +/* + * APEX_CONFIG0 + * Register Name: APEX_CONFIG0 + */ +/* + * DMP_MEM_RESET_EN + * enable DMP SRAM to be cleared to 0. + * [0]: 1 to clear DMP SRAM for APEX operation or Self-test operation. + * [1]: 1 to clear DMP SRAM for secure Authentication operation. Secure authetication is not supported in Xian so this bit will be un-used. + * This register field is clearred to 0 after SRAM contents are cleared. + */ +typedef enum +{ + APEX_CONFIG0_DMP_MEM_RESET_EN = (0x01 ), + APEX_CONFIG0_DMP_MEM_RESET_DIS = (0x00 ), +} APEX_CONFIG0_DMP_MEM_RESET_EN_t; + +/* DMP_INIT_EN + * Enable Init algorithm. It can only be set to 1. Once this bit is set, the algorithm must be executed in the next ODR with highest priority than the other features. + * The readback value is the internal signal resynchronized bit that once falling back to zero indicates that the algorithm has finished. + * This field can be changed on-the-fly even if accel sensor is already on. + */ +typedef enum +{ + APEX_CONFIG0_DMP_INIT_EN = (0x01 ), + APEX_CONFIG0_DMP_INIT_DIS = (0x00 ), +} APEX_CONFIG0_DMP_INIT_t; + +/* DMP_POWER_SAVE_EN + * Enable Power Saving for DMP algorithms. DMP block have 16-bit power_save_en. This control bit will be connected to LSB bit of dmp_power_save_en which dm_power_save_en[0] + */ + typedef enum +{ + APEX_CONFIG0_DMP_POWER_SAVE_EN = (0x1 ), + APEX_CONFIG0_DMP_POWER_SAVE_DIS = (0x0 ), +} APEX_CONFIG0_DMP_POWER_SAVE_t; + + +/* + * APEX_CONFIG1 + * Register Name: APEX_CONFIG1 + */ + +/* PEDO_EN */ +typedef enum +{ + APEX_CONFIG1_PEDO_EN_EN = (0x1 ), + APEX_CONFIG1_PEDO_EN_DIS = (0x0 ), +} APEX_CONFIG1_PEDO_EN_t; + +/* TILT_EN */ +typedef enum +{ + APEX_CONFIG1_TILT_EN_EN = (0x1 ), + APEX_CONFIG1_TILT_EN_DIS = (0x0 ), +} APEX_CONFIG1_TILT_EN_t; + +/* DMP_ODR */ +/** @brief DMP ODR selection + */ +typedef enum +{ + APEX_CONFIG1_DMP_ODR_25Hz = (0x0 ), /**< 25Hz (40ms) */ + APEX_CONFIG1_DMP_ODR_50Hz = (0x2 ), /**< 50Hz (20ms) */ + APEX_CONFIG1_DMP_ODR_100Hz = (0x3 ), /**< 100Hz (10ms) */ + APEX_CONFIG1_DMP_ODR_400Hz = (0x1 ), /**< 400Hz (2.5ms) */ +} APEX_CONFIG1_DMP_ODR_t; + +/* DMP_FF_EN */ +typedef enum +{ + APEX_CONFIG1_DMP_FF_DIS = (0x00 ), + APEX_CONFIG1_DMP_FF_EN = (0x01 ), +} APEX_CONFIG1_DMP_FF_EN_t; + +/* DMP_SMD_EN */ +typedef enum +{ + APEX_CONFIG1_DMP_SMD_DIS = (0x00 ), + APEX_CONFIG1_DMP_SMD_EN = (0x01 ) +} APEX_CONFIG1_DMP_SMD_EN_t; + + +/* + * WOM_CONFIG + * Register Name: WOM_CONFIG + */ + +/* WOM_INT_DUR */ +typedef enum +{ + WOM_CONFIG_WOM_INT_DUR_1_SMPL = (0x00 ), + WOM_CONFIG_WOM_INT_DUR_2_SMPL = (0x01 ), + WOM_CONFIG_WOM_INT_DUR_3_SMPL = (0x02 ), + WOM_CONFIG_WOM_INT_DUR_4_SMPL = (0x03 ), +} WOM_CONFIG_WOM_INT_DUR_t; + +/* WOM_INT_MODE */ +typedef enum +{ + WOM_CONFIG_WOM_INT_MODE_ANDED = (0x01 ), + WOM_CONFIG_WOM_INT_MODE_ORED = (0x00 ), +} WOM_CONFIG_WOM_INT_MODE_t; + +/* WOM_MODE */ +typedef enum +{ + WOM_CONFIG_WOM_MODE_CMP_PREV = (0x01 ), + WOM_CONFIG_WOM_MODE_CMP_INIT = (0x00 ), +} WOM_CONFIG_WOM_MODE_t; + +/* WOM_ENABLE */ +typedef enum +{ + WOM_CONFIG_WOM_EN_ENABLE = (0x01 ), + WOM_CONFIG_WOM_EN_DISABLE = (0x00 ), +} WOM_CONFIG_WOM_EN_t; + + +/* ---------------------------------------------------------------------------- + * Register bank 1 (MREG1) + * ---------------------------------------------------------------------------- */ +/* + * TMST_CONFIG1 + * Register Name: TMST_CONFIG1 + */ + +/* TMST_TO_REGS */ +typedef enum +{ + TMST_CONFIG1_TMST_ON_SREG_EN = (0x1 ), + TMST_CONFIG1_TMST_ON_SREG_DIS = (0x0 ), +} TMST_CONFIG1_TMST_ON_SREG_EN_t; + +/* TMST_RES */ +typedef enum +{ + TMST_CONFIG1_RESOL_16us = (0x01 ), + TMST_CONFIG1_RESOL_1us = (0x00 ), +} TMST_CONFIG1_RESOL_t; + +/* TMST_FSYNC */ +typedef enum +{ + TMST_CONFIG1_TMST_FSYNC_EN = (0x01 ), + TMST_CONFIG1_TMST_FSYNC_DIS = (0x00 ), +} TMST_CONFIG1_TMST_FSYNC_EN_t; + +/* TMST_EN */ +typedef enum +{ + TMST_CONFIG1_TMST_EN = 0x01, + TMST_CONFIG1_TMST_DIS = 0x00, +} TMST_CONFIG1_TMST_EN_t; + + +/* + * FIFO_CONFIG5 + * Register Name: FIFO_CONFIG5 + */ +/* FIFO_WM_GT_TH */ +typedef enum +{ + FIFO_CONFIG5_WM_GT_TH_EN = (0x1 ), + FIFO_CONFIG5_WM_GT_TH_DIS = (0x0 ), +} FIFO_CONFIG5_WM_GT_t; + +/* FIFO_HIRES_EN */ +typedef enum +{ + FIFO_CONFIG5_HIRES_EN = (0x1 ), + FIFO_CONFIG5_HIRES_DIS = (0x0 ), +} FIFO_CONFIG5_HIRES_t; + +/* FIFO_TMST_FSYNC_EN */ +typedef enum +{ + FIFO_CONFIG5_TMST_FSYNC_EN = (0x1 ), + FIFO_CONFIG5_TMST_FSYNC_DIS = (0x0 ), +} FIFO_CONFIG5_TMST_FSYNC_t; + +/* FIFO_GYRO_EN */ +typedef enum +{ + FIFO_CONFIG5_GYRO_EN = (0x1 ), + FIFO_CONFIG5_GYRO_DIS = (0x0 ), +} FIFO_CONFIG5_GYRO_t; + +/* FIFO_ACCEL_EN*/ +typedef enum +{ + FIFO_CONFIG5_ACCEL_EN = 0x01, + FIFO_CONFIG5_ACCEL_DIS = 0x00, +} FIFO_CONFIG5_ACCEL_t; + +/* + * FIFO_CONFIG6 + */ + +/* + * rcosc_req_on_fifo_ths_dis + * 0: when FIFO is operating in ALP mode and the watermark interrupt is enabled, FIFO wakes up the system oscillator as soon as the watermark level is reached. The oscillator remains enabled until a Host FIFO read operation happens. As side effect un extra power consumption could be seen in LP mode when the RCOSC is expected to be off. + * 1: RCOSC is not automatically requested by the FIFO/INT circuit when the WM interrupt is triggered. As side effect the host can receive invalid packets until RCOSC is off. + */ +typedef enum +{ + FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_EN = (0x00), + FIFO_CONFIG6_RCOSC_REQ_ON_FIFO_THS_DIS = (0x01), +} FIFO_CONFIG6_RCOSC_REQ_t; + + +/* + * APEX_CONFIG2 + * Register Name: APEX_CONFIG2 +*/ + +/* DMP_POWER_SAVE_TIME_SEL */ +typedef enum +{ + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_0S = 0x0, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_4S = 0x1, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_8S = 0x2, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_12S = 0x3, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_16S = 0x4, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_20S = 0x5, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_24S = 0x6, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_28S = 0x7, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_32S = 0x8, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_36S = 0x9, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_40S = 0xA, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_44S = 0xB, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_48S = 0xC, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_52S = 0xD, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_56S = 0xE, + APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_60S = 0xF + +} APEX_CONFIG2_DMP_POWER_SAVE_TIME_t; + +/* LOW_ENERGY_AMP_TH_SEL */ +typedef enum +{ + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_1006632MG = (0 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_1174405MG = (1 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_1342177MG = (2 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_1509949MG = (3 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_1677721MG = (4 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_1845493MG = (5 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_2013265MG = (6 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_2181038MG = (7 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_2348810MG = (8 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_2516582MG = (9 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_2684354MG = (10 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_2852126MG = (11 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_3019898MG = (12 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_3187671MG = (13 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_3355443MG = (14 ), + APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_3523215MG = (15 ) +} APEX_CONFIG2_LOW_ENERGY_AMP_TH_t; + +/* + * APEX_CONFIG3 + * Register Name: APEX_CONFIG3 +*/ +/* PEDO_AMP_TH_SEL */ +typedef enum +{ + APEX_CONFIG3_PEDO_AMP_TH_1006632_MG = (0 ), + APEX_CONFIG3_PEDO_AMP_TH_1140850_MG = (1 ), + APEX_CONFIG3_PEDO_AMP_TH_1275068_MG = (2 ), + APEX_CONFIG3_PEDO_AMP_TH_1409286_MG = (3 ), + APEX_CONFIG3_PEDO_AMP_TH_1543503_MG = (4 ), + APEX_CONFIG3_PEDO_AMP_TH_1677721_MG = (5 ), + APEX_CONFIG3_PEDO_AMP_TH_1811939_MG = (6 ), + APEX_CONFIG3_PEDO_AMP_TH_1946157_MG = (7 ), + APEX_CONFIG3_PEDO_AMP_TH_2080374_MG = (8 ), + APEX_CONFIG3_PEDO_AMP_TH_2214592_MG = (9 ), + APEX_CONFIG3_PEDO_AMP_TH_2348810_MG = (10 ), + APEX_CONFIG3_PEDO_AMP_TH_2483027_MG = (11 ), + APEX_CONFIG3_PEDO_AMP_TH_2617245_MG = (12 ), + APEX_CONFIG3_PEDO_AMP_TH_2751463_MG = (13 ), + APEX_CONFIG3_PEDO_AMP_TH_2885681_MG = (14 ), + APEX_CONFIG3_PEDO_AMP_TH_3019898_MG = (15 ) +} APEX_CONFIG3_PEDO_AMP_TH_t; + +/* + * APEX_CONFIG4 + * Register Name: APEX_CONFIG4 +*/ + +/* PEDO_SB_TIMER_TH_SEL */ +typedef enum +{ + APEX_CONFIG4_PEDO_SB_TIMER_TH_50_SAMPLES = (0 ), + APEX_CONFIG4_PEDO_SB_TIMER_TH_75_SAMPLES = (1 ), + APEX_CONFIG4_PEDO_SB_TIMER_TH_100_SAMPLES = (2 ), + APEX_CONFIG4_PEDO_SB_TIMER_TH_125_SAMPLES = (3 ), + APEX_CONFIG4_PEDO_SB_TIMER_TH_150_SAMPLES = (4 ), + APEX_CONFIG4_PEDO_SB_TIMER_TH_175_SAMPLES = (5 ), + APEX_CONFIG4_PEDO_SB_TIMER_TH_200_SAMPLES = (6 ), + APEX_CONFIG4_PEDO_SB_TIMER_TH_225_SAMPLES = (7 ) +} APEX_CONFIG4_PEDO_SB_TIMER_TH_t; + + +/* PEDO_HI_ENRGY_TH_SEL */ +typedef enum +{ + APEX_CONFIG4_PEDO_HI_ENRGY_TH_90 = (0 ), + APEX_CONFIG4_PEDO_HI_ENRGY_TH_107 = (1 ), + APEX_CONFIG4_PEDO_HI_ENRGY_TH_136 = (2 ), + APEX_CONFIG4_PEDO_HI_ENRGY_TH_159 = (3 ) +} APEX_CONFIG4_PEDO_HI_ENRGY_TH_t; + + +/* + * APEX_CONFIG5 + * Register Name: APEX_CONFIG5 +*/ +/* TILT_WAIT_TIME_SEL */ +typedef enum +{ + APEX_CONFIG5_TILT_WAIT_TIME_0S = (0 ), + APEX_CONFIG5_TILT_WAIT_TIME_2S = (1 ), + APEX_CONFIG5_TILT_WAIT_TIME_4S = (2 ), + APEX_CONFIG5_TILT_WAIT_TIME_6S = (3 ) +} APEX_CONFIG5_TILT_WAIT_TIME_t; + +/* LOWG_PEAK_TH_HYST_SEL */ +typedef enum +{ + APEX_CONFIG5_LOWG_PEAK_TH_HYST_31MG = (0 ), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_63MG = (1 ), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_94MG = (2 ), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_125MG = (3 ), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_156MG = (4 ), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_188MG = (5 ), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_219MG = (6 ), + APEX_CONFIG5_LOWG_PEAK_TH_HYST_250MG = (7 ) +} APEX_CONFIG5_LOWG_PEAK_TH_HYST_t; + +/* HIGHG_PEAK_TH_HYST_SEL */ +typedef enum +{ + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_31MG = (0 ), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_63MG = (1 ), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_94MG = (2 ), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_125MG = (3 ), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_156MG = (4 ), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_188MG = (5 ), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_219MG = (6 ), + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_250MG = (7 ) +} APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t; + +/* + * APEX_CONFIG9 + * Register Name: APEX_CONFIG9 +*/ + +/* FF_DEBOUNCE_DURATION_SEL */ +typedef enum +{ + APEX_CONFIG9_FF_DEBOUNCE_DURATION_0_MS = (0 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1250_MS = (1 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1375_MS = (2 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1500_MS = (3 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1625_MS = (4 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1750_MS = (5 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_1875_MS = (6 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2000_MS = (7 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2125_MS = (8 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2250_MS = (9 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2375_MS = (10 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2500_MS = (11 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2625_MS = (12 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2750_MS = (13 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_2875_MS = (14 ), + APEX_CONFIG9_FF_DEBOUNCE_DURATION_3000_MS = (15 ) +} APEX_CONFIG9_FF_DEBOUNCE_DURATION_t; + +/* SMD_SENSITIVITY_SEL */ +typedef enum +{ + APEX_CONFIG9_SMD_SENSITIVITY_0 = (0 ), + APEX_CONFIG9_SMD_SENSITIVITY_1 = (1 ), + APEX_CONFIG9_SMD_SENSITIVITY_2 = (2 ), + APEX_CONFIG9_SMD_SENSITIVITY_3 = (3 ), + APEX_CONFIG9_SMD_SENSITIVITY_4 = (4 ) +} APEX_CONFIG9_SMD_SENSITIVITY_t; + +/* SMD_SENSITIVITY_MODE */ +typedef enum +{ + APEX_CONFIG9_SENSITIVITY_MODE_NORMAL = (0 ), + APEX_CONFIG9_SENSITIVITY_MODE_SLOW_WALK = (1 ) +} APEX_CONFIG9_SENSITIVITY_MODE_t; + + +/* + * APEX_CONFIG10_MREG + * Register Name: APEX_CONFIG10 +*/ +typedef enum +{ + APEX_CONFIG10_LOWG_PEAK_TH_31MG = (0x00 ), + APEX_CONFIG10_LOWG_PEAK_TH_63MG = (0x01 ), + APEX_CONFIG10_LOWG_PEAK_TH_94MG = (0x02 ), + APEX_CONFIG10_LOWG_PEAK_TH_125MG = (0x03 ), + APEX_CONFIG10_LOWG_PEAK_TH_156MG = (0x04 ), + APEX_CONFIG10_LOWG_PEAK_TH_188MG = (0x05 ), + APEX_CONFIG10_LOWG_PEAK_TH_219MG = (0x06 ), + APEX_CONFIG10_LOWG_PEAK_TH_250MG = (0x07 ), + APEX_CONFIG10_LOWG_PEAK_TH_281MG = (0x08 ), + APEX_CONFIG10_LOWG_PEAK_TH_313MG = (0x09 ), + APEX_CONFIG10_LOWG_PEAK_TH_344MG = (0x0A ), + APEX_CONFIG10_LOWG_PEAK_TH_375MG = (0x0B ), + APEX_CONFIG10_LOWG_PEAK_TH_406MG = (0x0C ), + APEX_CONFIG10_LOWG_PEAK_TH_438MG = (0x0D ), + APEX_CONFIG10_LOWG_PEAK_TH_469MG = (0x0E ), + APEX_CONFIG10_LOWG_PEAK_TH_500MG = (0x0F ), + APEX_CONFIG10_LOWG_PEAK_TH_531MG = (0x10 ), + APEX_CONFIG10_LOWG_PEAK_TH_563MG = (0x11 ), + APEX_CONFIG10_LOWG_PEAK_TH_594MG = (0x12 ), + APEX_CONFIG10_LOWG_PEAK_TH_625MG = (0x13 ), + APEX_CONFIG10_LOWG_PEAK_TH_656MG = (0x14 ), + APEX_CONFIG10_LOWG_PEAK_TH_688MG = (0x15 ), + APEX_CONFIG10_LOWG_PEAK_TH_719MG = (0x16 ), + APEX_CONFIG10_LOWG_PEAK_TH_750MG = (0x17 ), + APEX_CONFIG10_LOWG_PEAK_TH_781MG = (0x18 ), + APEX_CONFIG10_LOWG_PEAK_TH_813MG = (0x19 ), + APEX_CONFIG10_LOWG_PEAK_TH_844MG = (0x1A ), + APEX_CONFIG10_LOWG_PEAK_TH_875MG = (0x1B ), + APEX_CONFIG10_LOWG_PEAK_TH_906MG = (0x1C ), + APEX_CONFIG10_LOWG_PEAK_TH_938MG = (0x1D ), + APEX_CONFIG10_LOWG_PEAK_TH_969MG = (0x1E ), + APEX_CONFIG10_LOWG_PEAK_TH_1000MG = (0x1F ) +} APEX_CONFIG10_LOWG_PEAK_TH_t; + +typedef enum +{ + APEX_CONFIG10_LOWG_TIME_TH_1_SAMPLE = (0x00 ), + APEX_CONFIG10_LOWG_TIME_TH_2_SAMPLES = (0x01 ), + APEX_CONFIG10_LOWG_TIME_TH_3_SAMPLES = (0x02 ), + APEX_CONFIG10_LOWG_TIME_TH_4_SAMPLES = (0x03 ), + APEX_CONFIG10_LOWG_TIME_TH_5_SAMPLES = (0x04 ), + APEX_CONFIG10_LOWG_TIME_TH_6_SAMPLES = (0x05 ), + APEX_CONFIG10_LOWG_TIME_TH_7_SAMPLES = (0x06 ), + APEX_CONFIG10_LOWG_TIME_TH_8_SAMPLES = (0x07 ) +} APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t; + +/* + * APEX_CONFIG11_MREG + * Register Name: APEX_CONFIG11 +*/ +typedef enum +{ + APEX_CONFIG11_HIGHG_PEAK_TH_250MG = (0x00 ), + APEX_CONFIG11_HIGHG_PEAK_TH_500MG = (0x01 ), + APEX_CONFIG11_HIGHG_PEAK_TH_750MG = (0x02 ), + APEX_CONFIG11_HIGHG_PEAK_TH_1000MG = (0x03 ), + APEX_CONFIG11_HIGHG_PEAK_TH_1250MG = (0x04 ), + APEX_CONFIG11_HIGHG_PEAK_TH_1500MG = (0x05 ), + APEX_CONFIG11_HIGHG_PEAK_TH_1750MG = (0x06 ), + APEX_CONFIG11_HIGHG_PEAK_TH_2000MG = (0x07 ), + APEX_CONFIG11_HIGHG_PEAK_TH_2250MG = (0x08 ), + APEX_CONFIG11_HIGHG_PEAK_TH_2500MG = (0x09 ), + APEX_CONFIG11_HIGHG_PEAK_TH_2750MG = (0x0A ), + APEX_CONFIG11_HIGHG_PEAK_TH_3000MG = (0x0B ), + APEX_CONFIG11_HIGHG_PEAK_TH_3250MG = (0x0C ), + APEX_CONFIG11_HIGHG_PEAK_TH_3500MG = (0x0D ), + APEX_CONFIG11_HIGHG_PEAK_TH_3750MG = (0x0E ), + APEX_CONFIG11_HIGHG_PEAK_TH_4000MG = (0x0F ), + APEX_CONFIG11_HIGHG_PEAK_TH_4250MG = (0x10 ), + APEX_CONFIG11_HIGHG_PEAK_TH_4500MG = (0x11 ), + APEX_CONFIG11_HIGHG_PEAK_TH_4750MG = (0x12 ), + APEX_CONFIG11_HIGHG_PEAK_TH_5000MG = (0x13 ), + APEX_CONFIG11_HIGHG_PEAK_TH_5250MG = (0x14 ), + APEX_CONFIG11_HIGHG_PEAK_TH_5500MG = (0x15 ), + APEX_CONFIG11_HIGHG_PEAK_TH_5750MG = (0x16 ), + APEX_CONFIG11_HIGHG_PEAK_TH_6000MG = (0x17 ), + APEX_CONFIG11_HIGHG_PEAK_TH_6250MG = (0x18 ), + APEX_CONFIG11_HIGHG_PEAK_TH_6500MG = (0x19 ), + APEX_CONFIG11_HIGHG_PEAK_TH_6750MG = (0x1A ), + APEX_CONFIG11_HIGHG_PEAK_TH_7000MG = (0x1B ), + APEX_CONFIG11_HIGHG_PEAK_TH_7250MG = (0x1C ), + APEX_CONFIG11_HIGHG_PEAK_TH_7500MG = (0x1D ), + APEX_CONFIG11_HIGHG_PEAK_TH_7750MG = (0x1E ), + APEX_CONFIG11_HIGHG_PEAK_TH_8000MG = (0x1F ) +} APEX_CONFIG11_HIGHG_PEAK_TH_t; + +typedef enum +{ + APEX_CONFIG11_HIGHG_TIME_TH_1_SAMPLE = (0x00 ), + APEX_CONFIG11_HIGHG_TIME_TH_2_SAMPLES = (0x01 ), + APEX_CONFIG11_HIGHG_TIME_TH_3_SAMPLES = (0x02 ), + APEX_CONFIG11_HIGHG_TIME_TH_4_SAMPLES = (0x03 ), + APEX_CONFIG11_HIGHG_TIME_TH_5_SAMPLES = (0x04 ), + APEX_CONFIG11_HIGHG_TIME_TH_6_SAMPLES = (0x05 ), + APEX_CONFIG11_HIGHG_TIME_TH_7_SAMPLES = (0x06 ), + APEX_CONFIG11_HIGHG_TIME_TH_8_SAMPLES = (0x07 ) +} APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t; + +/* + * APEX_CONFIG12_MREG + * Register Name: APEX_CONFIG12 +*/ +/* FF_MAX_DURATION_SEL */ +typedef enum +{ + APEX_CONFIG12_FF_MAX_DURATION_102_CM = (0 ), + APEX_CONFIG12_FF_MAX_DURATION_120_CM = (1 ), + APEX_CONFIG12_FF_MAX_DURATION_139_CM = (2 ), + APEX_CONFIG12_FF_MAX_DURATION_159_CM = (3 ), + APEX_CONFIG12_FF_MAX_DURATION_181_CM = (4 ), + APEX_CONFIG12_FF_MAX_DURATION_204_CM = (5 ), + APEX_CONFIG12_FF_MAX_DURATION_228_CM = (6 ), + APEX_CONFIG12_FF_MAX_DURATION_254_CM = (7 ), + APEX_CONFIG12_FF_MAX_DURATION_281_CM = (8 ), + APEX_CONFIG12_FF_MAX_DURATION_310_CM = (9 ), + APEX_CONFIG12_FF_MAX_DURATION_339_CM = (10 ), + APEX_CONFIG12_FF_MAX_DURATION_371_CM = (11 ), + APEX_CONFIG12_FF_MAX_DURATION_403_CM = (12 ), + APEX_CONFIG12_FF_MAX_DURATION_438_CM = (13 ), + APEX_CONFIG12_FF_MAX_DURATION_473_CM = (14 ), + APEX_CONFIG12_FF_MAX_DURATION_510_CM = (15 ) +} APEX_CONFIG12_FF_MAX_DURATION_t; + +/* FF_MIN_DURATION_SEL */ +typedef enum +{ + APEX_CONFIG12_FF_MIN_DURATION_10_CM = (0 ), + APEX_CONFIG12_FF_MIN_DURATION_12_CM = (1 ), + APEX_CONFIG12_FF_MIN_DURATION_13_CM = (2 ), + APEX_CONFIG12_FF_MIN_DURATION_16_CM = (3 ), + APEX_CONFIG12_FF_MIN_DURATION_18_CM = (4 ), + APEX_CONFIG12_FF_MIN_DURATION_20_CM = (5 ), + APEX_CONFIG12_FF_MIN_DURATION_23_CM = (6 ), + APEX_CONFIG12_FF_MIN_DURATION_25_CM = (7 ), + APEX_CONFIG12_FF_MIN_DURATION_28_CM = (8 ), + APEX_CONFIG12_FF_MIN_DURATION_31_CM = (9 ), + APEX_CONFIG12_FF_MIN_DURATION_34_CM = (10 ), + APEX_CONFIG12_FF_MIN_DURATION_38_CM = (11 ), + APEX_CONFIG12_FF_MIN_DURATION_41_CM = (12 ), + APEX_CONFIG12_FF_MIN_DURATION_45_CM = (13 ), + APEX_CONFIG12_FF_MIN_DURATION_48_CM = (14 ), + APEX_CONFIG12_FF_MIN_DURATION_52_CM = (15 ) +} APEX_CONFIG12_FF_MIN_DURATION_t; + + +/* + * ST_CONFIG_MREG + * Register Name : ST_CONFIG + */ + +/* + * st_number_sample + * This bit selects the number of sensor samples that should be used to process self-test. + * 0: 16 samples. + * 1: 200 samples. + */ +typedef enum { + ST_CONFIG_ST_NUMBER_SAMPLE_16 = (0 ), + ST_CONFIG_ST_NUMBER_SAMPLE_200 = (1 ), +} ST_CONFIG_ST_NUMBER_SAMPLE_t; + +/* + * accel_st_lim + * These bits control the tolerated ratio between self-test processed values and reference (fused) ones for accelerometer. + * 0 : 5% + * 1: 10% + * 2: 15% + * 3: 20% + * 4: 25% + * 5: 30% + * 6: 40% + * 7: 50% + we only support 50 % value now + */ + +#if 0 +typedef enum { + ST_CONFIG_ACCEL_ST_LIM_5 = (0 ), + ST_CONFIG_ACCEL_ST_LIM_10 = (1 ), + ST_CONFIG_ACCEL_ST_LIM_15 = (2 ), + ST_CONFIG_ACCEL_ST_LIM_20 = (3 ), + ST_CONFIG_ACCEL_ST_LIM_25 = (4 ), + ST_CONFIG_ACCEL_ST_LIM_30 = (5 ), + ST_CONFIG_ACCEL_ST_LIM_40 = (6 ), + ST_CONFIG_ACCEL_ST_LIM_50 = (7 ) +} ST_CONFIG_ACCEL_ST_LIM_t; +#else +#define ST_CONFIG_ACCEL_ST_LIM_50 7 +#endif + +/* + * gyro_st_lim + * These bits control the tolerated ratio between self-test processed values and reference (fused) ones for gyro. + * 0 : 5% + * 1: 10% + * 2: 15% + * 3: 20% + * 4: 25% + * 5: 30% + * 6: 40% + * 7: 50% + we only support 50 % value now + */ + +#if 0 +typedef enum { + ST_CONFIG_GYRO_ST_LIM_5 = (0 ), + ST_CONFIG_GYRO_ST_LIM_10 = (1 ), + ST_CONFIG_GYRO_ST_LIM_15 = (2 ), + ST_CONFIG_GYRO_ST_LIM_20 = (3 ), + ST_CONFIG_GYRO_ST_LIM_25 = (4 ), + ST_CONFIG_GYRO_ST_LIM_30 = (5 ), + ST_CONFIG_GYRO_ST_LIM_40 = (6 ), + ST_CONFIG_GYRO_ST_LIM_50 = (7 ) +} ST_CONFIG_GYRO_ST_LIM_t; +#else +#define ST_CONFIG_GYRO_ST_LIM_50 7 +#endif + +/* + * SELFTEST + * Register Name : SELFTEST + */ +/* + * gyro_st_en + * 1: enably gyro self test operation. Host needs to program this bit to 0 to move chip out of self test mode. If host programs this bit to 0 while st_busy = 1 and st_done =0, the current running self-test operation is terminated by host. + */ + typedef enum { + SELFTEST_GYRO_ST_EN_EN = (1 ), + SELFTEST_GYRO_ST_EN_DIS = (0 ), +} SELFTEST_GYRO_ST_EN_t; + +/* + * accel_st_en + * 1: enably accel self test operation. Host needs to program this bit to 0 to move chip out of self test mode. If host programs this bit to 0 while st_busy = 1 and st_done =0, the current running self-test operation is terminated by host. + */ + typedef enum { + SELFTEST_ACCEL_ST_EN_EN = (1 ), + SELFTEST_ACCEL_ST_EN_DIS = (0 ), +} SELFTEST_ACCEL_ST_EN_t; + + +/* + * ST_COPY_EN + * (OTP_CONFIG_MREG_TOP1, otp_copy_mode ) + * Specify OTP copy mode. + * 0: Reserved + * 1: copy OTP blk 1-n to SRAM, except the self-test related data. + * 2: copy security authentication keys. (Reserved for customer). + * 3: copy self-test data from OTP blk 1-n to SRAM. + */ +typedef enum { + OTP_COPY_EN_TRIM = (0x01 ), + OTP_COPY_EN_DATA = (0x03 ), +} ST_COPY_EN_t; + + +/* + * INT_CONFIG + * Register Name: INT_CONFIG + */ + +/* INT2_DRIVE_CIRCUIT */ +typedef enum +{ + INT_CONFIG_INT2_DRIVE_CIRCUIT_PP = (0x01 ), + INT_CONFIG_INT2_DRIVE_CIRCUIT_OD = (0x00 ), +} INT_CONFIG_INT2_DRIVE_CIRCUIT_t; + + +/* INT2_POLARITY */ +typedef enum +{ + INT_CONFIG_INT2_POLARITY_HIGH = (0x01 ), + INT_CONFIG_INT2_POLARITY_LOW = (0x00 ), +} INT_CONFIG_INT2_POLARITY_t; + + +/* INT1_DRIVE_CIRCUIT */ +typedef enum +{ + INT_CONFIG_INT1_DRIVE_CIRCUIT_PP = (0x01 ), + INT_CONFIG_INT1_DRIVE_CIRCUIT_OD = (0x00 ), +} INT_CONFIG_INT1_DRIVE_CIRCUIT_t; + + +/* INT1_POLARITY */ +typedef enum +{ + INT_CONFIG_INT1_POLARITY_HIGH = 0x01, + INT_CONFIG_INT1_POLARITY_LOW = 0x00, +} INT_CONFIG_INT1_POLARITY_t; + + +/* INT1_POLARITY */ +typedef enum +{ + INT_CONFIG_INT1_LATCH = 0x01, + INT_CONFIG_INT1_PULSE = 0x00, +} INT_CONFIG_INT1_MODE_t; + +/** @brief Configure Fifo usage + */ + +#define INV_IMU_FIFO_DISABLED 0 /**< Fifo is disabled and data source is sensors registers */ +#define INV_IMU_FIFO_ENABLED 1 /**< Fifo is used as data source */ + + + +/* ---------------------------------------------------------------------------- + * Register bank 2 + * MREG2 + * MREG_OTP + * ---------------------------------------------------------------------------- */ + +/* TRIGGER_ST_COPY + * OTP_CTRL7 + * 1: Triggers self-test data copy from OTP to SRAM; ST_COPY_EN must be set to b11 (0x03) to use this bit + */ +typedef enum +{ + OTP_CTRL7_RELOAD_EN = 0x01, + OTP_CTRL7_RELOAD_DIS = 0x00, +} OTP_CTRL7_RELOAD_t; + +/* OTP_CTRL7_PWR_DOWN + * 0: Power up OTP to copy from OTP to SRAM + * 1: Power down OTP + */ +typedef enum +{ + OTP_CTRL7_PWR_DOWN = 0x01, + OTP_CTRL7_PWR_UP = 0x00, +} OTP_CTRL7_PWR_DOWN_t; + +/* + * activity_class + * The walk/run activity classification is also an output in pedometer. This 2-bit register report the current user activity as following: + * 0: unknown activity as default output; + * 1: walk + * 2: run (the cadence is faster than a predefined threshold). + */ +#define APEX_DATA3_ACTIVITY_CLASS_POS 0x00 +#define APEX_DATA3_ACTIVITY_CLASS_MASK 0x03 + + +/* ---------------------------------------------------------------------------- */ + + +/** @brief IMU APEX inputs parameters definition + */ +typedef struct { + APEX_CONFIG3_PEDO_AMP_TH_t pedo_amp_th; /**< Peak threshold value to be considered as a valid step (mg) */ + uint8_t pedo_step_cnt_th; /**< Minimum number of steps that must be detected + before the pedometer step count begins incrementing */ + uint8_t pedo_step_det_th; /**< Minimum number of low latency steps that must be detected + before the pedometer step count begins incrementing */ + APEX_CONFIG4_PEDO_SB_TIMER_TH_t pedo_sb_timer_th; /**< Duration of non-walk to exit the current walk mode, + pedo_step_cnt_th number of steps must again be detected + before step count starts to increase */ + APEX_CONFIG4_PEDO_HI_ENRGY_TH_t pedo_hi_enrgy_th; /**< Threshold to improve run detection if not steps are counted while running */ + + APEX_CONFIG5_TILT_WAIT_TIME_t tilt_wait_time; /**< Number of accelerometer samples to wait before triggering tilt event */ + + APEX_CONFIG2_DMP_POWER_SAVE_TIME_t power_save_time; /**< The time after which DMP goes in power save mode according to the DMP ODR configured */ + + APEX_CONFIG9_SENSITIVITY_MODE_t sensitivity_mode; /**< Sensitivity mode Normal(0) or Slow walk(1). The Slow walk mode improve + the slow walk detection (<1Hz) but in return the number of false detection + might be increase. */ + APEX_CONFIG2_LOW_ENERGY_AMP_TH_t low_energy_amp_th; /**< Peak threshold value to be considered as a valid step (mg) in Slow walk mode */ + + APEX_CONFIG9_SMD_SENSITIVITY_t smd_sensitivity; /**< SMD algorithm resilience to false detection in rejection use case. + Note that a higher value will reject more transport situation */ + + APEX_CONFIG9_FF_DEBOUNCE_DURATION_t ff_debounce_duration; /**< Duration(us) during which LowG and HighG events are not taken into account after an HighG event. + The goal is to avoid detecting bounces as free falls */ + + APEX_CONFIG12_FF_MAX_DURATION_t ff_max_duration_cm; /**< Distance (cm) max crossed by the device after a LowG event, below which the detection of an HighG event triggers a freefall interrupt */ + APEX_CONFIG12_FF_MIN_DURATION_t ff_min_duration_cm; /**< Distance (cm) min crossed by the device after a LowG event, above which the detection of an HighG event triggers a freefall interrupt */ + + APEX_CONFIG10_LOWG_PEAK_TH_t lowg_peak_th; /**< Absolute low peak threshold (mg) to detect when it falls below on any of the accelerometer axis */ + APEX_CONFIG5_LOWG_PEAK_TH_HYST_t lowg_peak_hyst; /**< Hysteresis threshold (mg) added to the threshold after the initial threshold is met */ + APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t lowg_samples_th; /**< Time in number of samples to stay below the threshold before triggering the event (samples) */ + + APEX_CONFIG11_HIGHG_PEAK_TH_t highg_peak_th; /**< Absolute high peak threshold to detect when it goes above on any of the accelerometer axis */ + APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t highg_peak_hyst; /**< Hysteresis threshold (mg) substracted to the threshold after the initial threshold is met */ + APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t highg_samples_th; /**< Time in number of samples to stay above the threshold before triggering the event (samples) */ + +} inv_imu_apex_parameters_t; + +/** @brief APEX pedometer outputs + */ +typedef struct inv_imu_apex_step_activity { + uint16_t step_cnt; /**< Number of steps taken */ + uint8_t step_cadence; /**< Walk/run cadency in number of samples. + Format is u6.2. E.g, At 50Hz and 2Hz walk frequency, if the cadency is 25 samples. The register will output 100. */ + uint8_t activity_class; /**< Detected activity unknown (0), walk (1) or run (2) */ +} inv_imu_apex_step_activity_t; + + +/** @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 occured at transport level */ + INV_ERROR_TIMEOUT = -4, /**< action did not complete in the expected + time window */ + INV_ERROR_SIZE = -5, /**< size/length of given arguments 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 requestion 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 */ +}; + + +enum inv_log_level { + INV_LOG_LEVEL_OFF = 0, + INV_LOG_LEVEL_ERROR, + INV_LOG_LEVEL_WARNING, + INV_LOG_LEVEL_INFO, + INV_LOG_LEVEL_MAX +}; + + +typedef enum { + ACC = 0, + GYR, + TEMP, + WOM, + PEDO, + FF, + TS, + NUM_OF_SENSOR, +} SensorType_t; + + +struct accGyroData { + uint8_t sensType; + int16_t x, y, z; + int16_t temperature; + int8_t high_res[3]; + uint64_t timeStamp; +}; + +struct SensorData { +#if DATA_FORMAT_DPS_G + float x, y, z; +#else + int32_t x, y, z; +#endif + uint64_t timeStamp; +}; + +#if DATA_FORMAT_DPS_G +typedef float chip_temperature; + +#else +typedef int16_t chip_temperature; +#endif + +struct accGyroDataPacket { + uint8_t accOutSize; + uint8_t gyroOutSize; + uint64_t timeStamp; + float temperature; + struct accGyroData outBuf[MAX_RECV_PACKET*2]; + uint32_t magicNum; +}; + +typedef struct { + uint8_t accDataSize; + uint64_t timeStamp; + struct SensorData databuff[MAX_RECV_PACKET]; +}AccDataPacket; + +typedef struct { + uint8_t gyroDataSize; + uint64_t timeStamp; + struct SensorData databuff[MAX_RECV_PACKET]; +}GyroDataPacket; + +typedef struct inv_imu_selftest_output { + int8_t accel_status; /**< global accelerometer self-test passed */ + int8_t gyro_status; /**< global gyroscope self-test status: st_pass (bit0), st_incomplete (bit1) */ + +} inv_imu_selftest_output_t; + + +int inv_imu_initialize(void); +void inv_imu_set_serif(int (*read)(void *, uint8_t, uint8_t *, uint32_t), + int (*write)(void *, uint8_t, const uint8_t *, uint32_t)); +void inv_imu_set_delay(void (*delay_ms)(uint32_t), void (*delay_us)(uint32_t)); +int inv_imu_acc_enable(void); +int inv_imu_gyro_enable(void); + +int inv_imu_acc_disable(void); +int inv_imu_gyro_disable(void); + +int inv_imu_set_gyro_fsr(GYRO_CONFIG0_FS_SEL_t gyro_fsr_dps); + +int inv_imu_set_accel_fsr(ACCEL_CONFIG0_FS_SEL_t accel_fsr_g); + + +int inv_imu_acc_set_rate(float odr_hz, uint16_t packet_num,float *hw_odr); +int inv_imu_gyro_set_rate(float odr_hz, uint16_t packet_num,float *hw_odr); + +int inv_imu_get_rawdata_interrupt(struct accGyroDataPacket *dataPacket); + +int inv_data_handler(AccDataPacket *AccDatabuff,GyroDataPacket *GyroDatabuff,chip_temperature *chip_temper,bool polling); + +/* more interface function for deep usage */ +int inv_imu_enable_high_resolution_fifo(void); +int inv_imu_disable_high_resolution_fifo(void); +int inv_imu_enable_ff_register(void); +int inv_imu_disable_ff_register(void); +int inv_imu_apex_set_frequency(const APEX_CONFIG1_DMP_ODR_t frequency); +float convert_ff_duration_sample_to_cm(uint16_t ff_duration_samples); + + + +#if 0 +int inv_imu_polling_rawdata(struct accGyroDataPacket *dataPacket); +#endif + +int inv_imu_run_selftest(uint8_t acc_control,uint8_t gyro_control, struct inv_imu_selftest_output * st_output); + +int inv_imu_pedometer_enable(void); +int inv_imu_pedometer_disable(void); +int inv_imu_pedometer_get_event(float *cadence_step_per_sec,APEX_DATA3_ACTIVITY_CLASS_t *activity_class); + +int inv_imu_wom_get_event(bool *wom_detect); +int inv_imu_wom_enable(uint8_t wom_threshold_x,uint8_t wom_threshold_y,uint8_t wom_threshold_z,uint8_t duration); +int inv_imu_wom_disable(void); + +int inv_imu_freefall_enable(void); +int inv_imu_freefall_disable(void); +float inv_imu_freefall_get_event(bool *ff_detect); + + +void inv_imu_dumpRegs(void); + + +#ifdef __cplusplus +} +#endif +#endif diff --git a/components/i2c_devices/icm42670/inv_imu_driver.c b/components/i2c_devices/icm42670/inv_imu_driver.c new file mode 100644 index 0000000..e5bf9fc --- /dev/null +++ b/components/i2c_devices/icm42670/inv_imu_driver.c @@ -0,0 +1,3472 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2015-2015 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively Software is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +#include +#include +#include +#include +#include +#include +#include "inv_imu_driver.h" +#define NUM_TODISCARD 0 +#define SELFTEST_SAMPLES_NUMBER 200 +#define IMU_MAX_FIFO_SIZE 2000 // Max FIFO count size + +#ifndef SUPPORT_PEDOMETER +#define SUPPORT_PEDOMETER 0 +#endif + +#ifndef SUPPORT_SELFTEST +#define SUPPORT_SELFTEST 0 +#endif + + +#ifndef max +#define max(x, y) (x > y ? x : y) +#endif + +#ifndef ARRAY_SIZE +#define ARRAY_SIZE(a) (sizeof((a)) / sizeof((a)[0])) +#endif + +#ifndef abs +#define abs(a) ((a) > 0 ? (a) : -(a)) /*!< Absolute value */ +#endif + +#define G 9.80665 +#define PI 3.141592 +#define TEMP_SENSITIVITY_1_BYTE 0.4831f // Temperature in Degrees Centigrade = (FIFO_TEMP_DATA / 2.07) + 25; for FIFO 1 byte temperature data conversion. +#define TEMP_SENSITIVITY_2_BYTE 0.007548f // Temperature in Degrees Centigrade = (TEMP_DATA / 132.48) + 25; for data register and FIFO high res. data conversion. +#define ROOM_TEMP_OFFSET 25 //Celsius degree + +#define KSCALE_ACC_FSR 0.001197097f // ACC_FSR * G / (65536.0f/2); +#define KSCALE_GYRO_FSR 0.001065264f // GYR_FSR * PI / (180.0f * (65536.0f/2)); + +#define DEG2RAD(dps) ((dps) * PI / 180.0f) + +#define SENSOR_DATA_BITS(HI_RES) ((HI_RES) ? 20 : 16) /* internal use 19 bits, but it is 20 bits data */ +#define ODR2SMPLRT_DIV(odr) (1000/(odr) - 1) +//ACC:FSR_SEL=0->+/-2G, FSR_SEL=1->+/-4G, FSR_SEL=2->+/-8G, FSR_SEL=3->+/-16G +#define ACC_MIN_RANGE(FSR_SEL) \ + (-(pow(2, (4 - (FSR_SEL)))) * G) +#define ACC_MAX_RANGE(FSR_SEL) ((pow(2, (4 - (FSR_SEL)))) * G) +#define ACC_RESOLUTION(FSR_SEL) ACC_MAX_RANGE(FSR_SEL) / 32768.0f +#define ACC_RESOLUTION_HIRES(FSR_SEL,HI_RES) (ACC_MAX_RANGE(FSR_SEL) / (float)(1 << (SENSOR_DATA_BITS(HI_RES) -1))) + +//GYRO:FSR_SEL=0->+/-250dps, FSR_SEL=1->+/-500dps, FSR_SEL=2->+/-1000dps, FSR_SEL=3->+/-2000dps +#define GYRO_MIN_RANGE(FSR_SEL) \ + (DEG2RAD(-15.625f * pow(2, (7 - (FSR_SEL))))) +#define GYRO_MAX_RANGE(FSR_SEL) (DEG2RAD(15.625f * pow(2, (7 - (FSR_SEL))))) +#define GYRO_RESOLUTION(FSR_SEL) GYRO_MAX_RANGE(FSR_SEL) / 32768.0f +#define GYRO_RESOLUTION_HIRES(FSR_SEL,HI_RES) (GYRO_MAX_RANGE(FSR_SEL) / (float)(1 << (SENSOR_DATA_BITS(HI_RES) -1))) + +/* wom config */ +/** Default value for the WOM threshold + * Resolution of the threshold is 1g/256 ~= 3.9mg */ + +#define DEFAULT_WOM_THS_MG 52>>2 /* = 52mg/4 */ +#define EN 1 +#define DIS 0 + +#if SUPPORT_PEDOMETER +/* pedo config */ +/* LOW_ENERGY_AMP_TH_SEL */ +typedef enum +{ + LOW_ENERGY_AMP_TH_SEL_1006632MG = (0 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_1174405MG = (1 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_1342177MG = (2 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_1509949MG = (3 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_1677721MG = (4 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_1845493MG = (5 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_2013265MG = (6 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_2181038MG = (7 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_2348810MG = (8 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_2516582MG = (9 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_2684354MG = (10 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_2852126MG = (11 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_3019898MG = (12 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_3187671MG = (13 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_3355443MG = (14 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT), + LOW_ENERGY_AMP_TH_SEL_3523215MG = (15 << BIT_LOW_ENERGY_AMP_TH_SEL_SHIFT) +} LOW_ENERGY_AMP_TH_t; + +/* DMP_POWER_SAVE_TIME_SEL */ +typedef enum +{ + DMP_POWER_SAVE_TIME_SEL_0S = 0, + DMP_POWER_SAVE_TIME_SEL_4S = 1, + DMP_POWER_SAVE_TIME_SEL_8S = 2, + DMP_POWER_SAVE_TIME_SEL_12S = 3, + DMP_POWER_SAVE_TIME_SEL_16S = 4, + DMP_POWER_SAVE_TIME_SEL_20S = 5, + DMP_POWER_SAVE_TIME_SEL_24S = 6, + DMP_POWER_SAVE_TIME_SEL_28S = 7, + DMP_POWER_SAVE_TIME_SEL_32S = 8, + DMP_POWER_SAVE_TIME_SEL_36S = 9, + DMP_POWER_SAVE_TIME_SEL_40S = 10, + DMP_POWER_SAVE_TIME_SEL_44S = 11, + DMP_POWER_SAVE_TIME_SEL_48S = 12, + DMP_POWER_SAVE_TIME_SEL_52S = 13, + DMP_POWER_SAVE_TIME_SEL_56S = 14, + DMP_POWER_SAVE_TIME_SEL_60S = 15 +} DMP_POWER_SAVE_TIME_t; + +/* PEDO_AMP_TH_SEL */ +typedef enum +{ + PEDO_AMP_TH_1006632_MG = (0 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_1140850_MG = (1 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_1275068_MG = (2 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_1409286_MG = (3 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_1543503_MG = (4 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_1677721_MG = (5 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_1811939_MG = (6 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_1946157_MG = (7 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_2080374_MG = (8 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_2214592_MG = (9 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_2348810_MG = (10 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_2483027_MG = (11 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_2617245_MG = (12 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_2751463_MG = (13 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_2885681_MG = (14 << BIT_PED_AMP_TH_SEL_SHIFT), + PEDO_AMP_TH_3019898_MG = (15 << BIT_PED_AMP_TH_SEL_SHIFT) +} PEDO_AMP_TH_t; + +/* PED_STEP_CNT_TH_SEL */ +typedef enum +{ + PED_STEP_CNT_TH_0_STEP = 0, + PED_STEP_CNT_TH_1_STEP = 1, + PED_STEP_CNT_TH_2_STEP = 2, + PED_STEP_CNT_TH_3_STEP = 3, + PED_STEP_CNT_TH_4_STEP = 4, + PED_STEP_CNT_TH_5_STEP = 5, + PED_STEP_CNT_TH_6_STEP = 6, + PED_STEP_CNT_TH_7_STEP = 7, + PED_STEP_CNT_TH_8_STEP = 8, + PED_STEP_CNT_TH_9_STEP = 9, + PED_STEP_CNT_TH_10_STEP = 10, + PED_STEP_CNT_TH_11_STEP = 11, + PED_STEP_CNT_TH_12_STEP = 12, + PED_STEP_CNT_TH_13_STEP = 13, + PED_STEP_CNT_TH_14_STEP = 14, + PED_STEP_CNT_TH_15_STEP = 15, +} PED_STEP_CNT_TH_t; + +/* PED_STEP_DET_TH_SEL */ +typedef enum +{ + PED_STEP_DET_TH_0_STEP = (0 << BIT_PED_STEP_DET_TH_SEL_SHIFT), + PED_STEP_DET_TH_1_STEP = (1 << BIT_PED_STEP_DET_TH_SEL_SHIFT), + PED_STEP_DET_TH_2_STEP = (2 << BIT_PED_STEP_DET_TH_SEL_SHIFT), + PED_STEP_DET_TH_3_STEP = (3 << BIT_PED_STEP_DET_TH_SEL_SHIFT), + PED_STEP_DET_TH_4_STEP = (4 << BIT_PED_STEP_DET_TH_SEL_SHIFT), + PED_STEP_DET_TH_5_STEP = (5 << BIT_PED_STEP_DET_TH_SEL_SHIFT), + PED_STEP_DET_TH_6_STEP = (6 << BIT_PED_STEP_DET_TH_SEL_SHIFT), + PED_STEP_DET_TH_7_STEP = (7 << BIT_PED_STEP_DET_TH_SEL_SHIFT), +} PED_STEP_DET_TH_t; + +/* PEDO_SB_TIMER_TH_SEL */ +typedef enum +{ + PEDO_SB_TIMER_TH_50_SAMPLES = (0 << BIT_PED_SB_TIMER_TH_SEL_SHIFT), + PEDO_SB_TIMER_TH_75_SAMPLES = (1 << BIT_PED_SB_TIMER_TH_SEL_SHIFT), + PEDO_SB_TIMER_TH_100_SAMPLES = (2 << BIT_PED_SB_TIMER_TH_SEL_SHIFT), + PEDO_SB_TIMER_TH_125_SAMPLES = (3 << BIT_PED_SB_TIMER_TH_SEL_SHIFT), + PEDO_SB_TIMER_TH_150_SAMPLES = (4 << BIT_PED_SB_TIMER_TH_SEL_SHIFT), + PEDO_SB_TIMER_TH_175_SAMPLES = (5 << BIT_PED_SB_TIMER_TH_SEL_SHIFT), + PEDO_SB_TIMER_TH_200_SAMPLES = (6 << BIT_PED_SB_TIMER_TH_SEL_SHIFT), + PEDO_SB_TIMER_TH_225_SAMPLES = (7 << BIT_PED_SB_TIMER_TH_SEL_SHIFT) +} PEDO_SB_TIMER_TH_t; + +/* PEDO_HI_ENRGY_TH_SEL */ +typedef enum +{ + PEDO_HI_ENRGY_TH_90 = 0, + PEDO_HI_ENRGY_TH_107 = 1, + PEDO_HI_ENRGY_TH_136 = 2, + PEDO_HI_ENRGY_TH_159 = 3 +} PEDO_HI_ENRGY_TH_t; +#endif + +/* Error List */ +#define SENSOR_WHOAMI_INVALID_ERROR -1 +#define SENSOR_CONVERT_INVALID_ERROR -2 +#define SENSOR_INIT_INVALID_ERROR -3 +#define INT_STATUS_READ_ERROR -4 +#define DRDY_UNEXPECT_INT_ERROR -5 +#define DRDY_DATA_READ_ERROR -6 +#define DRDY_DATA_CONVERT_ERROR -7 +#define FIFO_COUNT_INVALID_ERROR -8 +#define FIFO_OVERFLOW_ERROR -9 +#define FIFO_UNEXPECT_WM_ERROR -10 +#define FIFO_DATA_READ_ERROR -11 +#define FIFO_DATA_CONVERT_ERROR -12 +#define FIFO_DATA_FULL_ERROR -13 +#if SUPPORT_SELFTEST +#define SELFTEST_INIT_ERROR -14 +#define SELFTEST_ACC_ENABLE_ERROR -15 +#define SELFTEST_ACC_READ_ERROR -16 +#define SELFTEST_ACC_ST_READ_ERROR -17 +#define SELFTEST_ACC_STCODE_READ_ERROR -18 +#define SELFTEST_GYR_ENABLE_ERROR -19 +#define SELFTEST_GYR_READ_ERROR -20 +#define SELFTEST_GYR_ST_READ_ERROR -21 +#define SELFTEST_GYR_STCODE_READ_ERROR -22 +#endif +#define DMP_TIMEOUT_ERROR -23 +#define DMP_IDLE_ERROR -24 +#define DMP_SIZE_ERROR -25 +#define DMP_SRAM_ERROR -26 +#define DMP_STATUS_ERROR -27 +#define DMP_LOAD_ERROR -28 +#if SUPPORT_PEDOMETER +#define PEDO_ENABLE_ERROR -29 +#endif +#define WOM_ENABLE_ERROR -32 +#define FF_ENABLE_ERROR -33 +#define FF_GETEVENT_ERROR -34 +#define PEDO_GETEVENT_ERROR -35 + + + +#define AXIS_X 0 +#define AXIS_Y 1 +#define AXIS_Z 2 +#define AXES_NUM 3 + +#define SENSOR_HZ(_hz) ((uint32_t)((_hz) * 1024.0f)) + +#define ACCEL_DATA_SIZE 6 +#define GYRO_DATA_SIZE 6 +#define TEMP_DATA_SIZE 2 + +#define FIFO_HEADER_SIZE 1 +#define FIFO_ACCEL_DATA_SHIFT 1 +#define FIFO_ACCEL_DATA_SIZE ACCEL_DATA_SIZE +#define FIFO_GYRO_DATA_SHIFT 7 +#define FIFO_GYRO_DATA_SIZE GYRO_DATA_SIZE +#define FIFO_TEMP_DATA_SHIFT 13 +#define FIFO_TEMP_DATA_SIZE 1 +#define FIFO_TIMESTAMP_DATA_SHIFT (icm_dev.fifo_highres_enabled ? 15: 14) +#define FIFO_TIMESTAMP_SIZE 2 +#define FIFO_TEMP_HIGH_RES_SIZE 1 +#define FIFO_ACCEL_GYRO_HIGH_RES_SIZE 3 +#define FIFO_ACCEL_GYRO_HIGH_RES_DATA_SHIFT FIFO_HEADER_SIZE + FIFO_ACCEL_DATA_SIZE + FIFO_GYRO_DATA_SIZE + FIFO_TEMP_DATA_SIZE + FIFO_TEMP_HIGH_RES_SIZE +FIFO_TIMESTAMP_SIZE + +#define FIFO_EMPTY_PACKAGE_SIZE 0 +#define FIFO_8BYTES_PACKET_SIZE (FIFO_HEADER_SIZE + FIFO_ACCEL_DATA_SIZE + FIFO_TEMP_DATA_SIZE) +#define FIFO_16BYTES_PACKET_SIZE (FIFO_HEADER_SIZE + FIFO_ACCEL_DATA_SIZE + FIFO_GYRO_DATA_SIZE + FIFO_TEMP_DATA_SIZE + FIFO_TIMESTAMP_SIZE) +#define FIFO_20BYTES_PACKET_SIZE (FIFO_HEADER_SIZE + FIFO_ACCEL_DATA_SIZE + FIFO_GYRO_DATA_SIZE + FIFO_TEMP_DATA_SIZE + FIFO_TIMESTAMP_SIZE +\ + FIFO_TEMP_HIGH_RES_SIZE + FIFO_ACCEL_GYRO_HIGH_RES_SIZE) +#define FIFO_HEADER_EMPTY_BIT 0x80 +#define FIFO_HEADER_A_BIT 0x40 +#define FIFO_HEADER_G_BIT 0x20 +#define FIFO_HEADER_20_BIT 0x10 + +#define DRI_ACCEL_DATA_SHIFT 2 +#define DRI_GYRO_DATA_SHIFT 8 +#define DRI_14BYTES_PACKET_SIZE (TEMP_DATA_SIZE + ACCEL_DATA_SIZE + GYRO_DATA_SIZE) + +typedef enum { + ODR_8KHZ = 3, + ODR_4KHZ = 4, + ODR_2KHZ = 5, + ODR_1KHZ = 6, + ODR_200HZ = 7, + ODR_100HZ = 8, + ODR_50HZ = 9, + ODR_25HZ = 10, + ODR_12_5HZ = 11, + ODR_500HZ = 15, +} imu_sensor_odr_t; + + +/** @brief IMU max FSR values for accel and gyro + * Dependent on chip + */ +#if defined(ICM43686) + #define ACCEL_CONFIG0_FS_SEL_MAX ACCEL_CONFIG0_FS_SEL_32g + #define GYRO_CONFIG0_FS_SEL_MAX GYRO_CONFIG0_FS_SEL_4000dps + + #define ACCEL_OFFUSER_MAX_MG 2000 + #define GYRO_OFFUSER_MAX_DPS 128 + +#else + #define ACCEL_CONFIG0_FS_SEL_MAX ACCEL_CONFIG0_FS_SEL_16g + #define GYRO_CONFIG0_FS_SEL_MAX GYRO_CONFIG0_FS_SEL_2000dps + + #define ACCEL_OFFUSER_MAX_MG 1000 + #define GYRO_OFFUSER_MAX_DPS 64 + +#endif + +/* Filter order */ +typedef enum { + FIRST_ORDER = 0, // 1st order + SEC_ORDER = 1, // 2nd order + THIRD_ORDER = 2, // 3rd order +} imu_filter_order_t; + +#if 0 +typedef enum { + I2C_SLEW_RATE_20_60NS = 0 << BIT_I2C_SLEW_RATE_SHIFT, + I2C_SLEW_RATE_12_36NS = 1 << BIT_I2C_SLEW_RATE_SHIFT, + I2C_SLEW_RATE_6_18NS = 2 << BIT_I2C_SLEW_RATE_SHIFT, + I2C_SLEW_RATE_4_12NS = 3 << BIT_I2C_SLEW_RATE_SHIFT, + I2C_SLEW_RATE_2_6NS = 4 << BIT_I2C_SLEW_RATE_SHIFT, + I2C_SLEW_RATE_2NS = 5 << BIT_I2C_SLEW_RATE_SHIFT, + SPI_SLEW_RATE_20_60NS = 0, + SPI_SLEW_RATE_12_36NS = 1, + SPI_SLEW_RATE_6_18NS = 2, + SPI_SLEW_RATE_4_12NS = 3, + SPI_SLEW_RATE_2_6NS = 4, + SPI_SLEW_RATE_2NS = 5, +} imu_slew_rate_t; +#endif + +/* sensor bandwidth */ +typedef enum { + BW_ODR_DIV_2 = 0, + BW_ODR_DIV_4 = 1, + BW_ODR_DIV_5 = 2, + BW_ODR_DIV_8 = 3, + BW_ODR_DIV_10 = 4, + BW_ODR_DIV_16 = 5, + BW_ODR_DIV_20 = 6, + BW_ODR_DIV_40 = 7, +} imu_bandwidth_t; + +static uint8_t IMU_ODR_MAPPING[] = { + ACCEL_CONFIG0_ODR_12_5_HZ, + ACCEL_CONFIG0_ODR_25_HZ, + ACCEL_CONFIG0_ODR_50_HZ, + ACCEL_CONFIG0_ODR_100_HZ, + ACCEL_CONFIG0_ODR_200_HZ, + ACCEL_CONFIG0_ODR_400_HZ, + ACCEL_CONFIG0_ODR_800_HZ, + ACCEL_CONFIG0_ODR_1600_HZ, +}; + +/* Support odr range 25HZ - 800HZ */ +static uint32_t IMUHWRates[] = { + SENSOR_HZ(12.5f), + SENSOR_HZ(25.0f), + SENSOR_HZ(50.0f), + SENSOR_HZ(100.0f), + SENSOR_HZ(200.0f), + SENSOR_HZ(400.0f), + SENSOR_HZ(800.0f), + SENSOR_HZ(1600.0f), +}; + +/* chip type */ +typedef enum { + INVALID_TYPE = 0, + CHIP_ICM42607, + CHIP_ICM42670, + CHIP_ICM43607, + CHIP_T1000, +} chip_type_t; + +struct sensorConvert { + int8_t sign[3]; + uint8_t axis[3]; +}; + + struct sensorConvert inv_map[] = { + { { 1, 1, 1}, {0, 1, 2} }, + { { -1, 1, 1}, {1, 0, 2} }, + { { -1, -1, 1}, {0, 1, 2} }, + { { 1, -1, 1}, {1, 0, 2} }, + + { { -1, 1, -1}, {0, 1, 2} }, + { { 1, 1, -1}, {1, 0, 2} }, + { { 1, -1, -1}, {0, 1, 2} }, + { { -1, -1, -1}, {1, 0, 2} }, +}; + +typedef struct { + uint32_t rate; //the rate from up layer want + uint32_t hwRate; // the rate currently set + uint32_t preRealRate; //the rate from sensor list sync with upper layer want + uint32_t samplesToDiscard; // depends on acc or gyro start up time to valid data + uint16_t wm; //the watermark bytes number setting for watermark from user + bool powered; + bool configed; + bool needDiscardSample; +} IMUSensor_t; + +typedef struct inv_imu { + IMUSensor_t sensors[NUM_OF_SENSOR-1]; //TS is not a real sensor + int (*read_reg)(void * context, uint8_t reg, uint8_t * buf, uint32_t len); + int (*write_reg)(void * context, uint8_t reg, const uint8_t * buf, uint32_t len); + + void (*delay_ms)(uint32_t); +#if SUPPORT_DELAY_US + void (*delay_us)(uint32_t); +#endif + + chip_type_t product; + uint8_t serif_type; + bool fifo_is_used; /**< Data are get from FIFO or from sensor registers. By default Fifo is used*/ + float chip_temper; + bool tmst_is_used; + bool dmp_is_on; + + /* For save reg status */ + uint8_t int_cfg; + int_source0_t int_src0; + pwr_mgmt0_t pwr_mgmt; + accel_config0_t acc_cfg0; + gyro_config0_t gyro_cfg0; + + /* For fifo */ + uint16_t watermark; + uint8_t dataBuf[IMU_MAX_FIFO_SIZE]; + uint32_t fifoDataToRead; + bool fifo_highres_enabled; /**< FIFO packets are 20 bytes long */ + bool polling_data_en; + uint16_t fifo_packet_size; + uint16_t dri_packet_size; + + uint8_t need_mclk_cnt; + uint16_t pre_fifo_ts; + uint64_t totol_sensor_ts; + uint32_t min_apex_odr; + uint32_t pre_min_apex_odr; // record previous min apex_odr + uint32_t pre_hwRate; + + bool init_cfg; + /* For sensor oriention convert, sync to Andriod coordinate */ + struct sensorConvert cvt; + +#if SUPPORT_SELFTEST + int avg[AXES_NUM]; + int acc_avg[AXES_NUM]; + int acc_avg_st[AXES_NUM]; + int gyro_avg[AXES_NUM]; + int gyro_avg_st[AXES_NUM]; + + uint8_t gyro_config; + uint8_t accel_config; + uint8_t st_rawdata_reg; + uint8_t acc_st_code[AXES_NUM]; + uint8_t gyro_st_code[AXES_NUM]; +#endif + + bool dmp_power_save; + bool apex_enable; + +} inv_imu_t; + +static inv_imu_t icm_dev; + +/* ****************************funcntion pre def **********************/ + +static int inv_imu_convert_rawdata(struct accGyroDataPacket *packet); + +static int inv_imu_switch_on_mclk(void); +static int inv_imu_switch_off_mclk(void); +int inv_imu_read_reg(uint32_t reg, uint32_t len, uint8_t * buf); +int inv_imu_write_reg(uint32_t reg, uint32_t len, const uint8_t * buf); +int inv_imu_set_timestamp_resolution(const TMST_CONFIG1_RESOL_t timestamp_resol); +int inv_imu_enable_accel_low_power_mode(uint32_t accel_rate_us); +int inv_imu_enable_accel_low_noise_mode(uint32_t accel_rate_us); +int inv_imu_config_drdy(bool enable); +int inv_imu_config_fifo_int(bool enable); +int inv_imu_config_fifofull_int(bool enable); +void inv_apply_mounting_matrix(int32_t raw[3]); +int inv_imu_device_reset(void); +int inv_imu_configure_fifo_interface(void); +int inv_imu_configure_wom(const uint8_t wom_x_th, const uint8_t wom_y_th, const uint8_t wom_z_th, + WOM_CONFIG_WOM_INT_MODE_t wom_int, WOM_CONFIG_WOM_INT_DUR_t wom_dur); +int inv_imu_process_selftest_end(inv_imu_selftest_output_t *st_output); +int inv_imu_control_selftest(uint8_t acc_control,uint8_t gyro_control); +int inv_imu_configure_selftest_parameters(void); +int inv_imu_reset_dmp(void); +int inv_imu_start_dmp_selftest(void); +int inv_imu_reset_fifo(void); +int inv_imu_read_rawdata(void); +int inv_imu_polling_rawdata(struct accGyroDataPacket *dataPacket); +int inv_imu_disable_wom_register(bool wom_disable); +int inv_imu_enable_wom_register(void); +int inv_imu_apex_init_parameters_struct( inv_imu_apex_parameters_t *apex_inputs); +int inv_imu_apex_configure_parameters(const inv_imu_apex_parameters_t *apex_inputs); + + +/* ****************************funcntion realization **********************/ + +void inv_imu_set_serif(int (*read)(void *, uint8_t, uint8_t *, uint32_t), + int (*write)(void *, uint8_t, const uint8_t *, uint32_t)) +{ + if (read == NULL || write == NULL) { + INV_LOG(INV_LOG_LEVEL_ERROR, "Read/Write API NULL POINTER!!"); + EXIT(-1); + } + icm_dev.read_reg = read; + icm_dev.write_reg = write; +} + +static int inv_read(uint8_t addr, uint32_t len, uint8_t *buf) +{ + return icm_dev.read_reg(0, addr, buf, len); +} + +static int inv_write(uint8_t addr, uint32_t len, const uint8_t *buf) +{ + return icm_dev.write_reg(0, addr, buf, len); +} + +void inv_imu_set_delay(void (*delay_ms)(uint32_t), void (*delay_us)(uint32_t)) +{ +#if SUPPORT_DELAY_US + if (delay_ms == NULL || delay_us == NULL) { + INV_LOG(INV_LOG_LEVEL_ERROR, "DELAY API NULL POINTER!!"); + EXIT(-1); + } + icm_dev.delay_ms = delay_ms; + icm_dev.delay_us = delay_us; +#else + if (delay_ms == NULL ) { + INV_LOG(INV_LOG_LEVEL_ERROR, "DELAY MS API NULL POINTER!!"); + EXIT(-1); + } + icm_dev.delay_ms = delay_ms; + #endif +} + +static void inv_delay_ms(uint16_t ms) +{ + icm_dev.delay_ms(ms); +} + +static void inv_delay_us(uint16_t us) +{ +#if SUPPORT_DELAY_US + icm_dev.delay_us(us); +#else + icm_dev.delay_ms(1); // When there is no microsecond delay per platform, we delay minimal 1ms. +#endif +} + +static int read_mclk_reg(uint16_t regaddr, uint8_t rd_cnt, uint8_t * buf) +{ + uint8_t data; + uint8_t blk_sel = (regaddr&0xFF00)>>8; + int status = 0; + + // Have IMU not in IDLE mode to access MCLK domain + status |= inv_imu_switch_on_mclk(); + + if(blk_sel == 0x10){ + blk_sel = 0x00; + } + // optimize by changing BLK_SEL only if not NULL + if(blk_sel) + status |= inv_write((uint8_t)BLK_SEL_R & 0xff, 1, &blk_sel); + + data = (regaddr&0x00FF); + status |= inv_write((uint8_t)MADDR_R, 1, &data); + // wait 16 MCLK (4MHz) clock cycles + inv_delay_us(10); + status |= inv_read((uint8_t)M_R, rd_cnt, buf); + // wait 16 MCLK (4MHz) clock cycles + inv_delay_us(10); + + if(blk_sel) { + data = 0; + status |= inv_write((uint8_t)BLK_SEL_R, 1, &data); + } + + // switch OFF MCLK if needed + status |= inv_imu_switch_off_mclk(); + + return status; +} + +static int write_mclk_reg(uint16_t regaddr, uint8_t wr_cnt, const uint8_t * buf) +{ + uint8_t data; + uint8_t blk_sel = (regaddr&0xFF00)>>8; + uint8_t i = 0; + int status = 0; + + // Have IMU not in IDLE mode to access MCLK domain + status |= inv_imu_switch_on_mclk(); + if(blk_sel == 0x10){ + blk_sel = 0x00; + } + // optimize by changing BLK_SEL only if not NULL + if(blk_sel) + status |= inv_write((uint8_t)BLK_SEL_W, 1, &blk_sel); + + data = (regaddr&0x00FF); + status |= inv_write((uint8_t)MADDR_W, 1, &data); + for (i = 0; i= sizeof(inv_map) / sizeof(inv_map[0]) ) + return -1; + + src = &inv_map[direction]; + memcpy(cvt, src, sizeof(struct sensorConvert)); + + return 0; +} + +static int inv_imu_init_config(void) +{ + int status = 0; + int_config_t reg_int_config; + inv_imu_apex_parameters_t apex_inputs; + + INV_LOG(SENSOR_LOG_LEVEL, "%s", __func__); + + inv_delay_ms(3); + + status += configure_serial_interface(); + inv_delay_ms(3); + status += inv_imu_device_reset(); + + /* set BANK1 bus setting + *change drive config for slew rate if necessary */ + + /* Setup MEMs properties. */ + status |= inv_imu_read_reg(GYRO_CONFIG0, 1, (uint8_t*)&icm_dev.gyro_cfg0); + status |= inv_imu_read_reg(ACCEL_CONFIG0, 1, (uint8_t*)&icm_dev.acc_cfg0); + + icm_dev.gyro_cfg0.gyro_ui_fs_sel = GYRO_CONFIG0_FS_SEL_2000dps; + icm_dev.acc_cfg0.accel_ui_fs_sel = ACCEL_CONFIG0_FS_SEL_4g; + status |= inv_imu_write_reg(GYRO_CONFIG0, 1, (uint8_t*)&icm_dev.gyro_cfg0); + status |= inv_imu_write_reg(ACCEL_CONFIG0, 1, (uint8_t*)&icm_dev.acc_cfg0); + + /* Enable push pull on INT1 to avoid moving in Test Mode after a soft reset */ + /* set INT1 push-pull, active high,pulse mode */ + status |= inv_imu_read_reg(INT_CONFIG_REG, 1, (uint8_t*)®_int_config); + + reg_int_config.int1_drive_circuit = INT_CONFIG_INT1_DRIVE_CIRCUIT_PP; + reg_int_config.int1_polarity = INT_CONFIG_INT1_POLARITY_HIGH; + reg_int_config.int1_mode = INT_CONFIG_INT1_PULSE; + + status |= inv_imu_write_reg(INT_CONFIG_REG, 1, (uint8_t*)®_int_config); + + //get inital int src reg value + inv_imu_read_reg(INT_SOURCE0, 1, (uint8_t*)&icm_dev.int_src0); + + /* Set default timestamp resolution 16us (Mobile use cases) */ + status |= inv_imu_set_timestamp_resolution(TMST_CONFIG1_RESOL_16us); + + status |= inv_imu_configure_fifo_interface(); + status |= inv_imu_configure_wom(DEFAULT_WOM_THS_MG, + DEFAULT_WOM_THS_MG, + DEFAULT_WOM_THS_MG, + WOM_CONFIG_WOM_INT_MODE_ANDED, + WOM_CONFIG_WOM_INT_DUR_1_SMPL); + + //config APEX parameter + status |= inv_imu_apex_init_parameters_struct(&apex_inputs); + status |= inv_imu_apex_configure_parameters(&apex_inputs); + + status |= inv_imu_set_autorcosc_power_on(false); + + return status; +} + +int inv_imu_initialize(void) +{ + int ret = 0; + + icm_dev.product = INVALID_TYPE; + icm_dev.int_cfg = 0; + icm_dev.init_cfg = false; + icm_dev.dmp_power_save = false; + icm_dev.apex_enable = false; + + icm_dev.dri_packet_size = 0; + icm_dev.polling_data_en = false; + icm_dev.fifo_is_used = FIFO_WM_MODE_EN; + icm_dev.fifo_highres_enabled = IS_HIGH_RES_MODE; + icm_dev.tmst_is_used = SUPPORT_FIFO_TS; + icm_dev.serif_type = 0; + icm_dev.pre_fifo_ts = 0; + icm_dev.totol_sensor_ts = 0; + icm_dev.min_apex_odr = SENSOR_HZ(25); + + icm_dev.sensors[PEDO].powered = false; + icm_dev.sensors[WOM].powered = false; + + if(icm_dev.fifo_highres_enabled) + icm_dev.fifo_packet_size = FIFO_20BYTES_PACKET_SIZE; + else + icm_dev.fifo_packet_size = FIFO_16BYTES_PACKET_SIZE; + + INV_LOG(SENSOR_LOG_LEVEL, "inv_imu_initialize"); + configure_serial_interface(); + inv_imu_get_whoami(); + + if (icm_dev.product == INVALID_TYPE) + return SENSOR_WHOAMI_INVALID_ERROR; + + ret += inv_imu_get_convert(SENSOR_DIRECTION, &icm_dev.cvt); + if (ret != 0) + return SENSOR_CONVERT_INVALID_ERROR; + + INV_LOG(SENSOR_LOG_LEVEL, "sensor axis[0]:%d, axis[1]:%d, axis[2]:%d, sign[0]:%d, sign[1]:%d, sign[2]:%d", + icm_dev.cvt.axis[AXIS_X], icm_dev.cvt.axis[AXIS_Y], icm_dev.cvt.axis[AXIS_Z], + icm_dev.cvt.sign[AXIS_X], icm_dev.cvt.sign[AXIS_Y], icm_dev.cvt.sign[AXIS_Z]); + + ret += inv_imu_init_config(); + + if (ret == 0) { + icm_dev.init_cfg = true; + INV_LOG(SENSOR_LOG_LEVEL, "Initialize Success"); + } else { + INV_LOG(SENSOR_LOG_LEVEL, "Initialize Failed %d", ret); + ret = SENSOR_INIT_INVALID_ERROR; + } + + return ret; +} + +int inv_imu_enable_accel_low_power_mode(uint32_t accel_rate_us ) +{ + int status = 0; + PWR_MGMT_0_ACCEL_MODE_t accel_mode; + PWR_MGMT_0_GYRO_MODE_t gyro_mode; + + status |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + accel_mode = (PWR_MGMT_0_ACCEL_MODE_t) icm_dev.pwr_mgmt.accel_mode; + gyro_mode = (PWR_MGMT_0_GYRO_MODE_t ) icm_dev.pwr_mgmt.gyro_mode; + + if (accel_rate_us == 0){ + accel_rate_us = 80000; // 80 ms for enough safe time ; + } + + /* Check if the accelerometer is the only one enabled */ + if( (accel_mode != PWR_MGMT_0_ACCEL_MODE_LP) && + ((gyro_mode == PWR_MGMT_0_GYRO_MODE_OFF) || + (gyro_mode == PWR_MGMT_0_GYRO_MODE_STANDBY)) ){ + /* Select the RC OSC as clock source for the accelerometer */ + status |= select_rcosc(); + } + + /* Enable/Switch the accelerometer in/to low power mode */ + status |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_LP; + status |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + inv_delay_us(200); +#if 0 + if( (accel_mode != PWR_MGMT_0_ACCEL_MODE_LP) && + (gyro_mode == PWR_MGMT_0_GYRO_MODE_OFF) || + (gyro_mode == PWR_MGMT_0_GYRO_MODE_STANDBY)) ){ + /* Wait one accelerometer ODR before switching to the WU OSC */ + if(accel_rate_us>=1000) + inv_delay_ms(accel_rate_us/1000+1); + else + inv_delay_us(accel_rate_us); + + status |= select_wuosc(); + } +#endif + + /* Enable the automatic RCOSC power on so that FIFO is entirely powered on */ + status |= inv_imu_set_autorcosc_power_on(true); + return status; +} + +int inv_imu_enable_accel_low_noise_mode(uint32_t accel_rate_us) +{ + int status = 0; + PWR_MGMT_0_ACCEL_MODE_t accel_mode; + PWR_MGMT_0_GYRO_MODE_t gyro_mode; + + if (accel_rate_us == 0 ) + accel_rate_us = 80000 ; // 80 ms for enough safe time ; + + status |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + accel_mode = (PWR_MGMT_0_ACCEL_MODE_t) icm_dev.pwr_mgmt.accel_mode; + gyro_mode = (PWR_MGMT_0_GYRO_MODE_t) icm_dev.pwr_mgmt.gyro_mode; + + /* Check if the accelerometer is the only one enabled */ + if ((accel_mode == PWR_MGMT_0_ACCEL_MODE_LP) && + ((gyro_mode == PWR_MGMT_0_GYRO_MODE_OFF) || (gyro_mode == PWR_MGMT_0_GYRO_MODE_STANDBY))){ + + /* Select the RC OSC as clock source for the accelerometer */ + status |= select_rcosc(); + /* Wait one accel ODR before switching to low noise mode */ + if(accel_rate_us>=1000) + inv_delay_ms(accel_rate_us/1000+1); + else + inv_delay_us(accel_rate_us); + } + + /* Enable/Switch the accelerometer in/to low noise mode */ + status |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_LN; + status |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + /* Disable the automatic RCOSC power on so that FIFO is entirely powered on */ + status |= inv_imu_set_autorcosc_power_on(false); + inv_delay_us(200); + + return status; +} + +static int inv_imu_set_odr(int index) +{ + int ret = 0; + uint8_t regValue = 0; + + regValue = IMU_ODR_MAPPING[index]; + INV_LOG(SENSOR_LOG_LEVEL, "Odr Reg value 0x%x", regValue); + /* update new odr */ + icm_dev.gyro_cfg0.gyro_odr = regValue; + ret |= inv_imu_write_reg(GYRO_CONFIG0, 1, (uint8_t*)&icm_dev.gyro_cfg0); + INV_LOG(SENSOR_LOG_LEVEL, "write GYRO_CONFIG0 0x%x", icm_dev.gyro_cfg0); + icm_dev.acc_cfg0.accel_odr = regValue; + ret |= inv_imu_write_reg(ACCEL_CONFIG0,1, (uint8_t*)&icm_dev.acc_cfg0); + INV_LOG(SENSOR_LOG_LEVEL, "write ACCEL_CONFIG0 0x%x", icm_dev.gyro_cfg0); + + return ret; +} + +static int inv_imu_cal_odr(uint32_t *rate, uint32_t *report_rate) +{ + uint8_t i; + + for (i = 0; i < (ARRAY_SIZE(IMUHWRates)); i++) { + if (*rate <= IMUHWRates[i]) { + *report_rate = IMUHWRates[i]; + break; + } + } + + if (*rate > IMUHWRates[(ARRAY_SIZE(IMUHWRates) - 1)]) { + i = (ARRAY_SIZE(IMUHWRates) - 1); + *report_rate = IMUHWRates[i]; + } + + return (int)i; +} + +/* return watermark in bytes with input as number of packets */ +static uint16_t inv_imu_cal_wm(uint16_t packet) +{ + uint8_t min_watermark = 1; + uint8_t max_watermark ; + uint16_t real_watermark = 0; //watermark in packet (record) number + + // For Xian the max FIFO size is 2048 Bytes, common FIFO package 16 Bytes, so the max number of FIFO pakage is 128. + max_watermark = MAX_RECV_PACKET/2 ; /*64*/ + + real_watermark = packet; + real_watermark = real_watermark < min_watermark ? min_watermark : real_watermark; + real_watermark = real_watermark > max_watermark ? max_watermark : real_watermark; + + return real_watermark * icm_dev.fifo_packet_size; /* byte mode*/ +} + +int inv_imu_reset_fifo(void) +{ + int status = 0; + signal_path_reset_t fifo_flush_status; + status |= inv_imu_read_reg(SIGNAL_PATH_RESET, 1,(uint8_t*) &fifo_flush_status); + + fifo_flush_status.fifo_flush = 1; + + status |= inv_imu_switch_on_mclk(); + + status |= inv_imu_write_reg(SIGNAL_PATH_RESET, 1, (uint8_t*)&fifo_flush_status); + inv_delay_us(10); + + /* Wait for FIFO flush (idle bit will go high at appropriate time and unlock flush) */ + while( (1==fifo_flush_status.fifo_flush) && (0==status) ) + { + status |= inv_imu_read_reg(SIGNAL_PATH_RESET, 1, (uint8_t*)&fifo_flush_status); + inv_delay_us(2); + } + + status |= inv_imu_switch_off_mclk(); + + return status; +} + +static int inv_imu_read_fifo(void) +{ + int ret = 0; + uint8_t count[2]; + ret |= inv_imu_switch_on_mclk(); + + /* FIFO byte mode configured at driver init, so we read byte number, not pakage count */ + if((ret |= inv_imu_read_reg(FIFO_COUNTH, 2, &count[0])) != INV_ERROR_SUCCESS){ + ret += inv_imu_switch_off_mclk(); + return ret; + } + icm_dev.fifoDataToRead = (((uint16_t)count[1]) <<8) | count[0]; + +#if !SENSOR_LOG_TS_ONLY + INV_LOG(SENSOR_LOG_LEVEL, "Fifo count is %d bytes", icm_dev.fifoDataToRead); +#endif + if (icm_dev.fifoDataToRead <= 0 || icm_dev.fifoDataToRead > IMU_MAX_FIFO_SIZE) + return FIFO_COUNT_INVALID_ERROR; + + ret += inv_imu_read_reg(FIFO_DATA, icm_dev.fifoDataToRead, icm_dev.dataBuf); + + ret += inv_imu_switch_off_mclk(); + INV_LOG(SENSOR_LOG_LEVEL, "read Fifo end"); + + return ret; +} + +static int inv_imu_config_fifo(bool enable) +{ + int ret = 0; + fifo_config1_t reg_fifo_config1; + uint8_t buffer[2],data; + uint16_t watermarkReg; + + watermarkReg = icm_dev.watermark; + + if (watermarkReg < icm_dev.fifo_packet_size) + watermarkReg = icm_dev.fifo_packet_size; + + INV_LOG(SENSOR_LOG_LEVEL, "%s watermarkReg %d", __func__, watermarkReg); + + buffer[0] = watermarkReg & 0x00FF; + buffer[1] = (watermarkReg & 0xFF00) >> 8; + + ret |= inv_imu_switch_on_mclk(); + + ret |= inv_imu_read_reg(FIFO_CONFIG1, 1, (uint8_t*)®_fifo_config1); + if (enable) { + /* fifo mode, set by pass on */ + + reg_fifo_config1.fifo_mode = FIFO_CONFIG1_FIFO_MODE_SNAPSHOT; + reg_fifo_config1.fifo_bypass = FIFO_CONFIG1_FIFO_BYPASS_ON; + ret |=inv_imu_write_reg(FIFO_CONFIG1,1,(uint8_t*)®_fifo_config1 ); + /* flush fifo */ + data = (uint8_t)SIGNAL_PATH_RESET_FIFO_FLUSH_EN; + ret |=inv_imu_write_reg(SIGNAL_PATH_RESET,1,&data); + + inv_delay_ms(10); + + /* set threshold */ + ret |=inv_imu_write_reg(FIFO_CONFIG2,1, &buffer[0]); + ret |=inv_imu_write_reg(FIFO_CONFIG3,1, &buffer[1]); + + if (icm_dev.sensors[ACC].configed || icm_dev.sensors[GYR].configed) { + /* set fifo stream mode */ + reg_fifo_config1.fifo_mode = FIFO_CONFIG1_FIFO_MODE_SNAPSHOT; + reg_fifo_config1.fifo_bypass = FIFO_CONFIG1_FIFO_BYPASS_OFF; + ret |=inv_imu_write_reg(FIFO_CONFIG1,1,(uint8_t*)®_fifo_config1 ); + } + INV_LOG(SENSOR_LOG_LEVEL, "ConfigFifo: Reset, TH_L:0x%x, TH_H:0x%x\n", + buffer[0], buffer[1]); + } else { + ret |=inv_imu_write_reg(FIFO_CONFIG2,1, &buffer[0]); + ret |=inv_imu_write_reg(FIFO_CONFIG3,1, &buffer[1]); + + reg_fifo_config1.fifo_mode = FIFO_CONFIG1_FIFO_MODE_SNAPSHOT; + + if (icm_dev.sensors[ACC].configed || icm_dev.sensors[GYR].configed) { + reg_fifo_config1.fifo_bypass = FIFO_CONFIG1_FIFO_BYPASS_OFF; + } else { + reg_fifo_config1.fifo_bypass = FIFO_CONFIG1_FIFO_BYPASS_ON; + } + + ret |=inv_imu_write_reg(FIFO_CONFIG1,1,(uint8_t*)®_fifo_config1 ); + INV_LOG(SENSOR_LOG_LEVEL, "ConfigFifo, TH_L:0x%x, TH_H:0x%x\n", + buffer[0], buffer[1]); + + /*to do if necessary , add fifo disable for gyro& accel in FIFO_CONFIG5_MREG */ + } + + inv_imu_switch_off_mclk(); + + return ret; +} + +int inv_imu_enable_high_resolution_fifo(void) +{ + fifo_config5_t reg_fifo_config5; + int status = 0; + + /* set FIFO packets to 20bit format (i.e. high res is enabled) */ + + status |= inv_imu_read_reg(FIFO_CONFIG5_MREG, 1, (uint8_t*)®_fifo_config5); + reg_fifo_config5.fifo_hires_en = FIFO_CONFIG5_HIRES_EN; + status |= inv_imu_write_reg(FIFO_CONFIG5_MREG, 1, (uint8_t*)®_fifo_config5); + + return status; +} + +int inv_imu_disable_high_resolution_fifo(void) +{ + fifo_config5_t reg_fifo_config5; + int status = 0; + + status |= inv_imu_read_reg(FIFO_CONFIG5_MREG, 1, (uint8_t*)®_fifo_config5); + reg_fifo_config5.fifo_hires_en = FIFO_CONFIG5_HIRES_DIS; + status |= inv_imu_write_reg(FIFO_CONFIG5_MREG, 1, (uint8_t*)®_fifo_config5); + + return status; +} + +int inv_imu_config_drdy(bool enable) +{ + int ret = 0; + + if (enable && + (true == icm_dev.sensors[ACC].configed || + true == icm_dev.sensors[GYR].configed)) { + if (!icm_dev.int_src0.drdy_int1_en) { + icm_dev.int_src0.drdy_int1_en = 1; + ret += inv_imu_write_reg(INT_SOURCE0,1,(uint8_t*)&icm_dev.int_src0); + INV_LOG(SENSOR_LOG_LEVEL, "Enable DRDY INT1 0x%x", icm_dev.int_src0); + } + } else { + icm_dev.int_src0.drdy_int1_en = 0; + ret += inv_imu_write_reg(INT_SOURCE0,1,(uint8_t*)&icm_dev.int_src0); + INV_LOG(SENSOR_LOG_LEVEL, "Disable DRDY INT1 0x%x", icm_dev.int_src0); + } + + return ret; +} + +int inv_imu_config_fifo_int(bool enable) +{ + int ret = 0; + + if (enable && + (true == icm_dev.sensors[ACC].configed || + true == icm_dev.sensors[GYR].configed)) { + if ( !icm_dev.int_src0.fifo_ths_int1_en ) { + icm_dev.int_src0.fifo_ths_int1_en = 1; + ret += inv_imu_write_reg(INT_SOURCE0,1,(uint8_t*)&icm_dev.int_src0); + INV_LOG(SENSOR_LOG_LEVEL, "Enable FIFO WM INT1 0x%x", icm_dev.int_src0); + } + } else { + icm_dev.int_src0.fifo_ths_int1_en = 0; + ret += inv_imu_write_reg(INT_SOURCE0,1,(uint8_t*)&icm_dev.int_src0); + INV_LOG(SENSOR_LOG_LEVEL, "Disable FIFO WM INT1 0x%x", icm_dev.int_src0); + } + + return ret; +} + +int inv_imu_config_fifofull_int(bool enable) +{ + int ret = 0; + + if (enable && + (true == icm_dev.sensors[ACC].configed || + true == icm_dev.sensors[GYR].configed)) { + if (!icm_dev.int_src0.fifo_full_int1_en) { + icm_dev.int_src0.fifo_full_int1_en =1 ; + ret += inv_imu_write_reg(INT_SOURCE0,1,(uint8_t*)&icm_dev.int_src0); + INV_LOG(SENSOR_LOG_LEVEL, "Enable FIFO WM INT1 0x%x", icm_dev.int_src0); + } + } else { + icm_dev.int_src0.fifo_full_int1_en = 0 ; + ret += inv_imu_write_reg(INT_SOURCE0,1,(uint8_t*)&icm_dev.int_src0); + INV_LOG(SENSOR_LOG_LEVEL, "Disable FIFO WM INT1 0x%x", icm_dev.int_src0); + } + + return ret; +} + +int inv_imu_set_accel_fsr(ACCEL_CONFIG0_FS_SEL_t accel_fsr_g) +{ + int status = 0; + + status |= inv_imu_read_reg(ACCEL_CONFIG0, 1, (uint8_t*)&icm_dev.acc_cfg0); + + if((icm_dev.fifo_highres_enabled) && (icm_dev.fifo_is_used == INV_IMU_FIFO_ENABLED)) + icm_dev.acc_cfg0.accel_ui_fs_sel = ACCEL_CONFIG0_FS_SEL_MAX; + else{ + icm_dev.acc_cfg0.accel_ui_fs_sel = accel_fsr_g; + } + + status |= inv_imu_write_reg(ACCEL_CONFIG0, 1, (uint8_t*)&icm_dev.acc_cfg0); + + return status; +} + +int inv_imu_set_gyro_fsr(GYRO_CONFIG0_FS_SEL_t gyro_fsr_dps) +{ + int status = 0; + + status |= inv_imu_read_reg(GYRO_CONFIG0, 1, (uint8_t*)&icm_dev.gyro_cfg0); + + if((icm_dev.fifo_highres_enabled) && (icm_dev.fifo_is_used == INV_IMU_FIFO_ENABLED)) + icm_dev.gyro_cfg0.gyro_ui_fs_sel = GYRO_CONFIG0_FS_SEL_MAX; + else{ + icm_dev.gyro_cfg0.gyro_ui_fs_sel = gyro_fsr_dps; + } + + status |= inv_imu_write_reg(GYRO_CONFIG0, 1, (uint8_t*)&icm_dev.gyro_cfg0); + + return status; +} + +int inv_imu_acc_enable(void) +{ + int ret = 0; + + ret |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + icm_dev.sensors[ACC].powered = true; + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_LN; + ret |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + INV_LOG(SENSOR_LOG_LEVEL, "accel Enable power mode=%d, pwr %x \n", icm_dev.sensors[ACC].powered, icm_dev.pwr_mgmt); + + /* Enable the automatic RCOSC power on so that FIFO is entirely powered on */ + //ret |= inv_imu_set_autorcosc_power_on(true); + // set 20ms for accel start-up time + inv_delay_ms(20); + + return ret; +} + +int inv_imu_acc_disable() +{ + int ret = 0; + int odr_index = 0; + uint32_t sampleRate = 0; + bool accelOdrChanged = false; + bool watermarkChanged = false; + + INV_LOG(SENSOR_LOG_LEVEL, "%s", __func__); + + icm_dev.sensors[ACC].preRealRate = 0; + icm_dev.sensors[ACC].hwRate = 0; + icm_dev.sensors[ACC].needDiscardSample = false; + icm_dev.sensors[ACC].samplesToDiscard = 0; + icm_dev.sensors[ACC].wm = 0; + + //get new pwr_mgmt reg value + ret |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + if ((false == icm_dev.sensors[GYR].powered) + && (false == icm_dev.sensors[PEDO].powered) + && (false == icm_dev.sensors[WOM].powered)) + { + inv_imu_switch_off_mclk(); + /* turn off acc & gyro */ + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_OFF; + icm_dev.pwr_mgmt.gyro_mode = PWR_MGMT_0_GYRO_MODE_OFF; + ret |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + inv_delay_us(200); //spec: 200us + + INV_LOG(SENSOR_LOG_LEVEL, "acc off pwr: 0x%x", icm_dev.pwr_mgmt); + } else if (true == icm_dev.sensors[GYR].powered) { // update gyro old odr + if (icm_dev.sensors[GYR].hwRate != icm_dev.sensors[GYR].preRealRate) { + icm_dev.sensors[GYR].hwRate = icm_dev.sensors[GYR].preRealRate; + odr_index = inv_imu_cal_odr(&icm_dev.sensors[GYR].hwRate, &sampleRate); + ret += inv_imu_set_odr(odr_index); + INV_LOG(SENSOR_LOG_LEVEL, "gyro revert rate to preRealRate: %f Hz", + (icm_dev.sensors[GYR].hwRate/1024.0f)); + accelOdrChanged = true; + } + if (icm_dev.fifo_is_used && (icm_dev.watermark != icm_dev.sensors[GYR].wm)) { + icm_dev.watermark = icm_dev.sensors[GYR].wm; + watermarkChanged = true; + INV_LOG(SENSOR_LOG_LEVEL, "watermark revert to: %d", icm_dev.watermark/icm_dev.fifo_packet_size); + } + + if ((false == icm_dev.sensors[PEDO].powered) + && (false == icm_dev.sensors[WOM].powered) + ) { + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_OFF; + ret |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + inv_delay_us(200); //spec: 200us + + INV_LOG(SENSOR_LOG_LEVEL, "gyro on and acc off pwr: 0x%x", icm_dev.pwr_mgmt); + } else { + INV_LOG(SENSOR_LOG_LEVEL, "gyro on and apex on pwr: 0x%x", icm_dev.pwr_mgmt); + } + } + else if((true == icm_dev.sensors[PEDO].powered) + ||(true == icm_dev.sensors[WOM].powered)) + { + if(icm_dev.sensors[ACC].hwRate!=0) + inv_imu_enable_accel_low_power_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + else + inv_imu_enable_accel_low_power_mode(0); + } + icm_dev.sensors[ACC].powered = false; + icm_dev.sensors[ACC].configed = false; + + if (true == icm_dev.fifo_is_used) { + inv_imu_config_fifo(accelOdrChanged | watermarkChanged); + } + + if ((false == icm_dev.sensors[GYR].powered) + && (false == icm_dev.sensors[ACC].powered)){ + //disable fifo rcosc + inv_imu_set_autorcosc_power_on(false); + + // disable interruput + if (true == icm_dev.fifo_is_used) { + inv_imu_config_fifo_int(false); + inv_imu_config_fifofull_int(false); + } else { + inv_imu_config_drdy(false); + } + } + + #if 0 + if (accelOdrChanged | watermarkChanged) { + //do nothing + } + #endif + + return ret; +} + +int inv_imu_gyro_enable() +{ + int ret = 0; + + INV_LOG(SENSOR_LOG_LEVEL, "%s", __func__); + + ret |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + icm_dev.sensors[GYR].powered = true; + icm_dev.pwr_mgmt.gyro_mode = PWR_MGMT_0_GYRO_MODE_LN; + ret |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + //inv_imu_set_autorcosc_power_on(true); + + //100ms here to discard invalid data + inv_delay_ms(100); + INV_LOG(SENSOR_LOG_LEVEL, "GYR PWR 0x%x", icm_dev.pwr_mgmt); + + return ret; +} + +int inv_imu_gyro_disable() +{ + int ret = 0; + int odr_index = 0; + uint32_t sampleRate = 0; + bool gyroOdrChanged = false; + bool watermarkChanged = false; + + INV_LOG(SENSOR_LOG_LEVEL, "%s", __func__); + + icm_dev.sensors[GYR].preRealRate = 0; + icm_dev.sensors[GYR].hwRate = 0; + icm_dev.sensors[GYR].needDiscardSample = false; + icm_dev.sensors[GYR].samplesToDiscard = 0; + icm_dev.sensors[GYR].wm = 0; + + //get new pwr_mgmt reg value + ret |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + if (true == icm_dev.sensors[ACC].powered) { // update ACC old ord + if(icm_dev.sensors[ACC].hwRate != icm_dev.sensors[ACC].preRealRate) { + icm_dev.sensors[ACC].hwRate = icm_dev.sensors[ACC].preRealRate; + odr_index = inv_imu_cal_odr(&icm_dev.sensors[ACC].hwRate, &sampleRate); + ret += inv_imu_set_odr(odr_index); + gyroOdrChanged = true; + INV_LOG(SENSOR_LOG_LEVEL, "acc revert rate to preRealRate: %f Hz", + (icm_dev.sensors[ACC].hwRate/1024.0f)); + if (icm_dev.fifo_is_used && (icm_dev.watermark != icm_dev.sensors[ACC].wm)) { + icm_dev.watermark = icm_dev.sensors[ACC].wm; + watermarkChanged = true; + INV_LOG(SENSOR_LOG_LEVEL, "watermark revert to: %d", icm_dev.watermark/icm_dev.fifo_packet_size); + } + } + + odr_index = inv_imu_cal_odr(&icm_dev.sensors[ACC].hwRate, &sampleRate); + + if(odr_index <2) + inv_imu_enable_accel_low_power_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + else + inv_imu_enable_accel_low_noise_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + + icm_dev.pwr_mgmt.gyro_mode = PWR_MGMT_0_GYRO_MODE_OFF; + ret += inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + inv_delay_ms(20); // for 20ms gyro ringdwon + INV_LOG(SENSOR_LOG_LEVEL, "acc on and gyro off pwr: 0x%x", icm_dev.pwr_mgmt); + }else if((icm_dev.sensors[WOM].powered == true) ||(icm_dev.sensors[PEDO].powered == true)){ + + icm_dev.pwr_mgmt.gyro_mode = PWR_MGMT_0_GYRO_MODE_OFF; + ret += inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + inv_imu_enable_accel_low_power_mode(0); + }else { + /* Gyro OFF & Accel off*/ + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_OFF; + icm_dev.pwr_mgmt.gyro_mode = PWR_MGMT_0_GYRO_MODE_OFF; + ret |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + inv_delay_ms(20); // for 20ms gyro ringdwon + INV_LOG(SENSOR_LOG_LEVEL, "gyro off pwr: 0x%x", icm_dev.pwr_mgmt); + } + + icm_dev.sensors[GYR].powered = false; + icm_dev.sensors[GYR].configed = false; + + if (true == icm_dev.fifo_is_used) { + inv_imu_config_fifo(gyroOdrChanged | watermarkChanged); + } + + if ((false == icm_dev.sensors[GYR].powered) + && (false == icm_dev.sensors[ACC].powered)) { + //disable fifo rcosc + inv_imu_set_autorcosc_power_on(false); + + // disable interruput + if (true == icm_dev.fifo_is_used) { + inv_imu_config_fifo_int(false); + inv_imu_config_fifofull_int(false); + } else { + inv_imu_config_drdy(false); + } + } + + #if 0 + if (gyroOdrChanged | watermarkChanged) { + //do nothing + } + #endif + + return ret; +} + +int inv_imu_acc_set_rate(float odr_hz, uint16_t packet_num,float *hw_odr) +{ + int ret = 0; + int odr_index = 0; + uint32_t sampleRate = 0; + uint32_t maxRate = 0; + bool accelOdrChanged = false; + bool watermarkChanged = false; + + INV_LOG(SENSOR_LOG_LEVEL, "%s odr_hz %f package num setting for wm %d", __func__, odr_hz, packet_num); + icm_dev.sensors[ACC].rate = SENSOR_HZ(odr_hz); + + if ((true == icm_dev.apex_enable) && + (icm_dev.sensors[ACC].rate < icm_dev.min_apex_odr)) { + INV_LOG(SENSOR_LOG_LEVEL, "APEX Enabled, Acc min odr to 50Hz!!"); + icm_dev.sensors[ACC].rate = icm_dev.min_apex_odr; + } + + if (icm_dev.sensors[ACC].preRealRate == 0) { + icm_dev.sensors[ACC].needDiscardSample = true; + icm_dev.sensors[ACC].samplesToDiscard = NUM_TODISCARD; + } + + odr_index = inv_imu_cal_odr(&icm_dev.sensors[ACC].rate, &sampleRate); + icm_dev.sensors[ACC].preRealRate = sampleRate; + + /* if gyr configed ,compare maxRate with acc and gyr rate */ + if (true == icm_dev.sensors[GYR].configed) { + maxRate = max(sampleRate, icm_dev.sensors[GYR].preRealRate);// choose with preRealRate + if (maxRate != icm_dev.sensors[ACC].hwRate || + maxRate != icm_dev.sensors[GYR].hwRate) { + icm_dev.sensors[ACC].hwRate = maxRate; + icm_dev.sensors[GYR].hwRate = maxRate; + INV_LOG(SENSOR_LOG_LEVEL, "New Acc/Gyro config Rate %f Hz", + (icm_dev.sensors[ACC].hwRate/1024.0f)); + odr_index = inv_imu_cal_odr(&maxRate, &sampleRate); + ret += inv_imu_set_odr(odr_index); + accelOdrChanged = true; + } else + accelOdrChanged = false; + } else { + if ((sampleRate != icm_dev.sensors[ACC].hwRate)) { + icm_dev.sensors[ACC].hwRate = sampleRate; + INV_LOG(SENSOR_LOG_LEVEL, "New Acc config Rate %f Hz", + (icm_dev.sensors[ACC].hwRate/1024.0f)); + ret += inv_imu_set_odr(odr_index); + /*enable ACCEL LP mode when odr < 50hz + only Accel on */ + if(odr_index <2) + inv_imu_enable_accel_low_power_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + else + inv_imu_enable_accel_low_noise_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + accelOdrChanged = true; + } else + accelOdrChanged = false; + } + icm_dev.sensors[ACC].configed = true; + *hw_odr = icm_dev.sensors[ACC].hwRate / 1024 ; + //For fifo mode + if (true == icm_dev.fifo_is_used) { + icm_dev.sensors[ACC].wm = inv_imu_cal_wm(packet_num); + if (icm_dev.sensors[ACC].wm != icm_dev.watermark) { + watermarkChanged = true; + icm_dev.watermark = icm_dev.sensors[ACC].wm; + INV_LOG(SENSOR_LOG_LEVEL, "New package num for watermark is %d", icm_dev.watermark/icm_dev.fifo_packet_size); + } else { + watermarkChanged = false; + } + inv_imu_config_fifo(accelOdrChanged | watermarkChanged); + } + + // config interruput + if (true == icm_dev.fifo_is_used) { + inv_imu_config_fifo_int(true); + inv_imu_config_fifofull_int(true); + } else { + inv_imu_config_drdy(true); + } + +#if SENSOR_REG_DUMP + inv_imu_dumpRegs(); +#endif + + return ret; +} + +int inv_imu_gyro_set_rate(float odr_hz, uint16_t packet_num,float *hw_odr) +{ + int ret = 0; + int odr_index = 0; + uint32_t sampleRate = 0; + uint32_t maxRate = 0; + bool gyroOdrChanged = false; + bool watermarkChanged = false; + + INV_LOG(SENSOR_LOG_LEVEL, "%s odr_hz %f packnum %d for setting wm", __func__, odr_hz, packet_num); + icm_dev.sensors[GYR].rate = SENSOR_HZ(odr_hz); + + if (icm_dev.sensors[GYR].preRealRate == 0) { + icm_dev.sensors[GYR].needDiscardSample = true; + icm_dev.sensors[GYR].samplesToDiscard = NUM_TODISCARD; + } + + /* get hw sample rate */ + odr_index = inv_imu_cal_odr(&icm_dev.sensors[GYR].rate, &sampleRate); + + icm_dev.sensors[GYR].preRealRate = sampleRate; + + /* if acc configed ,compare maxRate with acc and gyr rate */ + if (true == icm_dev.sensors[ACC].configed) { + maxRate = max(sampleRate, icm_dev.sensors[ACC].preRealRate); + if (maxRate != icm_dev.sensors[ACC].hwRate || + maxRate != icm_dev.sensors[GYR].hwRate) { + icm_dev.sensors[ACC].hwRate = maxRate; + icm_dev.sensors[GYR].hwRate = maxRate; + INV_LOG(SENSOR_LOG_LEVEL, "New Gyro/Acc config Rate %f Hz", + (icm_dev.sensors[GYR].hwRate/1024.0f)); + /* update new odr */ + odr_index = inv_imu_cal_odr(&maxRate, &sampleRate); + ret += inv_imu_set_odr(odr_index); + gyroOdrChanged = true; + } else + gyroOdrChanged = false; + + //confirm accel back to LN mode + inv_imu_enable_accel_low_noise_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + } else { + if ((sampleRate != icm_dev.sensors[GYR].hwRate)) { + icm_dev.sensors[GYR].hwRate = sampleRate; + INV_LOG(SENSOR_LOG_LEVEL, "New Gyro config Rate %f Hz", + (icm_dev.sensors[GYR].hwRate/1024.0f)); + /* update new odr */ + ret += inv_imu_set_odr(odr_index); + gyroOdrChanged = true; + } else + gyroOdrChanged = false; + } + + icm_dev.sensors[GYR].configed = true; + + *hw_odr = icm_dev.sensors[GYR].hwRate / 1024 ; + + //For fifo mode + if (true == icm_dev.fifo_is_used) { + icm_dev.sensors[GYR].wm = inv_imu_cal_wm(packet_num); + if (icm_dev.sensors[GYR].wm != icm_dev.watermark) { + watermarkChanged = true; + icm_dev.watermark = icm_dev.sensors[GYR].wm; + INV_LOG(SENSOR_LOG_LEVEL, "New package number for Watermark is %d", icm_dev.watermark/icm_dev.fifo_packet_size); + } else { + watermarkChanged = false; + } + inv_imu_config_fifo(gyroOdrChanged | watermarkChanged); + } + + + // config interruput + if (true == icm_dev.fifo_is_used) { + inv_imu_config_fifo_int(true); + inv_imu_config_fifofull_int(true); + } else { + inv_imu_config_drdy(true); + } + + #if 0 + if(gyroOdrChanged || watermarkChanged) + { + + //* Clear the interrupt */ + //do nothing right now + } + #endif + +#if SENSOR_REG_DUMP + inv_imu_dumpRegs(); +#endif + + return ret; +} + +static void inv_imu_parse_rawdata(uint8_t *buf, uint8_t cur_index,SensorType_t sensorType, struct accGyroData *data) +{ + uint8_t high_res_rel_addr=0; + uint16_t cur_fifo_ts = 0; + int i=0; + + // Output raw A+G data. + if( TEMP==sensorType ) + { + if (true == icm_dev.fifo_is_used) + { + if( icm_dev.fifo_highres_enabled) + /* Use little endian mode */ + data->temperature = (int16_t)(buf[0] | buf[1] << 8); + else data->temperature = (int8_t)buf[0]; + } + else + /* Use little endian mode */ + data->temperature = (int16_t)(buf[0] | buf[1] << 8); + + return; + } + + if( TS==sensorType && true == icm_dev.fifo_is_used) + { + /* Use little endian mode */ + cur_fifo_ts = (uint16_t)(buf[0] | buf[1] << 8); + + //INV_LOG(SENSOR_LOG_LEVEL, "TS reg %x %x", buf[1],buf[0]); + + if (cur_fifo_ts != icm_dev.pre_fifo_ts) { + /* Check for possible overflow */ + if (cur_fifo_ts < icm_dev.pre_fifo_ts) { + icm_dev.totol_sensor_ts += cur_fifo_ts + (0xFFFF - icm_dev.pre_fifo_ts); + } else { + icm_dev.totol_sensor_ts += (cur_fifo_ts - icm_dev.pre_fifo_ts); + } + + icm_dev.pre_fifo_ts = cur_fifo_ts; + //INV_LOG(SENSOR_LOG_LEVEL, "totol %lld cur %d, pre %d", icm_dev.totol_sensor_ts,cur_fifo_ts,icm_dev.pre_fifo_ts); + } + data->timeStamp = icm_dev.totol_sensor_ts; + } + + if( (ACC==sensorType)||(GYR==sensorType) ) + { + /* Use little endian mode */ + data->x = (int16_t)(buf[0] | buf[1] << 8); + data->y = (int16_t)(buf[2] | buf[3] << 8); + data->z = (int16_t)(buf[4] | buf[5] << 8); + data->sensType = sensorType; + + if( (icm_dev.fifo_highres_enabled)&&(icm_dev.fifo_is_used) ) + { + + high_res_rel_addr = 0 + FIFO_ACCEL_GYRO_HIGH_RES_DATA_SHIFT - cur_index; + + data->high_res[0] = buf[high_res_rel_addr]; + data->high_res[1] = buf[high_res_rel_addr+1]; + data->high_res[2] = buf[high_res_rel_addr+2]; + + if( ACC==sensorType ) + { + for (i=0; i<3; i++) + data->high_res[i] = (data->high_res[i] & 0xF0 )>>4; + } + else + { + for (i=0; i<3; i++) + data->high_res[i] &= 0x0F; + } + } + } +} + +static int inv_imu_convert_rawdata(struct accGyroDataPacket *packet) +{ + int ret = 0; + uint32_t i = 0; + uint8_t accEventSize = 0; + uint8_t gyroEventSize = 0; + uint8_t accEventSize_Discard = 0; + uint8_t gyroEventSize_Discard = 0; + uint64_t tick_ts = 0; + int16_t temper = 0; + + struct accGyroData *data = packet->outBuf; + + if (true == icm_dev.fifo_is_used && false == icm_dev.polling_data_en) { //fifo mode + for (i = 0; i < icm_dev.fifoDataToRead; i += icm_dev.fifo_packet_size) { + //INV_LOG(SENSOR_LOG_LEVEL, "Fifo head format is 0x%x", icm_dev.dataBuf[i]); + if ((accEventSize + gyroEventSize) < MAX_RECV_PACKET*2) { + if ((true == icm_dev.sensors[ACC].configed) || + (true == icm_dev.sensors[GYR].configed)){ + + inv_imu_parse_rawdata(&(icm_dev.dataBuf[i + FIFO_TEMP_DATA_SHIFT]),FIFO_TEMP_DATA_SHIFT, TEMP, &data[accEventSize + gyroEventSize]); + temper = data[accEventSize+gyroEventSize].temperature; // Copy temperature data to gyro record + // data[accEventSize+gyroEventSize-2].temperature = data[accEventSize+gyroEventSize-1].temperature; + + if(icm_dev.tmst_is_used){ + inv_imu_parse_rawdata(&(icm_dev.dataBuf[i + FIFO_TIMESTAMP_DATA_SHIFT]), FIFO_TIMESTAMP_DATA_SHIFT,TS, &data[accEventSize + gyroEventSize]); + // Copy TS data to gyro record + tick_ts = data[accEventSize + gyroEventSize].timeStamp; + } + } + + if ((true == icm_dev.sensors[ACC].powered) && + (true == icm_dev.sensors[ACC].configed)) { + if (icm_dev.sensors[ACC].samplesToDiscard) { + icm_dev.sensors[ACC].samplesToDiscard--; + accEventSize_Discard++; + } else { + inv_imu_parse_rawdata(&(icm_dev.dataBuf[i + FIFO_ACCEL_DATA_SHIFT]), FIFO_ACCEL_DATA_SHIFT,ACC, &data[accEventSize + gyroEventSize]); + accEventSize++; + } + } + if ((true == icm_dev.sensors[GYR].powered) && + (true == icm_dev.sensors[GYR].configed)) { + if (icm_dev.sensors[GYR].samplesToDiscard) { + icm_dev.sensors[GYR].samplesToDiscard--; + gyroEventSize_Discard++; + } else { + inv_imu_parse_rawdata(&(icm_dev.dataBuf[i + FIFO_GYRO_DATA_SHIFT]),FIFO_GYRO_DATA_SHIFT,GYR, &data[accEventSize + gyroEventSize]); + data[accEventSize + gyroEventSize].timeStamp = tick_ts; + data[accEventSize + gyroEventSize].temperature = temper; + gyroEventSize++; + } + } + + } else { + INV_LOG(INV_LOG_LEVEL_ERROR, "outBuf full, accEventSize = %d, gyroEventSize = %d", accEventSize, gyroEventSize); + ret = FIFO_DATA_FULL_ERROR; + } + } + } else { //dri mode or polling mode + if ((true == icm_dev.sensors[ACC].configed) && + (true == icm_dev.sensors[ACC].powered)) { + if (icm_dev.sensors[ACC].samplesToDiscard) { + icm_dev.sensors[ACC].samplesToDiscard--; + accEventSize_Discard++; + } else { + inv_imu_parse_rawdata(&(icm_dev.dataBuf[DRI_ACCEL_DATA_SHIFT]),DRI_ACCEL_DATA_SHIFT, ACC, &data[accEventSize + gyroEventSize]); + data[accEventSize + gyroEventSize].timeStamp = tick_ts; + accEventSize++; + } + } + if ((true == icm_dev.sensors[GYR].configed) && + (true == icm_dev.sensors[GYR].powered)) { + if (icm_dev.sensors[GYR].samplesToDiscard) { + icm_dev.sensors[GYR].samplesToDiscard--; + gyroEventSize_Discard++; + } else { + inv_imu_parse_rawdata(&(icm_dev.dataBuf[DRI_GYRO_DATA_SHIFT]), DRI_GYRO_DATA_SHIFT,GYR, &data[accEventSize + gyroEventSize]); + data[accEventSize + gyroEventSize].timeStamp = tick_ts; + gyroEventSize++; + } + } + if ((true == icm_dev.sensors[ACC].configed) || + (true == icm_dev.sensors[GYR].configed)) + { + inv_imu_parse_rawdata(&(icm_dev.dataBuf[0]),0, TEMP, &data[accEventSize + gyroEventSize-1]); + + // Copy temperature data to ACC record + data[accEventSize+gyroEventSize-2].temperature = data[accEventSize+gyroEventSize-1].temperature; + } + } + + packet->accOutSize = accEventSize; + packet->gyroOutSize = gyroEventSize; + packet->temperature = icm_dev.chip_temper; + packet->timeStamp = 0; + + return ret; +} + +int inv_imu_read_rawdata(void) +{ + int ret = 0; + + ret += inv_imu_read_reg(TEMP_DATA1, icm_dev.dri_packet_size, icm_dev.dataBuf); + + return ret; +} + + +int inv_imu_polling_rawdata(struct accGyroDataPacket *dataPacket) +{ + int ret = 0; + + icm_dev.dri_packet_size = DRI_14BYTES_PACKET_SIZE; + //icm_dev.polling_data_en = true; // enable polling flag when polling data + + if (dataPacket == NULL) { + INV_LOG(INV_LOG_LEVEL_ERROR, "DATAPACKET NULL POINTER!!"); + EXIT(-1); + } + + ret += inv_imu_read_rawdata(); + + if (ret != 0) { + INV_LOG(INV_LOG_LEVEL_ERROR, "Read Raw Data Error %d", ret); + return DRDY_DATA_READ_ERROR; + } + ret += inv_imu_convert_rawdata(dataPacket); + + if (ret != 0) { + INV_LOG(INV_LOG_LEVEL_ERROR, "Convert Raw Data Error %d", ret); + return DRDY_DATA_CONVERT_ERROR; + } + //icm_dev.polling_data_en = false; // disable polling flag when finish polling data + return ret; +} + + +int inv_imu_get_rawdata_interrupt(struct accGyroDataPacket *dataPacket) +{ + int_status_t int_status_reg; + int_status_drdy_t int_status_drdy; + int ret = 0; + bool pre_polling_mode = icm_dev.polling_data_en; + + if (dataPacket == NULL) { + INV_LOG(INV_LOG_LEVEL_ERROR, "DATAPACKET NULL POINTER!!"); + EXIT(-1); + } + /* Ensure data ready status bit is set */ + if((ret |= inv_imu_read_reg(INT_STATUS, 1, (uint8_t*)&int_status_reg))) + return INT_STATUS_READ_ERROR; + + if((ret |= inv_imu_read_reg(INT_STATUS_DRDY, 1, (uint8_t*)&int_status_drdy))) + return ret; + + //INV_LOG(SENSOR_LOG_LEVEL, "INT_STATUS 0x%x, int_status_drdy 0x%x", int_status,int_status_drdy); + + /* Read data from data register according to DRI (data ready interrupt) */ + //force polling sensor data once because polling request + if(icm_dev.polling_data_en){ + ret += inv_imu_polling_rawdata(dataPacket); + if (ret != 0) { + INV_LOG(INV_LOG_LEVEL_ERROR, "force polling_rawdata Error %d", ret); + return DRDY_DATA_CONVERT_ERROR; + } + } + else if ((INT_STATUS_DRDY_DATA_RDY_INT_GENERATED==int_status_drdy.data_rdy_int) && (false == icm_dev.fifo_is_used)) { + INV_LOG(SENSOR_LOG_LEVEL, "DRDY INT Detected"); + if ((false == icm_dev.sensors[ACC].configed) && + (false == icm_dev.sensors[GYR].configed)) { + INV_LOG(INV_LOG_LEVEL_ERROR, "Unexpected DRDY INTR fired"); + return DRDY_UNEXPECT_INT_ERROR; + } + + ret += inv_imu_polling_rawdata(dataPacket); + if (ret != 0) { + INV_LOG(INV_LOG_LEVEL_ERROR, "inv_imu_polling_rawdata Error %d", ret); + return DRDY_DATA_CONVERT_ERROR; + } + } + + /* Read data from FIFO */ + if (INT_STATUS_FIFO_FULL_INT_GENERATED==int_status_reg.fifo_full_int) { + INV_LOG(INV_LOG_LEVEL_ERROR, "FIFO Overflow!!!"); + // reset fifo + inv_imu_reset_fifo(); + return FIFO_OVERFLOW_ERROR; + } else if ( INT_STATUS_FIFO_THS_INT_GENERATED==int_status_reg.fifo_ths_int) { + + icm_dev.polling_data_en = false; // disable polling flag when operate fifo data + + //INV_LOG(SENSOR_LOG_LEVEL, "WM INT Detected"); + if ((false == icm_dev.sensors[ACC].configed) && + (false == icm_dev.sensors[GYR].configed)) { + INV_LOG(SENSOR_LOG_LEVEL, "Unexpected FIFO WM INTR fired"); + // reset fifo + inv_imu_reset_fifo(); + return FIFO_UNEXPECT_WM_ERROR; + } + ret += inv_imu_read_fifo(); + if (ret != 0) { + INV_LOG(INV_LOG_LEVEL_ERROR, "Fifo Data Read Error %d", ret); + return FIFO_DATA_READ_ERROR; + } + ret += inv_imu_convert_rawdata(dataPacket); + icm_dev.polling_data_en = pre_polling_mode; //recover pre polling status + + if (ret != 0) { + INV_LOG(INV_LOG_LEVEL_ERROR, "Fifo Data Convert Error %d", ret); + return FIFO_DATA_CONVERT_ERROR; + } + } + /* else: FIFO threshold was not reached and FIFO was not full */ + + return ret; +} + +void inv_apply_mounting_matrix(int32_t raw[3]) +{ + int32_t data[3]; + int i=0; + + for (i=0; i<3; i++) + data[icm_dev.cvt.axis[i]] = icm_dev.cvt.sign[i] * raw[i]; + + for (i=0; i<3; i++) + raw[i]= data[i]; +} + +int inv_data_handler(AccDataPacket *accDatabuff,GyroDataPacket *gyroDatabuff, + chip_temperature *chip_temper,bool polling) +{ + int ret=0; + int32_t data[3]; + int acc_index = 0; + int gyro_index = 0; + int i; + struct accGyroDataPacket *datapacket = (struct accGyroDataPacket *)calloc(1, sizeof(struct accGyroDataPacket)); + if (NULL == datapacket) + { + INV_LOG(INV_LOG_LEVEL_ERROR, "malloc for datapacket failed"); + ret = INV_ERROR_MEM; + goto err; + } + + AccDataPacket *AccDatabuff = accDatabuff; + GyroDataPacket *GyroDatabuff = gyroDatabuff; + icm_dev.polling_data_en = polling; + + /* Raw A+G */ + if (true == icm_dev.sensors[ACC].configed || icm_dev.sensors[GYR].configed) + { + if(0 != inv_imu_get_rawdata_interrupt(datapacket)) + { + ret = INV_ERROR; + goto err; + } + + #if !SENSOR_LOG_TS_ONLY + INV_LOG(SENSOR_LOG_LEVEL, "Get Sensor Data ACC %d GYR %d ", + datapacket->accOutSize, datapacket->gyroOutSize); + #endif + AccDatabuff->accDataSize = datapacket->accOutSize; + GyroDatabuff->gyroDataSize = datapacket->gyroOutSize; + + for (i = 0; i < datapacket->accOutSize + datapacket->gyroOutSize; i++) + { + if(IS_HIGH_RES_MODE) + { + data[0] = ((int32_t)datapacket->outBuf[i].x <<4) | datapacket->outBuf[i].high_res[0]; + data[1] = ((int32_t)datapacket->outBuf[i].y <<4) | datapacket->outBuf[i].high_res[1]; + data[2] = ((int32_t)datapacket->outBuf[i].z <<4) | datapacket->outBuf[i].high_res[2]; + } + else + { + data[0] = (int32_t)datapacket->outBuf[i].x; + data[1] = (int32_t)datapacket->outBuf[i].y; + data[2] = (int32_t)datapacket->outBuf[i].z; + } + + #if DATA_FORMAT_DPS_G // Output data in dps/g/degree format. + inv_apply_mounting_matrix(data); + + /* convert to physical unit: Celsius degree */ + if (FIFO_WM_MODE_EN && (!IS_HIGH_RES_MODE)) // Normal FIFO mode + *chip_temper = ((float)datapacket->outBuf[i].temperature * TEMP_SENSITIVITY_1_BYTE) + ROOM_TEMP_OFFSET; + else // FIFO High Resolution mode and DRI mode. + *chip_temper = ((float)datapacket->outBuf[i].temperature * TEMP_SENSITIVITY_2_BYTE) + ROOM_TEMP_OFFSET; + + if( ACC==datapacket->outBuf[i].sensType ) + { + AccDatabuff->databuff[acc_index].x = (float)data[0] * ACC_RESOLUTION_HIRES(icm_dev.acc_cfg0.accel_ui_fs_sel,IS_HIGH_RES_MODE); + AccDatabuff->databuff[acc_index].y = (float)data[1] * ACC_RESOLUTION_HIRES(icm_dev.acc_cfg0.accel_ui_fs_sel,IS_HIGH_RES_MODE); + AccDatabuff->databuff[acc_index].z = (float)data[2] * ACC_RESOLUTION_HIRES(icm_dev.acc_cfg0.accel_ui_fs_sel,IS_HIGH_RES_MODE); + AccDatabuff->databuff[acc_index].timeStamp =((uint64_t) datapacket->outBuf[i].timeStamp)*16; + + #if !SENSOR_LOG_TS_ONLY + INV_LOG(SENSOR_LOG_LEVEL, "ACC TS %lld C %.1f %.1f %.1f %.1f", AccDatabuff->databuff[acc_index].timeStamp, *chip_temper, AccDatabuff->databuff[acc_index].x, AccDatabuff->databuff[acc_index].y, AccDatabuff->databuff[acc_index].z ); + #endif + acc_index ++; + } + else if(GYR==datapacket->outBuf[i].sensType) + { + + GyroDatabuff->databuff[gyro_index].x = (float)data[0] * GYRO_RESOLUTION_HIRES(icm_dev.gyro_cfg0.gyro_ui_fs_sel,IS_HIGH_RES_MODE); + GyroDatabuff->databuff[gyro_index].y = (float)data[1] * GYRO_RESOLUTION_HIRES(icm_dev.gyro_cfg0.gyro_ui_fs_sel,IS_HIGH_RES_MODE); + GyroDatabuff->databuff[gyro_index].z = (float)data[2] * GYRO_RESOLUTION_HIRES(icm_dev.gyro_cfg0.gyro_ui_fs_sel,IS_HIGH_RES_MODE); + GyroDatabuff->databuff[gyro_index].timeStamp = ((uint64_t) datapacket->outBuf[i].timeStamp)*16; + + #if !SENSOR_LOG_TS_ONLY + INV_LOG(SENSOR_LOG_LEVEL, "GYR TS %lld C %.1f %.1f %.1f %.1f", GyroDatabuff->databuff[gyro_index].timeStamp, *chip_temper, GyroDatabuff->databuff[gyro_index].x, GyroDatabuff->databuff[gyro_index].y, GyroDatabuff->databuff[gyro_index].z ); + #endif + gyro_index ++; + } + + #else // Output data in LSB format. + if ( ACC==datapacket->outBuf[i].sensType ){ + + AccDatabuff->databuff[acc_index].x = data[0] ; + AccDatabuff->databuff[acc_index].y = data[1] ; + AccDatabuff->databuff[acc_index].z = data[2] ; + + AccDatabuff->databuff[acc_index].timeStamp = (uint64_t) datapacket->outBuf[i].timeStamp *16; + *chip_temper = datapacket->outBuf[i].temperature; + acc_index ++; + + #if !SENSOR_LOG_TS_ONLY + INV_LOG(SENSOR_LOG_LEVEL, "ACC data TS %lld C %d %d %d %d", datapacket->outBuf[i].timeStamp, datapacket->outBuf[i].temperature, data[0], data[1], data[2]); + #endif + } + else if ( GYR==datapacket->outBuf[i].sensType ){ + GyroDatabuff->databuff[gyro_index].x = data[0]; + GyroDatabuff->databuff[gyro_index].y = data[1]; + GyroDatabuff->databuff[gyro_index].z = data[2]; + GyroDatabuff->databuff[gyro_index].timeStamp =(uint64_t)datapacket->outBuf[i].timeStamp *16; + *chip_temper = datapacket->outBuf[i].temperature; + + gyro_index ++; + #if !SENSOR_LOG_TS_ONLY + INV_LOG(SENSOR_LOG_LEVEL, "GYR data TS %lld C %d %d %d %d", datapacket->outBuf[i].timeStamp, datapacket->outBuf[i].temperature, data[0], data[1], data[2]); + #endif + } + #endif + } + + } + return ret; +err: + free(datapacket); + return ret; +} + +int inv_imu_run_selftest(uint8_t acc_control,uint8_t gyro_control, struct inv_imu_selftest_output * st_output) +{ + uint8_t st_done; + int_status_t reg_int_status; + int result = 0; + int count =0; + + result |= inv_imu_switch_on_mclk(); + + result |= inv_imu_start_dmp_selftest(); + result |= inv_imu_configure_selftest_parameters(); + /* RC is kept on before this function in order to guarantee proper access to MCLK register without having to rewrite SCLK */ + result |= inv_imu_control_selftest(acc_control,gyro_control); + + /* check st_status1/2 (active polling) */ + st_done = 0; + + if( (SELFTEST_ACCEL_ST_EN_DIS==acc_control) && (SELFTEST_GYRO_ST_EN_DIS==gyro_control) ) { + /* Nothing else required if self-test is not being run */ + INV_LOG(SENSOR_LOG_LEVEL, "no selftest item enable and need to run"); + return 0; + } + + do { + inv_delay_ms(1); + result |= inv_imu_read_reg(INT_STATUS, 1, (uint8_t*)®_int_status); + st_done = reg_int_status.st_int; + count ++; + + if(count > 5000) + INV_LOG(SENSOR_LOG_LEVEL, "selftest timeout no result return %d",count); + } while ((!st_done) && (count < 5000) ); + + /* report self-test status */ + result |= inv_imu_process_selftest_end(st_output); + + /* Restore idle bit */ + result |= inv_imu_switch_off_mclk(); + + /* reset device and reinit sensor */ + result |= inv_imu_init_config(); + return result; +} + +int inv_imu_start_dmp_selftest(void) +{ + int status = 0; + mclk_rdy_t mclk_rdy; + otp_config_t otp_config; + otp_ctrl7_t otp_ctrl7; + + /* Disables Gyro/Accel sensors */ + status |= inv_imu_read_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + icm_dev.pwr_mgmt.gyro_mode = PWR_MGMT_0_GYRO_MODE_OFF; + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_OFF; + + status |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + + icm_dev.sensors[GYR].powered = false; + icm_dev.sensors[ACC].powered = false; + + // Reset SRAM to 0's + status |= inv_imu_reset_dmp(); + if(status) + return status; + + // APEX algorithms will restart from scratch after self-test + icm_dev.dmp_is_on = false; + + // Trigger OTP load for ST data + status |= inv_imu_read_reg(OTP_CONFIG, 1, (uint8_t*)&otp_config); + otp_config.otp_copy_mode = OTP_COPY_EN_DATA; + status |= inv_imu_write_reg(OTP_CONFIG, 1, (uint8_t*)&otp_config); + + status |= inv_imu_read_reg(OTP_CTRL7, 1, (uint8_t*)&otp_ctrl7); + otp_ctrl7.otp_pwr_down = OTP_CTRL7_PWR_UP; + status |= inv_imu_write_reg(OTP_CTRL7, 1, (uint8_t*)&otp_ctrl7); + + inv_delay_us(100); + + /* Host should disable INT function first before kicking off OTP copy operation */ + status |= inv_imu_read_reg(OTP_CTRL7, 1, (uint8_t*)&otp_ctrl7); + otp_ctrl7.otp_reload= OTP_CTRL7_RELOAD_EN; + status |= inv_imu_write_reg(OTP_CTRL7, 1, (uint8_t*)&otp_ctrl7); + + inv_delay_us(20); + + // Sanity check to reload autocleared and otp_done is raised + status |= inv_imu_read_reg(OTP_CTRL7, 1, (uint8_t*)&otp_ctrl7); + + status |= inv_imu_read_reg(MCLK_RDY, 1, (uint8_t*)&mclk_rdy); + if((OTP_DONE_NO==mclk_rdy.otp_done) || (otp_ctrl7.otp_reload != OTP_CTRL7_RELOAD_DIS)){ + INV_LOG(SENSOR_LOG_LEVEL, "misc_otp_value %x , otp_reload %x not right !!!!\n",mclk_rdy,otp_ctrl7.otp_reload); + return INV_ERROR_UNEXPECTED; + } + + icm_dev.int_src0.st_int1_en = 1; + status |= inv_imu_write_reg(INT_SOURCE0,1,(uint8_t*)&icm_dev.int_src0); + INV_LOG(SENSOR_LOG_LEVEL, "Enable ST INT1 0x%x", icm_dev.int_src0); + + return status; +} + +int inv_imu_configure_selftest_parameters(void) +{ + int status = 0; + selftest_t reg_selftest; + st_config_t reg_st_config; + uint8_t st_num_samples = ST_CONFIG_ST_NUMBER_SAMPLE_16; + uint8_t accel_limit = ST_CONFIG_ACCEL_ST_LIM_50; + uint8_t gyro_limit = ST_CONFIG_GYRO_ST_LIM_50; + + inv_imu_switch_on_mclk(); + + /* Self-test configuration cannot be updated if it already running */ + status |= inv_imu_read_reg(SELFTEST_MREG, 1, (uint8_t*)®_selftest); + /* make sure there is no selftest running configuration */ + if( reg_selftest.accel_st_en ==1 || reg_selftest.gyro_st_en ==1) + return INV_ERROR_UNEXPECTED; + + status |= inv_imu_read_reg(ST_CONFIG_MREG, 1, (uint8_t*)®_st_config); + reg_st_config.st_num_sample = st_num_samples; + reg_st_config.accel_st_lim = accel_limit; + reg_st_config.gyro_st_lim = gyro_limit; + status |= inv_imu_write_reg(ST_CONFIG_MREG, 1, (uint8_t*)®_st_config); + + status |= inv_imu_switch_off_mclk(); + + return status; +} + +int inv_imu_control_selftest(uint8_t acc_control,uint8_t gyro_control) +{ + /* This function can be used to either enable or abort self-test */ + selftest_t reg_selftest; + int status = 0; + + status |= inv_imu_read_reg(SELFTEST_MREG, 1, (uint8_t*)®_selftest); + reg_selftest.accel_st_en = acc_control; + reg_selftest.gyro_st_en = gyro_control; + + /* Both accel and gyro MUST be enabled simulaneously for DMP to take it into account properly */ + status |= inv_imu_write_reg(SELFTEST_MREG, 1, (uint8_t*)®_selftest); + + return status; +} + +int inv_imu_process_selftest_end(inv_imu_selftest_output_t *st_output) +{ + + int status = 0; + //uint8_t data; + st_status1_t reg_st_status1; + st_status2_t reg_st_status2; + selftest_t reg_selftest; + otp_ctrl7_t otp_ctrl7; + + inv_imu_selftest_output_t *st_result = st_output; + + status |= inv_imu_read_reg(ST_STATUS1_MREG, 1, (uint8_t*)®_st_status1); + status |= inv_imu_read_reg(ST_STATUS2_MREG, 1, (uint8_t*)®_st_status2); + + INV_LOG(SENSOR_LOG_LEVEL, "ST_STATUS1 0x%x ST_STATUS2 0x%x ", reg_st_status1,reg_st_status2); + if(0 == status) { + st_result->accel_status = reg_st_status1.accel_st_pass; + st_result->gyro_status = reg_st_status2.gyro_st_pass | (reg_st_status2.st_incomplete << 1); + } + + if (st_result->accel_status == 0x1) + INV_LOG(SENSOR_LOG_LEVEL, "accel Selftest PASS\n"); + else + INV_LOG(SENSOR_LOG_LEVEL, "accel Selftest fail\n"); + + if (st_result->gyro_status == 0x1){ + INV_LOG(SENSOR_LOG_LEVEL, "gyro Selftest PASS\n"); + }else{ + INV_LOG(SENSOR_LOG_LEVEL, "gyro Selftest fail\n"); + } + + /* Turn off self-test */ + status |= inv_imu_read_reg(SELFTEST_MREG, 1, (uint8_t*)®_selftest); + reg_selftest.accel_st_en = 0; + reg_selftest.gyro_st_en = 0; + status |= inv_imu_write_reg(SELFTEST_MREG, 1, (uint8_t*)®_selftest); + + /* Re-sync all clocking scheme with a dummy write + data = 0; + status |= inv_imu_write_reg(WHO_AM_I, 1, &data);*/ + + /* Turn off OTP macro */ + + status |= inv_imu_read_reg(OTP_CTRL7, 1, (uint8_t*)&otp_ctrl7); + otp_ctrl7.otp_pwr_down = OTP_CTRL7_PWR_DOWN; + status |= inv_imu_write_reg(OTP_CTRL7, 1, (uint8_t*)&otp_ctrl7); + + inv_delay_ms(20);//100-1000u + + return status; +} + +/* DMP Power save mode */ +static int inv_imu_dmp_powersave(bool powersave) +{ + int status = 0; + apex_config0_t reg_apex_config0; + wom_config_t wom_config_reg; + /* APEX_CONFIG0 */ + status |= inv_imu_read_reg(APEX_CONFIG0, 1, (uint8_t*)®_apex_config0); + + if(true == powersave){ + reg_apex_config0.dmp_power_save_en = APEX_CONFIG0_DMP_POWER_SAVE_EN; + } else { + reg_apex_config0.dmp_power_save_en = APEX_CONFIG0_DMP_POWER_SAVE_DIS; + } + + status |= inv_imu_write_reg(APEX_CONFIG0, 1, (uint8_t*)®_apex_config0); + + if(true == powersave){; + + /* Enable WOM */ + status |= inv_imu_read_reg(WOM_CONFIG, 1, (uint8_t*)&wom_config_reg); + wom_config_reg.wom_en = WOM_CONFIG_WOM_EN_ENABLE; + status |= inv_imu_write_reg(WOM_CONFIG, 1, (uint8_t*)&wom_config_reg); + } + INV_LOG(SENSOR_LOG_LEVEL, "DMP_powersave apex_config0 %x, wom_cfg %x \n", reg_apex_config0,wom_config_reg); + + return status; +} + + +static int resume_dmp(void) +{ + int status = 0; + apex_config0_t reg_apex_config0; + uint16_t count=0; + + status |= inv_imu_read_reg(APEX_CONFIG0, 1, (uint8_t*)®_apex_config0); + reg_apex_config0.dmp_init_en = APEX_CONFIG0_DMP_INIT_EN; + status |= inv_imu_write_reg(APEX_CONFIG0, 1, (uint8_t*)®_apex_config0); + + INV_LOG(SENSOR_LOG_LEVEL, "resume_dmp apex_config0 %x\n", reg_apex_config0); + + inv_delay_ms(50); //test 50ms first + /* wait to make sure dmp_init_en = 0 */ + do { + inv_delay_us(100); + inv_imu_read_reg(APEX_CONFIG0, 1, (uint8_t*)®_apex_config0); + + if (reg_apex_config0.dmp_init_en== 0) + break; + count++; + } while (count < 500); + + return status; +} + +static int inv_imu_start_dmp(void) +{ + int status = 0; + + // On first enabling of DMP, reset internal state + if(!icm_dev.dmp_is_on) { + // Reset SRAM to 0's + status |= inv_imu_reset_dmp(); + if(status) + return status; + icm_dev.dmp_is_on = true; + } + + // Initialize DMP + status |= resume_dmp(); + + return status; +} + +int inv_imu_reset_dmp(void) +{ + const int ref_timeout = 5000; /*50 ms*/ + int status = 0; + int timeout = ref_timeout; + apex_config0_t reg_apex_config0; + + status |= inv_imu_switch_on_mclk(); + + // Reset DMP internal memories + status |= inv_imu_read_reg(APEX_CONFIG0, 1, (uint8_t*)®_apex_config0); + reg_apex_config0.dmp_mem_reset_en = APEX_CONFIG0_DMP_MEM_RESET_EN; + status |= inv_imu_write_reg(APEX_CONFIG0, 1, (uint8_t*)®_apex_config0); + + INV_LOG(SENSOR_LOG_LEVEL, "inv_imu_reset_dmp w APEX_CONFIG0 %x \n", reg_apex_config0); + + inv_delay_ms(1); + + // Make sure reset procedure has finished by reading back mem_reset_en bit + do { + inv_delay_us(10); + status |= inv_imu_read_reg(APEX_CONFIG0, 1, (uint8_t*)®_apex_config0); + } while ((reg_apex_config0.dmp_mem_reset_en != APEX_CONFIG0_DMP_MEM_RESET_DIS) && timeout-- && !status); + + status |= inv_imu_switch_off_mclk(); + + if (timeout <= 0){ + INV_LOG(SENSOR_LOG_LEVEL, "time out reg %x \n", reg_apex_config0); + return INV_ERROR_TIMEOUT; + } + + return status; +} + +int inv_imu_configure_wom(const uint8_t wom_x_th, const uint8_t wom_y_th, const uint8_t wom_z_th, + WOM_CONFIG_WOM_INT_MODE_t wom_int, WOM_CONFIG_WOM_INT_DUR_t wom_dur) +{ + int status = 0; + uint8_t data[3]; + wom_config_t wom_config_reg; + + data[0] = wom_x_th; // Set X threshold + data[1] = wom_y_th; // Set Y threshold + data[2] = wom_z_th; // Set Z threshold + status |= inv_imu_write_reg(ACCEL_WOM_X_THR_MREG, sizeof(data), &data[0]); + + // Compare current sample with the previous sample and WOM from the 3 axis are ORed or ANDed to produce WOM signal. + status |= inv_imu_read_reg(WOM_CONFIG, 1, (uint8_t*)&wom_config_reg); + wom_config_reg.wom_int_mode = wom_int; + wom_config_reg.wom_mode = WOM_CONFIG_WOM_MODE_CMP_PREV; + + // Configure the number of overthreshold event to wait before producing the WOM signal. + + wom_config_reg.wom_int_dur = wom_dur; + status |= inv_imu_write_reg(WOM_CONFIG, 1, (uint8_t*)&wom_config_reg); + + return status; +} + +int inv_imu_enable_wom_register(void) +{ + int status = 0; + wom_config_t wom_config_reg; + int_source1_t int_source1_reg; + + /* Enable WOM */ + status |= inv_imu_read_reg(WOM_CONFIG, 1, (uint8_t*)&wom_config_reg); + wom_config_reg.wom_en = WOM_CONFIG_WOM_EN_ENABLE; + status |= inv_imu_write_reg(WOM_CONFIG, 1, (uint8_t*)&wom_config_reg); + + /* enable wom int1 */ + status |= inv_imu_read_reg(INT_SOURCE1, 1, (uint8_t*)&int_source1_reg); + int_source1_reg.wom_x_int1_en = 1; + int_source1_reg.wom_y_int1_en = 1; + int_source1_reg.wom_z_int1_en = 1; + status |= inv_imu_write_reg(INT_SOURCE1, 1, (uint8_t*)&int_source1_reg); + + return status; +} + +int inv_imu_disable_wom_register(bool wom_disable) +{ + int status = 0; + wom_config_t wom_config_reg; + int_source1_t int_source1_reg; + + /* disable WOM */ + if(wom_disable){ + status |= inv_imu_read_reg(WOM_CONFIG, 1, (uint8_t*)&wom_config_reg); + wom_config_reg.wom_en = WOM_CONFIG_WOM_EN_DISABLE; + status |= inv_imu_write_reg(WOM_CONFIG, 1, (uint8_t*)&wom_config_reg); + } + + /* disable wom int1 */ + status |= inv_imu_read_reg(INT_SOURCE1, 1, (uint8_t*)&int_source1_reg); + int_source1_reg.wom_x_int1_en = 0; + int_source1_reg.wom_y_int1_en = 0; + int_source1_reg.wom_z_int1_en = 0; + status |= inv_imu_write_reg(INT_SOURCE1, 1, (uint8_t*)&int_source1_reg); + + return status; +} + +int inv_imu_wom_enable(uint8_t wom_threshold_x,uint8_t wom_threshold_y,uint8_t wom_threshold_z,uint8_t duration) +{ + int ret = 0; + uint8_t regValue = 0; + int odr_index = 0; + uint32_t sampleRate = 0; + + // if acc is not in streaming mode enable acc + if (!icm_dev.sensors[ACC].configed) { + if(icm_dev.sensors[GYR].configed) + icm_dev.sensors[ACC].hwRate = icm_dev.sensors[GYR].hwRate; + else + icm_dev.sensors[ACC].hwRate = icm_dev.min_apex_odr; + + odr_index = inv_imu_cal_odr(&icm_dev.sensors[ACC].hwRate, &sampleRate); + regValue = IMU_ODR_MAPPING[odr_index]; + INV_LOG(SENSOR_LOG_LEVEL, "set acc Odr Reg value 0x%x", regValue); + /* update new odr */ + + icm_dev.acc_cfg0.accel_odr = regValue; + ret += inv_imu_write_reg(ACCEL_CONFIG0,1, (uint8_t*)&icm_dev.acc_cfg0); + INV_LOG(SENSOR_LOG_LEVEL, "write ACCEL_CONFIG0 0x%x", icm_dev.acc_cfg0); + + + inv_delay_us(200); + INV_LOG(SENSOR_LOG_LEVEL, "wom enable, acc on"); + } + + //config acc power mode + if((!icm_dev.sensors[ACC].configed) && (!icm_dev.sensors[GYR].configed)) + ret += inv_imu_enable_accel_low_power_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + else + ret += inv_imu_enable_accel_low_noise_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + + /* set X,Y,Z threshold */ + ret += inv_imu_configure_wom(wom_threshold_x,wom_threshold_y,wom_threshold_z, + WOM_CONFIG_WOM_INT_MODE_ORED,(WOM_CONFIG_WOM_INT_DUR_t)duration); + + ret += inv_imu_enable_wom_register(); + if (ret != 0) + return WOM_ENABLE_ERROR; + + icm_dev.sensors[WOM].powered = true; + + #if SENSOR_REG_DUMP + inv_imu_dumpRegs(); + #endif + + return ret; +} + +int inv_imu_wom_disable() +{ + int ret = 0; + + INV_LOG(SENSOR_LOG_LEVEL, "%s", __func__); + + if(icm_dev.sensors[PEDO].powered == false) + ret |= inv_imu_disable_wom_register(true); + else + ret |= inv_imu_disable_wom_register(false); + + if (false == icm_dev.sensors[ACC].powered && + false == icm_dev.sensors[ACC].configed && + false == icm_dev.sensors[FF].powered ) { + + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_OFF; + ret |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + inv_delay_us(200); //spec: 200us + + INV_LOG(SENSOR_LOG_LEVEL, "wom disable, acc off pwr: 0x%x", icm_dev.pwr_mgmt); + } + + icm_dev.sensors[WOM].powered = false; + + return ret; +} + +int inv_imu_wom_get_event(bool *wom_detect) +{ + int_status2_t int_status2; + int rc = 0; + int wom_flag = 0; + + if (wom_detect == NULL) { + INV_LOG(INV_LOG_LEVEL_ERROR, "EVENT NULL POINTER!!"); + EXIT(-1); + } + /* + * Read WOM interrupt status + */ + rc = inv_imu_read_reg(INT_STATUS2, 1, (uint8_t*)&int_status2); + if (rc != INV_ERROR_SUCCESS) + return -1; + + if ( (INT_STATUS2_WOM_X_INT_GENERATED==int_status2.wom_x_int) || + (INT_STATUS2_WOM_Y_INT_GENERATED==int_status2.wom_y_int) || + (INT_STATUS2_WOM_Z_INT_GENERATED==int_status2.wom_z_int) ) { + if(INT_STATUS2_WOM_Z_INT_GENERATED==int_status2.wom_z_int) wom_flag = 0x01; + if(INT_STATUS2_WOM_Y_INT_GENERATED==int_status2.wom_y_int) wom_flag |= 0x02; + if(INT_STATUS2_WOM_X_INT_GENERATED==int_status2.wom_x_int) wom_flag |= 0x04; + + INV_LOG(SENSOR_LOG_LEVEL, "WoM interrupt at (X, Y, Z): %d, %d, %d", + int_status2.wom_x_int,int_status2.wom_y_int, int_status2.wom_z_int ); + + *wom_detect = true; + } + + return wom_flag; +} + +int inv_imu_enable_pedo_register(void) +{ + int status = 0; + apex_config1_t reg_apex_config1; + int_source6_t reg_int_source6_t; + + /* enable FF int1 */ + status |= inv_imu_read_reg(INT_SOURCE6_MREG, 1, (uint8_t*)®_int_source6_t); + reg_int_source6_t.step_cnt_ovfl_int1_en = EN; + reg_int_source6_t.step_det_int1_en = EN; + status |= inv_imu_write_reg(INT_SOURCE6_MREG, 1, (uint8_t*)®_int_source6_t); + + inv_delay_ms(50); + + /* Enable PEDO */ + status |= inv_imu_read_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + reg_apex_config1.ped_en = APEX_CONFIG1_PEDO_EN_EN; + status |= inv_imu_write_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + + return status; +} + +int inv_imu_disable_pedo_register(void) +{ + int status = 0; + apex_config1_t reg_apex_config1; + int_source6_t reg_int_source6_t; + + /* disable PEDO int1 */ + status |= inv_imu_read_reg(INT_SOURCE6_MREG, 1, (uint8_t*)®_int_source6_t); + reg_int_source6_t.step_cnt_ovfl_int1_en = DIS; + reg_int_source6_t.step_det_int1_en = DIS; + status |= inv_imu_write_reg(INT_SOURCE6_MREG, 1, (uint8_t*)®_int_source6_t); + + /* disable PEDO */ + status |= inv_imu_read_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + reg_apex_config1.ped_en = APEX_CONFIG1_PEDO_EN_EN; + status |= inv_imu_write_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + return status; +} + + +int inv_imu_enable_ff_register(void) +{ + int status = 0; + apex_config1_t reg_apex_config1; + int_source6_t reg_int_source6_t; + + /* enable FF int1 */ + status |= inv_imu_read_reg(INT_SOURCE6_MREG, 1, (uint8_t*)®_int_source6_t); + reg_int_source6_t.ff_int1_en = EN; + + status |= inv_imu_write_reg(INT_SOURCE6_MREG, 1, (uint8_t*)®_int_source6_t); + + inv_delay_ms(50); + + /* Enable FF */ + status |= inv_imu_read_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + reg_apex_config1.ff_en = APEX_CONFIG1_DMP_FF_EN; + status |= inv_imu_write_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + + return status; +} + +int inv_imu_disable_ff_register(void) +{ + int status = 0; + apex_config1_t reg_apex_config1; + int_source6_t reg_int_source6_t; + + /* disable FF int1 */ + status |= inv_imu_read_reg(INT_SOURCE6_MREG, 1, (uint8_t*)®_int_source6_t); + reg_int_source6_t.ff_int1_en = DIS; + reg_int_source6_t.lowg_int1_en = DIS; + status |= inv_imu_write_reg(INT_SOURCE6_MREG, 1, (uint8_t*)®_int_source6_t); + + /* disable FF */ + status |= inv_imu_read_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + reg_apex_config1.ff_en = APEX_CONFIG1_DMP_FF_DIS; + status |= inv_imu_write_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + return status; +} + +int inv_imu_apex_set_frequency(const APEX_CONFIG1_DMP_ODR_t frequency) +{ + int status = 0; + apex_config1_t reg_apex_config1; + + status |= inv_imu_read_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + + reg_apex_config1.dmp_odr = frequency; + + status |= inv_imu_write_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + return status; +} + +int inv_imu_apex_init_parameters_struct( inv_imu_apex_parameters_t *apex_inputs) +{ + int status = 0; + + /* Default parameters at POR */ + apex_inputs->pedo_amp_th = APEX_CONFIG3_PEDO_AMP_TH_2080374_MG; + apex_inputs->pedo_step_cnt_th = 0x5; + apex_inputs->pedo_step_det_th = APEX_CONFIG4_PEDO_SB_TIMER_TH_100_SAMPLES; + apex_inputs->pedo_sb_timer_th = APEX_CONFIG4_PEDO_SB_TIMER_TH_100_SAMPLES; + apex_inputs->pedo_hi_enrgy_th = APEX_CONFIG4_PEDO_HI_ENRGY_TH_107; + apex_inputs->tilt_wait_time = APEX_CONFIG5_TILT_WAIT_TIME_4S; + apex_inputs->power_save_time = APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_8S; + apex_inputs->sensitivity_mode = APEX_CONFIG9_SENSITIVITY_MODE_NORMAL; + apex_inputs->low_energy_amp_th = APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_2684354MG; + 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_563MG; + apex_inputs->lowg_peak_hyst = APEX_CONFIG5_LOWG_PEAK_TH_HYST_156MG; + apex_inputs->lowg_samples_th = APEX_CONFIG10_LOWG_TIME_TH_5_SAMPLES; + apex_inputs->highg_peak_th = APEX_CONFIG11_HIGHG_PEAK_TH_2500MG; + apex_inputs->highg_peak_hyst = APEX_CONFIG5_HIGHG_PEAK_TH_HYST_156MG; + apex_inputs->highg_samples_th = APEX_CONFIG11_HIGHG_TIME_TH_1_SAMPLE; + + + return status; +} + +int inv_imu_apex_configure_parameters(const inv_imu_apex_parameters_t *apex_inputs) +{ + int status = 0; + apex_config2_t reg_apex_config2; + apex_config3_t reg_apex_config3; + apex_config4_t reg_apex_config4; + apex_config5_t reg_apex_config5; + apex_config9_t reg_apex_config9; + apex_config10_t reg_apex_config10; + apex_config11_t reg_apex_config11; + apex_config12_t reg_apex_config12; + + status |= inv_imu_switch_on_mclk(); + + /* Power Save mode parameters & Additionnal parameters for Pedometer in Slow Walk mode */ + /* APEX_CONFIG2_MREG_TOP1 */ + reg_apex_config2.dmp_power_save_time_sel = apex_inputs->power_save_time; + reg_apex_config2.low_energy_amp_th_sel = apex_inputs->low_energy_amp_th; + status |= inv_imu_write_reg(APEX_CONFIG2_MREG, 1, (uint8_t*)®_apex_config2 ); + + /* Pedometer parameters */ + /* APEX_CONFIG3_MREG_TOP1 */ + reg_apex_config3.pedo_amp_th_sel = apex_inputs->pedo_amp_th; + reg_apex_config3.pedo_step_cnt_th_sel = apex_inputs->pedo_step_cnt_th; + status |= inv_imu_write_reg(APEX_CONFIG3_MREG, 1, (uint8_t*)®_apex_config3 ); + + /* APEX_CONFIG4_MREG_TOP1 */ + reg_apex_config4.pedo_step_det_th_sel = apex_inputs->pedo_step_det_th; + reg_apex_config4.pedo_sb_timer_th_sel = apex_inputs->pedo_sb_timer_th; + reg_apex_config4.pedo_hi_enrgy_th_sel = apex_inputs->pedo_hi_enrgy_th; + status |= inv_imu_write_reg(APEX_CONFIG4_MREG, 1, (uint8_t*)®_apex_config4 ); + + /* Tilt, Lowg and highg parameters */ + /* APEX_CONFIG5_MREG_TOP1 */ + reg_apex_config5.tilt_wait_time_sel = apex_inputs->tilt_wait_time; + reg_apex_config5.lowg_peak_th_hyst_sel = apex_inputs->lowg_peak_hyst; + reg_apex_config5.highg_peak_th_hyst_sel = apex_inputs->highg_peak_hyst; + status |= inv_imu_write_reg(APEX_CONFIG5_MREG, 1, (uint8_t*)®_apex_config5 ); + + /* free fall parameter, SMD parameter and parameters for Pedometer in Slow Walk mode */ + /* APEX_CONFIG9_MREG_TOP1 */ + reg_apex_config9.ff_debounce_duration_sel = apex_inputs->ff_debounce_duration; + reg_apex_config9.smd_sensitivity_sel = apex_inputs->smd_sensitivity; + reg_apex_config9.sensitivity_mode = apex_inputs->sensitivity_mode; + status |= inv_imu_write_reg(APEX_CONFIG9_MREG, 1, (uint8_t*)®_apex_config9 ); + + /* Lowg and highg parameters and free fall parameters */ + /* APEX_CONFIG10_MREG_TOP1 */ + reg_apex_config10.lowg_peak_th_sel = apex_inputs->lowg_peak_th; + reg_apex_config10.lowg_time_th_sel = apex_inputs->lowg_samples_th; + status |= inv_imu_write_reg(APEX_CONFIG10_MREG, 1, (uint8_t*)®_apex_config10 ); + + /* APEX_CONFIG11_MREG_TOP1 */ + reg_apex_config11.highg_peak_th_sel = apex_inputs->highg_peak_th; + reg_apex_config11.highg_time_th_sel = apex_inputs->highg_samples_th; + status |= inv_imu_write_reg(APEX_CONFIG11_MREG, 1, (uint8_t*)®_apex_config11 ); + + + /* APEX_CONFIG12_MREG_TOP1 */ + reg_apex_config12.ff_max_duration_sel = apex_inputs->ff_max_duration_cm; + reg_apex_config12.ff_min_duration_sel = apex_inputs->ff_min_duration_cm; + + status |= inv_imu_write_reg(APEX_CONFIG12_MREG, 1, (uint8_t*)®_apex_config12 ); + + status |= inv_imu_switch_off_mclk(); + + return status; +} + +int inv_imu_apex_get_data_activity(inv_imu_apex_step_activity_t * apex_activity) +{ + uint8_t data[4]; + int status = inv_imu_read_reg(APEX_DATA0, 4, data); + + apex_activity->step_cnt = 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_pedometer_get_event(float *cadence_step_per_sec,APEX_DATA3_ACTIVITY_CLASS_t *activity_class) +{ + int_status3_t reg_int_status3; + int rc = 0; + float nb_samples = 0; + static uint64_t step_cnt = 0; + /* + * Read Pedometer interrupt status + */ + rc = inv_imu_read_reg(INT_STATUS3, 1, (uint8_t*)®_int_status3); + if (rc != INV_ERROR_SUCCESS) + return rc; + + if (reg_int_status3.step_det_int) { + + uint8_t step_cnt_ovflw = (reg_int_status3.step_cnt_ovf_int) ? 1 : 0; + + inv_imu_apex_step_activity_t apex_data0; + rc |= inv_imu_apex_get_data_activity(&apex_data0); + + + /* Converting u6.2 to float */ + nb_samples = (apex_data0.step_cadence >> 2) + (float)(apex_data0.step_cadence & 0x03)*0.25; + *cadence_step_per_sec = (float) 50 / nb_samples; + + if (rc != INV_ERROR_SUCCESS) + return rc; + + if (step_cnt == apex_data0.step_cnt + step_cnt_ovflw * (uint64_t)65536) + return rc; + + step_cnt = apex_data0.step_cnt + step_cnt_ovflw * 65536; + *activity_class = (APEX_DATA3_ACTIVITY_CLASS_t)apex_data0.activity_class; + + switch (apex_data0.activity_class) { + case APEX_DATA3_ACTIVITY_CLASS_WALK: + INV_LOG(SENSOR_LOG_LEVEL, "%d steps - cadence: %.2f steps/sec - WALK", + (uint32_t)step_cnt, + *cadence_step_per_sec); + break; + case APEX_DATA3_ACTIVITY_CLASS_RUN: + INV_LOG(SENSOR_LOG_LEVEL, "%d steps - cadence: %.2f steps/sec - RUN", + (uint32_t)step_cnt, + *cadence_step_per_sec); + break; + default: + INV_LOG(SENSOR_LOG_LEVEL, ": %d steps - cadence: %.2f steps/sec", + (uint32_t)step_cnt, + *cadence_step_per_sec); + break; + } + } + + return step_cnt; +} + +int inv_imu_pedometer_enable(void) +{ + int ret = 0; + uint8_t regValue = 0; + int odr_index = 0; + uint32_t sampleRate = 0; + + if(icm_dev.min_apex_odr < SENSOR_HZ(50)){ + icm_dev.pre_min_apex_odr = icm_dev.min_apex_odr; + icm_dev.min_apex_odr = SENSOR_HZ(50); + } + + //record previous chip hardware_rate + icm_dev.pre_hwRate = max(icm_dev.sensors[ACC].hwRate,icm_dev.sensors[GYR].hwRate); + + if(icm_dev.pre_hwRate < icm_dev.min_apex_odr) { + //update new hardware rate + //icm4x6xx_reconfig_samplerate(mTask.hw_min_rate,mTask.hw_min_rate); + + // temp code we will use samplerate reconfig function later. + icm_dev.sensors[ACC].hwRate = icm_dev.min_apex_odr; + if(icm_dev.sensors[GYR].configed == true){ + icm_dev.sensors[GYR].hwRate = icm_dev.min_apex_odr; + } + odr_index = inv_imu_cal_odr(&icm_dev.min_apex_odr, &sampleRate); + regValue = IMU_ODR_MAPPING[odr_index]; + INV_LOG(SENSOR_LOG_LEVEL, "set acc Odr Reg value 0x%x", regValue); + /* update new odr */ + + icm_dev.acc_cfg0.accel_odr = regValue; + ret += inv_imu_write_reg(ACCEL_CONFIG0,1, (uint8_t*)&icm_dev.acc_cfg0); + INV_LOG(SENSOR_LOG_LEVEL, "write ACCEL_CONFIG0 0x%x", icm_dev.acc_cfg0); + + if(icm_dev.sensors[GYR].configed == true){ + icm_dev.gyro_cfg0.gyro_odr = regValue; + ret |= inv_imu_write_reg(GYRO_CONFIG0, 1, (uint8_t*)&icm_dev.gyro_cfg0); + INV_LOG(SENSOR_LOG_LEVEL, "write GYRO_CONFIG0 0x%x", icm_dev.gyro_cfg0); + } + + inv_delay_us(200); + } + + // if acc is not in streaming mode enable acc current work in LN mode first + if((!icm_dev.sensors[ACC].configed) && (!icm_dev.sensors[GYR].configed)) + ret += inv_imu_enable_accel_low_power_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + else + ret += inv_imu_enable_accel_low_noise_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + + + INV_LOG(SENSOR_LOG_LEVEL, "FF enable, acc on"); + + + if(false == icm_dev.dmp_is_on){ + ret += inv_imu_dmp_powersave(DMP_POWERSAVE_PEDO); + } + + ret += inv_imu_apex_set_frequency(APEX_CONFIG1_DMP_ODR_50Hz); + ret += inv_imu_start_dmp(); + ret += inv_imu_enable_pedo_register(); + + if (ret != 0) + return FF_ENABLE_ERROR; + + icm_dev.sensors[PEDO].powered = true; + + #if SENSOR_REG_DUMP + inv_imu_dumpRegs(); + #endif + + return ret; +} + +int inv_imu_pedometer_disable(void) +{ + int ret = 0; + + INV_LOG(SENSOR_LOG_LEVEL, "%s", __func__); + + ret |= inv_imu_disable_pedo_register(); + icm_dev.min_apex_odr = icm_dev.pre_min_apex_odr; + + if (false == icm_dev.sensors[ACC].powered && + false == icm_dev.sensors[ACC].configed && + false == icm_dev.sensors[WOM].powered && + false == icm_dev.sensors[PEDO].powered) { + + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_OFF; + ret |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + inv_delay_us(200); //spec: 200us + + INV_LOG(SENSOR_LOG_LEVEL, "wom disable, acc off pwr: 0x%x", icm_dev.pwr_mgmt); + } + + icm_dev.sensors[FF].powered = false; + + return ret; +} + + +int inv_imu_freefall_enable(void) +{ + int ret = 0; + uint8_t regValue = 0; + int odr_index = 0; + uint32_t sampleRate = 0; + + if(icm_dev.min_apex_odr < SENSOR_HZ(400)){ + icm_dev.pre_min_apex_odr = icm_dev.min_apex_odr; + icm_dev.min_apex_odr = SENSOR_HZ(400); + } + + //record previous chip hardware_rate + icm_dev.pre_hwRate = max(icm_dev.sensors[ACC].hwRate,icm_dev.sensors[GYR].hwRate); + + if(icm_dev.pre_hwRate < icm_dev.min_apex_odr) { + //update new hardware rate + //icm4x6xx_reconfig_samplerate(mTask.hw_min_rate,mTask.hw_min_rate); + + // temp code we will use samplerate reconfig function later. + icm_dev.sensors[ACC].hwRate = icm_dev.min_apex_odr; + if(icm_dev.sensors[GYR].configed == true){ + icm_dev.sensors[GYR].hwRate = icm_dev.min_apex_odr; + } + odr_index = inv_imu_cal_odr(&icm_dev.min_apex_odr, &sampleRate); + regValue = IMU_ODR_MAPPING[odr_index]; + INV_LOG(SENSOR_LOG_LEVEL, "set acc Odr Reg value 0x%x", regValue); + /* update new odr */ + + icm_dev.acc_cfg0.accel_odr = regValue; + ret += inv_imu_write_reg(ACCEL_CONFIG0,1, (uint8_t*)&icm_dev.acc_cfg0); + INV_LOG(SENSOR_LOG_LEVEL, "write ACCEL_CONFIG0 0x%x", icm_dev.acc_cfg0); + + if(icm_dev.sensors[GYR].configed == true){ + icm_dev.gyro_cfg0.gyro_odr = regValue; + ret |= inv_imu_write_reg(GYRO_CONFIG0, 1, (uint8_t*)&icm_dev.gyro_cfg0); + INV_LOG(SENSOR_LOG_LEVEL, "write GYRO_CONFIG0 0x%x", icm_dev.gyro_cfg0); + } + + inv_delay_us(200); + } + + // if acc is not in streaming mode enable acc current work in LN mode first + if((!icm_dev.sensors[ACC].configed) && (!icm_dev.sensors[GYR].configed)) +// ret += inv_imu_enable_accel_low_power_mode(1024000000 / icm_dev.sensors[ACC].hwRate); +// else + ret += inv_imu_enable_accel_low_noise_mode(1024000000 / icm_dev.sensors[ACC].hwRate); + + + INV_LOG(SENSOR_LOG_LEVEL, "FF enable, acc on"); + + + if(false == icm_dev.dmp_is_on){ + ret += inv_imu_dmp_powersave(DMP_POWERSAVE_FF); + } + + ret += inv_imu_apex_set_frequency(APEX_CONFIG1_DMP_ODR_400Hz); + ret += inv_imu_start_dmp(); + ret += inv_imu_enable_ff_register(); + + if (ret != 0) + return FF_ENABLE_ERROR; + + icm_dev.sensors[FF].powered = true; + + #if SENSOR_REG_DUMP + inv_imu_dumpRegs(); + #endif + + return ret; +} + +int inv_imu_freefall_disable(void) +{ + int ret = 0; + + INV_LOG(SENSOR_LOG_LEVEL, "%s", __func__); + + ret |= inv_imu_disable_ff_register(); + icm_dev.min_apex_odr = icm_dev.pre_min_apex_odr; + + if (false == icm_dev.sensors[ACC].powered && + false == icm_dev.sensors[ACC].configed && + false == icm_dev.sensors[WOM].powered && + false == icm_dev.sensors[PEDO].powered) { + + icm_dev.pwr_mgmt.accel_mode = PWR_MGMT_0_ACCEL_MODE_OFF; + ret |= inv_imu_write_reg(PWR_MGMT_0, 1, (uint8_t*)&icm_dev.pwr_mgmt); + inv_delay_us(200); //spec: 200us + + INV_LOG(SENSOR_LOG_LEVEL, "wom disable, acc off pwr: 0x%x", icm_dev.pwr_mgmt); + } + + icm_dev.sensors[FF].powered = false; + + return ret; +} + +float convert_ff_duration_sample_to_cm(uint16_t ff_duration_samples) +{ + int rc = 0; + apex_config1_t reg_apex_config1; + APEX_CONFIG1_DMP_ODR_t dmp_odr; + uint32_t dmp_odr_us = 0; + uint16_t ff_duration_ms; + float ff_duration_cm; + + rc = inv_imu_read_reg(APEX_CONFIG1, 1, (uint8_t*)®_apex_config1); + + if (rc != 0) + return -1; + + dmp_odr = (APEX_CONFIG1_DMP_ODR_t) (reg_apex_config1.dmp_odr); + + switch (dmp_odr) { + case APEX_CONFIG1_DMP_ODR_25Hz: dmp_odr_us = 40000; break; + case APEX_CONFIG1_DMP_ODR_50Hz: dmp_odr_us = 20000; break; + case APEX_CONFIG1_DMP_ODR_100Hz: dmp_odr_us = 10000; break; + case APEX_CONFIG1_DMP_ODR_400Hz: dmp_odr_us = 2500; break; + } + + ff_duration_ms = (ff_duration_samples * dmp_odr_us) / 1000; + // dist = speed_average * time = (9.81 * time ^ 2) / 2) + ff_duration_cm = (9.81 * ff_duration_ms * ff_duration_ms) / 2 / 10000 /* to cm/s^2 */; + + return ff_duration_cm; +} + +float inv_imu_freefall_get_event(bool *ff_detect) +{ + + float ff_duration_cm=0.0f; + int_status3_t reg_int_status3; + int rc = 0; + uint16_t ff_duration_samples; + uint8_t data[2]; + + if (ff_detect == NULL) { + INV_LOG(INV_LOG_LEVEL_ERROR, "EVENT NULL POINTER!!"); + return FF_GETEVENT_ERROR; + } + /* + * Read FF interrupt status + */ + rc = inv_imu_read_reg(INT_STATUS3, 1, (uint8_t*)®_int_status3); + + if (rc != INV_ERROR_SUCCESS) + return -1; + + if (reg_int_status3.lg_det_int) { + INV_LOG(SENSOR_LOG_LEVEL, "low g detect") ; + } + + if (reg_int_status3.ff_det_int) { + rc |= inv_imu_read_reg(APEX_DATA4, 2, &data[0]); + ff_duration_samples = (data[1] << 8) | data[0]; + + ff_duration_cm = convert_ff_duration_sample_to_cm(ff_duration_samples); + + INV_LOG(SENSOR_LOG_LEVEL, " Duration of the fall : %.2f cm - %d samples", + ff_duration_cm, + ff_duration_samples); + INV_LOG(SENSOR_LOG_LEVEL, "ff detect") ; + + *ff_detect = true; + } + + return ff_duration_cm; +} + + +void inv_imu_dumpRegs() +{ + uint8_t data = 0; + uint16_t size = 0; + int i; + uint32_t reg_map_bank_0[] = { + DEVICE_CONFIG_REG, + SIGNAL_PATH_RESET, + INT_CONFIG_REG, + PWR_MGMT_0, + GYRO_CONFIG0, + ACCEL_CONFIG0, + TEMP_CONFIG0, + GYRO_CONFIG1, + ACCEL_CONFIG1, + APEX_CONFIG0, + APEX_CONFIG1, + WOM_CONFIG, + FIFO_CONFIG1, + FIFO_CONFIG2, + FIFO_CONFIG3, + INT_SOURCE0, + INT_SOURCE1, + INTF_CONFIG0, + INTF_CONFIG1 + }; + + uint32_t reg_map_bank_top1[] = { + TMST_CONFIG1_MREG, + FIFO_CONFIG5_MREG, + FIFO_CONFIG6_MREG, + INT_SOURCE6_MREG, + APEX_CONFIG2_MREG, + APEX_CONFIG3_MREG, + APEX_CONFIG4_MREG, + APEX_CONFIG5_MREG, + APEX_CONFIG9_MREG, + APEX_CONFIG10_MREG, + APEX_CONFIG11_MREG, + APEX_CONFIG12_MREG, + }; + + size = ARRAY_SIZE(reg_map_bank_0); + for (i = 0; i < size; i++) + { + inv_imu_read_reg(reg_map_bank_0[i], 1, &data); + INV_LOG(SENSOR_LOG_LEVEL, "bank0 reg[0x%x] 0x%x", reg_map_bank_0[i], data); + } + + size = ARRAY_SIZE(reg_map_bank_top1); + for (i = 0; i < size; i++) + { + inv_imu_read_reg(reg_map_bank_top1[i], 1, &data); + INV_LOG(SENSOR_LOG_LEVEL, "top1 reg[0x%x] 0x%x", reg_map_bank_top1[i], data); + } + + +} + diff --git a/components/i2c_devices/icm42670/test/CMakeLists.txt b/components/i2c_devices/icm42670/test/CMakeLists.txt new file mode 100644 index 0000000..3677df8 --- /dev/null +++ b/components/i2c_devices/icm42670/test/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRC_DIRS "." + PRIV_INCLUDE_DIRS "." + PRIV_REQUIRES unity test_utils icm42670) diff --git a/components/i2c_devices/icm42670/test/component.mk b/components/i2c_devices/icm42670/test/component.mk new file mode 100644 index 0000000..5dd172b --- /dev/null +++ b/components/i2c_devices/icm42670/test/component.mk @@ -0,0 +1,5 @@ +# +#Component Makefile +# + +COMPONENT_ADD_LDFLAGS = -Wl,--whole-archive -l$(COMPONENT_NAME) -Wl,--no-whole-archive diff --git a/components/i2c_devices/icm42670/test/example/example_freefall.c b/components/i2c_devices/icm42670/test/example/example_freefall.c new file mode 100644 index 0000000..692c350 --- /dev/null +++ b/components/i2c_devices/icm42670/test/example/example_freefall.c @@ -0,0 +1,273 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2017 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively Software? is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +/* std */ +#include + +/* board driver */ +#include "common.h" +#include "uart_mngr.h" +#include "delay.h" +#include "gpio.h" +#include "timer.h" +#include "system_interface.h" + +/* InvenSense utils */ +#include "Invn/EmbUtils/Message.h" +#include "Invn/EmbUtils/RingBuffer.h" + +#include "inv_imu_driver.h" + +/* + * Select UART port on which INV_MSG() will be printed. + */ +#define LOG_UART_ID INV_UART_SENSOR_CTRL + +/* + * Define msg level + */ +#define MSG_LEVEL INV_MSG_LEVEL_DEBUG + +/* + * Set of timers used throughout standalone applications + */ +#define TIMEBASE_TIMER INV_TIMER1 +#define DELAY_TIMER INV_TIMER2 + +#if !USE_FIFO + /* + * Buffer to keep track of the timestamp when IMU data ready interrupt fires. + * The buffer can contain up to 64 items in order to store one timestamp for each packet in FIFO. + */ + RINGBUFFER(timestamp_buffer, 64, uint64_t); +#endif + +/* -------------------------------------------------------------------------------------- + * Static variables + * -------------------------------------------------------------------------------------- */ + +/* Flag set from IMU device irq handler */ +static volatile int irq_from_device; + + +/* -------------------------------------------------------------------------------------- + * Forward declaration + * -------------------------------------------------------------------------------------- */ +static int setup_mcu(); +static void ext_interrupt_cb(void * context, unsigned int int_num); + +void msg_printer(int level, const char * str, va_list ap); + + +/* -------------------------------------------------------------------------------------- + * Functions definitions + * -------------------------------------------------------------------------------------- */ + +/* + * This function initializes MCU on which this software is running. + * It configures: + * - a UART link used to print some messages + * - interrupt priority group and GPIO so that MCU can receive interrupts from IMU + * - a microsecond timer requested by IMU driver to compute some delay + * - a microsecond timer used to get some timestamps + * - a serial link to communicate from MCU to IMU + */ +static int setup_mcu() +{ + int rc = 0; + + platform_io_hal_board_init(); + + /* configure UART */ + config_uart(LOG_UART_ID); + + /* Setup message facility to see internal traces from FW */ + PLATFORM_MSG_SETUP(MSG_LEVEL, msg_printer); + + INV_MSG(INV_MSG_LEVEL_INFO, "######################################"); + INV_MSG(INV_MSG_LEVEL_INFO, "# #XIAN MCU Driver V1.0 FF EXAMPLE #"); + INV_MSG(INV_MSG_LEVEL_INFO, "######################################"); + + /* + * Configure input capture mode GPIO connected to pin EXT3-9 (pin PB03). + * This pin is connected to Icm406xx INT1 output and thus will receive interrupts + * enabled on INT1 from the device. + * A callback function is also passed that will be executed each time an interrupt + * fires. + */ + platform_gpio_sensor_irq_init(INV_GPIO_INT1, ext_interrupt_cb, 0); + + /* Init timer peripheral for delay */ + rc |= platform_delay_init(DELAY_TIMER); + + /* + * Configure the timer for the timebase + */ + rc |= platform_timer_configure_timebase(1000000); + platform_timer_enable(TIMEBASE_TIMER); + + inv_imu_set_serif(platform_io_hal_read_reg, platform_io_hal_write_reg); + inv_imu_set_delay(platform_delay_ms,platform_delay_us); //void (*delay_ms)(uint32_t), void (*delay_us)(uint32_t)) + rc |= platform_io_hal_init(); + + return rc; +} + + +/* + * IMU interrupt handler. + * Function is executed when an IMU interrupt rises on MCU. + * This function get a timestamp and store it in the timestamp buffer. + * Note that this function is executed in an interrupt handler and thus no protection + * are implemented for shared variable timestamp_buffer. + */ +static void ext_interrupt_cb(void * context, unsigned int int_num) +{ + (void)context; + +#if !USE_FIFO + /* + * Read timestamp from the timer dedicated to timestamping + */ + uint64_t timestamp = platform_timer_get_counter(TIMEBASE_TIMER); + + if(int_num == INV_GPIO_INT1) { + if (!RINGBUFFER_FULL(×tamp_buffer)) + RINGBUFFER_PUSH(×tamp_buffer, ×tamp); + } +#endif + + irq_from_device |= TO_MASK(int_num); +} + + +/* -------------------------------------------------------------------------------------- + * Functions definition + * -------------------------------------------------------------------------------------- */ + +int setup_imu_device() +{ + int rc = 0; + + /* Init device */ + rc = inv_imu_initialize(); + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize IMU!"); + return rc; + } + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + return rc; +} + + +/* -------------------------------------------------------------------------------------- + * Main + * -------------------------------------------------------------------------------------- */ + +int main(void) +{ + int rc = 0; + bool ff_detect_result = false; + float freefall_distance = 0.0f; + + rc |= setup_mcu(); + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + /* Init device */ + rc = inv_imu_initialize(); + + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize INV concise driver!"); + return rc; + } + + INV_MSG(INV_MSG_LEVEL_INFO, "IMU device successfully initialized"); + + rc = inv_imu_freefall_enable() ; + + + if (rc != 0) { + INV_LOG(SENSOR_LOG_LEVEL, "freefall enable Failed. Do nothing %d", rc); + return rc; + } + + int i = 0; + + do { + /* Poll device for data */ + if (irq_from_device & TO_MASK(INV_GPIO_INT1)) { + + inv_disable_irq(); + + freefall_distance = inv_imu_freefall_get_event(&ff_detect_result); + INV_LOG(SENSOR_LOG_LEVEL, "ff result %d, cm %f",ff_detect_result,freefall_distance); + + irq_from_device &= ~TO_MASK(INV_GPIO_INT1); + inv_enable_irq(); + + i++; + } + + if (i == 30) { + rc = inv_imu_freefall_disable(); + i++; + } + } while(1); +} + +/* + * Printer function for message facility + */ +void msg_printer(int level, const char * str, va_list ap) +{ + static char out_str[256]; /* static to limit stack usage */ + unsigned idx = 0; + const char * s[INV_MSG_LEVEL_MAX] = { + "", // INV_MSG_LEVEL_OFF + "[E] ", // INV_MSG_LEVEL_ERROR + "[W] ", // INV_MSG_LEVEL_WARNING + "[I] ", // INV_MSG_LEVEL_INFO + "[V] ", // INV_MSG_LEVEL_VERBOSE + "[D] ", // INV_MSG_LEVEL_DEBUG + }; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "%s", s[level]); + if(idx >= (sizeof(out_str))) + return; + idx += vsnprintf(&out_str[idx], sizeof(out_str) - idx, str, ap); + if(idx >= (sizeof(out_str))) + return; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "\r\n"); + if(idx >= (sizeof(out_str))) + return; + + platform_uart_mngr_puts(LOG_UART_ID, out_str, idx); +} diff --git a/components/i2c_devices/icm42670/test/example/example_pedometer.c b/components/i2c_devices/icm42670/test/example/example_pedometer.c new file mode 100644 index 0000000..9901ff1 --- /dev/null +++ b/components/i2c_devices/icm42670/test/example/example_pedometer.c @@ -0,0 +1,277 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2017 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively Software? is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +/* std */ +#include + +/* board driver */ +#include "common.h" +#include "uart_mngr.h" +#include "delay.h" +#include "gpio.h" +#include "timer.h" +#include "system_interface.h" + +/* InvenSense utils */ +#include "Invn/EmbUtils/Message.h" +#include "Invn/EmbUtils/RingBuffer.h" + +#include "inv_imu_driver.h" + +/* + * Select UART port on which INV_MSG() will be printed. + */ +#define LOG_UART_ID INV_UART_SENSOR_CTRL + +/* + * Define msg level + */ +#define MSG_LEVEL INV_MSG_LEVEL_DEBUG + +/* + * Set of timers used throughout standalone applications + */ +#define TIMEBASE_TIMER INV_TIMER1 +#define DELAY_TIMER INV_TIMER2 + +#if !USE_FIFO + /* + * Buffer to keep track of the timestamp when IMU data ready interrupt fires. + * The buffer can contain up to 64 items in order to store one timestamp for each packet in FIFO. + */ + RINGBUFFER(timestamp_buffer, 64, uint64_t); +#endif + +/* -------------------------------------------------------------------------------------- + * Static variables + * -------------------------------------------------------------------------------------- */ + +/* Flag set from IMU device irq handler */ +static volatile int irq_from_device; + + +/* -------------------------------------------------------------------------------------- + * Forward declaration + * -------------------------------------------------------------------------------------- */ +static int setup_mcu(); +static void ext_interrupt_cb(void * context, unsigned int int_num); + +void msg_printer(int level, const char * str, va_list ap); + + +/* -------------------------------------------------------------------------------------- + * Functions definitions + * -------------------------------------------------------------------------------------- */ + +/* + * This function initializes MCU on which this software is running. + * It configures: + * - a UART link used to print some messages + * - interrupt priority group and GPIO so that MCU can receive interrupts from IMU + * - a microsecond timer requested by IMU driver to compute some delay + * - a microsecond timer used to get some timestamps + * - a serial link to communicate from MCU to IMU + */ +static int setup_mcu() +{ + int rc = 0; + + platform_io_hal_board_init(); + + /* configure UART */ + config_uart(LOG_UART_ID); + + /* Setup message facility to see internal traces from FW */ + PLATFORM_MSG_SETUP(MSG_LEVEL, msg_printer); + + INV_MSG(INV_MSG_LEVEL_INFO, "######################################"); + INV_MSG(INV_MSG_LEVEL_INFO, "# #XIAN MCU Driver V1.0 PEDO EXAMPLE#"); + INV_MSG(INV_MSG_LEVEL_INFO, "######################################"); + + /* + * Configure input capture mode GPIO connected to pin EXT3-9 (pin PB03). + * This pin is connected to Icm406xx INT1 output and thus will receive interrupts + * enabled on INT1 from the device. + * A callback function is also passed that will be executed each time an interrupt + * fires. + */ + platform_gpio_sensor_irq_init(INV_GPIO_INT1, ext_interrupt_cb, 0); + + /* Init timer peripheral for delay */ + rc |= platform_delay_init(DELAY_TIMER); + + /* + * Configure the timer for the timebase + */ + rc |= platform_timer_configure_timebase(1000000); + platform_timer_enable(TIMEBASE_TIMER); + + inv_imu_set_serif(platform_io_hal_read_reg, platform_io_hal_write_reg); + inv_imu_set_delay(platform_delay_ms,platform_delay_us); //void (*delay_ms)(uint32_t), void (*delay_us)(uint32_t)) + rc |= platform_io_hal_init(); + + return rc; +} + + +/* + * IMU interrupt handler. + * Function is executed when an IMU interrupt rises on MCU. + * This function get a timestamp and store it in the timestamp buffer. + * Note that this function is executed in an interrupt handler and thus no protection + * are implemented for shared variable timestamp_buffer. + */ +static void ext_interrupt_cb(void * context, unsigned int int_num) +{ + (void)context; + +#if !USE_FIFO + /* + * Read timestamp from the timer dedicated to timestamping + */ + uint64_t timestamp = platform_timer_get_counter(TIMEBASE_TIMER); + + if(int_num == INV_GPIO_INT1) { + if (!RINGBUFFER_FULL(×tamp_buffer)) + RINGBUFFER_PUSH(×tamp_buffer, ×tamp); + } +#endif + + irq_from_device |= TO_MASK(int_num); +} + + +/* -------------------------------------------------------------------------------------- + * Functions definition + * -------------------------------------------------------------------------------------- */ + +int setup_imu_device() +{ + int rc = 0; + + /* Init device */ + rc = inv_imu_initialize(); + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize IMU!"); + return rc; + } + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + return rc; +} + + +/* -------------------------------------------------------------------------------------- + * Main + * -------------------------------------------------------------------------------------- */ + +int main(void) +{ + int rc = 0; + APEX_DATA3_ACTIVITY_CLASS_t activity_class; + float cadence_step_per_sec; + int step_result = 0; + + rc |= setup_mcu(); + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + /* Init device */ + rc = inv_imu_initialize(); + + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize INV concise driver!"); + return rc; + } + + INV_MSG(INV_MSG_LEVEL_INFO, "IMU device successfully initialized"); + + rc = inv_imu_pedometer_enable() ; + + if (rc != 0) { + INV_MSG(INV_MSG_LEVEL_INFO, "pedometer enable Failed. Do nothing %d", rc); + return rc; + } + + int i = 0; + + do { + /* Poll device for data */ + if (irq_from_device & TO_MASK(INV_GPIO_INT1)) { + + inv_disable_irq(); + + step_result = inv_imu_pedometer_get_event(&cadence_step_per_sec,&activity_class); + + if(step_result < 0) + INV_MSG(INV_MSG_LEVEL_ERROR, "pedometer get event failure %d",step_result); + else + INV_MSG(INV_MSG_LEVEL_INFO, "step value %d, %.2f steps/sec, type %d",step_result,cadence_step_per_sec,activity_class); + + irq_from_device &= ~TO_MASK(INV_GPIO_INT1); + inv_enable_irq(); + + i++; + } + + if (i == 100) { + rc = inv_imu_pedometer_disable(); + i++; + } + } while(1); +} + +/* + * Printer function for message facility + */ +void msg_printer(int level, const char * str, va_list ap) +{ + static char out_str[256]; /* static to limit stack usage */ + unsigned idx = 0; + const char * s[INV_MSG_LEVEL_MAX] = { + "", // INV_MSG_LEVEL_OFF + "[E] ", // INV_MSG_LEVEL_ERROR + "[W] ", // INV_MSG_LEVEL_WARNING + "[I] ", // INV_MSG_LEVEL_INFO + "[V] ", // INV_MSG_LEVEL_VERBOSE + "[D] ", // INV_MSG_LEVEL_DEBUG + }; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "%s", s[level]); + if(idx >= (sizeof(out_str))) + return; + idx += vsnprintf(&out_str[idx], sizeof(out_str) - idx, str, ap); + if(idx >= (sizeof(out_str))) + return; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "\r\n"); + if(idx >= (sizeof(out_str))) + return; + + platform_uart_mngr_puts(LOG_UART_ID, out_str, idx); +} diff --git a/components/i2c_devices/icm42670/test/example/example_raw.c b/components/i2c_devices/icm42670/test/example/example_raw.c new file mode 100644 index 0000000..1f3347a --- /dev/null +++ b/components/i2c_devices/icm42670/test/example/example_raw.c @@ -0,0 +1,396 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2017 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively Software? is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +/* std */ +#include + +/* board driver */ +#include "common.h" +#include "uart_mngr.h" +#include "delay.h" +#include "gpio.h" +#include "timer.h" +#include "system_interface.h" + +/* InvenSense utils */ +#include "Invn/EmbUtils/Message.h" +#include "Invn/EmbUtils/RingBuffer.h" + +#include "inv_imu_driver.h" + + +struct SensorData Acc_Receive_dataBuff[2048]; +struct SensorData Gyro_Receive_dataBuff[2048]; + + +AccDataPacket accData; +GyroDataPacket gyroData; +chip_temperature imu_chip_temperature; + +uint32_t cur_index = 0; + + +/* + * Select UART port on which INV_MSG() will be printed. + */ +#define LOG_UART_ID INV_UART_SENSOR_CTRL + +/* + * Define msg level + */ +#define MSG_LEVEL INV_MSG_LEVEL_DEBUG + +/* + * Set of timers used throughout standalone applications + */ +#define TIMEBASE_TIMER INV_TIMER1 +#define DELAY_TIMER INV_TIMER2 + +#define SENSOR_ODR_1600HZ 1600 +#define SENSOR_ODR_800HZ 800 +#define SENSOR_ODR_400HZ 400 +#define SENSOR_ODR_200HZ 200 +#define SENSOR_ODR_100HZ 100 +#define SENSOR_ODR_50HZ 50 +#define SENSOR_ODR_25HZ 25 +#define SENSOR_ODR_12_5HZ 12.5 + + +#if !USE_FIFO + /* + * Buffer to keep track of the timestamp when IMU data ready interrupt fires. + * The buffer can contain up to 64 items in order to store one timestamp for each packet in FIFO. + */ + RINGBUFFER(timestamp_buffer, 64, uint64_t); +#endif + +/* -------------------------------------------------------------------------------------- + * Static variables + * -------------------------------------------------------------------------------------- */ + +/* Flag set from IMU device irq handler */ +static volatile int irq_from_device; + + +/* -------------------------------------------------------------------------------------- + * Forward declaration + * -------------------------------------------------------------------------------------- */ +static int setup_mcu(); +static void ext_interrupt_cb(void * context, unsigned int int_num); + +void msg_printer(int level, const char * str, va_list ap); + + +/* -------------------------------------------------------------------------------------- + * Functions definitions + * -------------------------------------------------------------------------------------- */ + +/* + * This function initializes MCU on which this software is running. + * It configures: + * - a UART link used to print some messages + * - interrupt priority group and GPIO so that MCU can receive interrupts from IMU + * - a microsecond timer requested by IMU driver to compute some delay + * - a microsecond timer used to get some timestamps + * - a serial link to communicate from MCU to IMU + */ +static int setup_mcu() +{ + int rc = 0; + + platform_io_hal_board_init(); + + /* configure UART */ + config_uart(LOG_UART_ID); + + /* Setup message facility to see internal traces from FW */ + PLATFORM_MSG_SETUP(MSG_LEVEL, msg_printer); + + INV_MSG(INV_MSG_LEVEL_INFO, "######################"); + INV_MSG(INV_MSG_LEVEL_INFO, "# Example Raw AG #"); + INV_MSG(INV_MSG_LEVEL_INFO, "######################"); + + /* + * Configure input capture mode GPIO connected to pin EXT3-9 (pin PB03). + * This pin is connected to Icm406xx INT1 output and thus will receive interrupts + * enabled on INT1 from the device. + * A callback function is also passed that will be executed each time an interrupt + * fires. + */ + platform_gpio_sensor_irq_init(INV_GPIO_INT1, ext_interrupt_cb, 0); + + /* Init timer peripheral for delay */ + rc |= platform_delay_init(DELAY_TIMER); + + /* + * Configure the timer for the timebase + */ + rc |= platform_timer_configure_timebase(1000000); + platform_timer_enable(TIMEBASE_TIMER); + + inv_imu_set_serif(platform_io_hal_read_reg, platform_io_hal_write_reg); + inv_imu_set_delay(platform_delay_ms,platform_delay_us); //void (*delay_ms)(uint32_t), void (*delay_us)(uint32_t)) + rc |= platform_io_hal_init(); + + return rc; +} + + +/* + * IMU interrupt handler. + * Function is executed when an IMU interrupt rises on MCU. + * This function get a timestamp and store it in the timestamp buffer. + * Note that this function is executed in an interrupt handler and thus no protection + * are implemented for shared variable timestamp_buffer. + */ +static void ext_interrupt_cb(void * context, unsigned int int_num) +{ + (void)context; + +#if !USE_FIFO + /* + * Read timestamp from the timer dedicated to timestamping + */ + uint64_t timestamp = platform_timer_get_counter(TIMEBASE_TIMER); + + if(int_num == INV_GPIO_INT1) { + if (!RINGBUFFER_FULL(×tamp_buffer)) + RINGBUFFER_PUSH(×tamp_buffer, ×tamp); + } +#endif + + irq_from_device |= TO_MASK(int_num); +} + + +/* -------------------------------------------------------------------------------------- + * Functions definition + * -------------------------------------------------------------------------------------- */ + +int setup_imu_device() +{ + int rc = 0; + + /* Init device */ + rc = inv_imu_initialize(); + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize IMU!"); + return rc; + } + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + return rc; +} + + +uint32_t get_imu_data() +{ + inv_data_handler(&accData,&gyroData,&imu_chip_temperature,false); + if(accData.accDataSize != 0){ + memcpy(&Acc_Receive_dataBuff[cur_index], &accData.databuff[0], accData.accDataSize * sizeof(accData.databuff[0])); + //INV_MSG(INV_MSG_LEVEL_ERROR, "acDS %u cidex %u",accData.accDataSize,cur_index); + } + + if(gyroData.gyroDataSize != 0){ + memcpy(&Gyro_Receive_dataBuff[cur_index], &gyroData.databuff[0], gyroData.gyroDataSize * sizeof(gyroData.databuff[0])); + } + + if ((accData.accDataSize != 0) && (gyroData.gyroDataSize != 0)) + cur_index = cur_index + (uint32_t)accData.accDataSize; + else if(accData.accDataSize != 0) + cur_index = cur_index + (uint32_t)accData.accDataSize; + else if(gyroData.gyroDataSize != 0) + cur_index = cur_index + (uint32_t)gyroData.gyroDataSize; + + return 0; +} + + +/* -------------------------------------------------------------------------------------- + * Main + * -------------------------------------------------------------------------------------- */ + + + +int main(void) +{ + int rc = 0; + float hw_rate = 0.0; + + memset(Acc_Receive_dataBuff,0,sizeof(Acc_Receive_dataBuff)); + memset(Gyro_Receive_dataBuff,0,sizeof(Gyro_Receive_dataBuff)); + rc |= setup_mcu(); + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + /* Init device */ + rc = inv_imu_initialize(); + + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize INV concise driver!"); + return rc; + } + + INV_MSG(INV_MSG_LEVEL_INFO, "IMU device successfully initialized"); + + // We may customize full scale range here. + rc |= inv_imu_set_accel_fsr(ACCEL_CONFIG0_FS_SEL_4g); + rc |= inv_imu_set_gyro_fsr(GYRO_CONFIG0_FS_SEL_2000dps); + + // Below settings are required to configure and enable sensors. + rc |= inv_imu_acc_enable(); + rc |= inv_imu_acc_set_rate(SENSOR_ODR_400HZ, 2,&hw_rate); //200Hz ODR, watermark: 2 packets. + rc |= inv_imu_gyro_enable(); + rc |= inv_imu_gyro_set_rate(SENSOR_ODR_400HZ, 4,&hw_rate); //50Hz ODR, watermark 4. + + if (rc != 0) { + INV_LOG(SENSOR_LOG_LEVEL, "Feature enable Failed. Do nothing %d", rc); + return rc; + } + INV_LOG(SENSOR_LOG_LEVEL, "HW odr setting %f ", hw_rate); + + do { + + /* Poll device for data */ + if (irq_from_device & TO_MASK(INV_GPIO_INT1)) { + + get_imu_data(); + //INV_MSG(INV_MSG_LEVEL_ERROR, "cur idex %lu!",cur_index); + + inv_disable_irq(); + irq_from_device &= ~TO_MASK(INV_GPIO_INT1); + inv_enable_irq(); + + } + } while(cur_index <2048); + + rc |= inv_imu_acc_disable(); + rc |= inv_imu_gyro_disable(); + + #if SENSOR_LOG_TS_ONLY + uint32_t i; + for(i=0;i< cur_index; i++){ + INV_LOG(SENSOR_LOG_LEVEL, "ACC %d TS %lld ", i+1,Acc_Receive_dataBuff[i].timeStamp); + platform_delay_ms(5); + } + + for(i=0;i< cur_index; i++){ + INV_LOG(SENSOR_LOG_LEVEL, "Gyro %d TS %lld ", i+1,Gyro_Receive_dataBuff[i].timeStamp); + platform_delay_ms(5); + } + #endif + + // following code is just repeating the previous code, just to confirm the testing could be alwasys run + int j = 2; + while(1){ + + memset(Acc_Receive_dataBuff,0,sizeof(Acc_Receive_dataBuff)); + memset(Gyro_Receive_dataBuff,0,sizeof(Gyro_Receive_dataBuff)); + + memset(&accData,0,sizeof(accData)); + memset(&gyroData,0,sizeof(gyroData)); + + INV_LOG(SENSOR_LOG_LEVEL, "repeating test round %d", j); + // Below settings are required to configure and enable sensors. + rc |= inv_imu_acc_enable(); + rc |= inv_imu_acc_set_rate(SENSOR_ODR_200HZ, 2,&hw_rate); //200Hz ODR, watermark: 2 packets. + rc |= inv_imu_gyro_enable(); + rc |= inv_imu_gyro_set_rate(SENSOR_ODR_200HZ, 4,&hw_rate); //50Hz ODR, watermark 4. + + if (rc != 0) { + INV_LOG(SENSOR_LOG_LEVEL, "Feature enable Failed. Do nothing %d", rc); + return rc; + } + INV_LOG(SENSOR_LOG_LEVEL, "HW odr setting %f ", hw_rate); + + + cur_index = 0; + do { + + /* Poll device for data */ + if (irq_from_device & TO_MASK(INV_GPIO_INT1)) { + + get_imu_data(); + //INV_MSG(INV_MSG_LEVEL_ERROR, "cur idex %lu!",cur_index); + + inv_disable_irq(); + irq_from_device &= ~TO_MASK(INV_GPIO_INT1); + inv_enable_irq(); + + } + } while(cur_index <2048); + + rc |= inv_imu_acc_disable(); + rc |= inv_imu_gyro_disable(); + + #if SENSOR_LOG_TS_ONLY + + for(i=0;i< cur_index; i++){ + INV_LOG(SENSOR_LOG_LEVEL, "ACC %d TS %lld ", i+1,Acc_Receive_dataBuff[i].timeStamp); + platform_delay_ms(5); + } + + for(i=0;i< cur_index; i++){ + INV_LOG(SENSOR_LOG_LEVEL, "Gyro %d TS %lld ", i+1,Gyro_Receive_dataBuff[i].timeStamp); + platform_delay_ms(5); + } + #endif + + j++; + } +} + +/* + * Printer function for message facility + */ +void msg_printer(int level, const char * str, va_list ap) +{ + static char out_str[256]; /* static to limit stack usage */ + unsigned idx = 0; + const char * s[INV_MSG_LEVEL_MAX] = { + "", // INV_MSG_LEVEL_OFF + "[E] ", // INV_MSG_LEVEL_ERROR + "[W] ", // INV_MSG_LEVEL_WARNING + "[I] ", // INV_MSG_LEVEL_INFO + "[V] ", // INV_MSG_LEVEL_VERBOSE + "[D] ", // INV_MSG_LEVEL_DEBUG + }; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "%s", s[level]); + if(idx >= (sizeof(out_str))) + return; + idx += vsnprintf(&out_str[idx], sizeof(out_str) - idx, str, ap); + if(idx >= (sizeof(out_str))) + return; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "\r\n"); + if(idx >= (sizeof(out_str))) + return; + + platform_uart_mngr_puts(LOG_UART_ID, out_str, idx); +} diff --git a/components/i2c_devices/icm42670/test/example/example_selftest.c b/components/i2c_devices/icm42670/test/example/example_selftest.c new file mode 100644 index 0000000..4243fbd --- /dev/null +++ b/components/i2c_devices/icm42670/test/example/example_selftest.c @@ -0,0 +1,266 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2017 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively Software? is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +/* std */ +#include + +/* board driver */ +#include "common.h" +#include "uart_mngr.h" +#include "delay.h" +#include "gpio.h" +#include "timer.h" +#include "system_interface.h" + +/* InvenSense utils */ +#include "Invn/EmbUtils/Message.h" +#include "Invn/EmbUtils/RingBuffer.h" + +#include "inv_imu_driver.h" + +/* + * Select UART port on which INV_MSG() will be printed. + */ +#define LOG_UART_ID INV_UART_SENSOR_CTRL + +/* + * Define msg level + */ +#define MSG_LEVEL INV_MSG_LEVEL_DEBUG + +/* + * Set of timers used throughout standalone applications + */ +#define TIMEBASE_TIMER INV_TIMER1 +#define DELAY_TIMER INV_TIMER2 + +#if !USE_FIFO + /* + * Buffer to keep track of the timestamp when IMU data ready interrupt fires. + * The buffer can contain up to 64 items in order to store one timestamp for each packet in FIFO. + */ + RINGBUFFER(timestamp_buffer, 64, uint64_t); +#endif + +/* -------------------------------------------------------------------------------------- + * Static variables + * -------------------------------------------------------------------------------------- */ + +/* Flag set from IMU device irq handler */ +static volatile int irq_from_device; + + +/* -------------------------------------------------------------------------------------- + * Forward declaration + * -------------------------------------------------------------------------------------- */ +static int setup_mcu(); +static void ext_interrupt_cb(void * context, unsigned int int_num); + +void msg_printer(int level, const char * str, va_list ap); + + +/* -------------------------------------------------------------------------------------- + * Functions definitions + * -------------------------------------------------------------------------------------- */ + +/* + * This function initializes MCU on which this software is running. + * It configures: + * - a UART link used to print some messages + * - interrupt priority group and GPIO so that MCU can receive interrupts from IMU + * - a microsecond timer requested by IMU driver to compute some delay + * - a microsecond timer used to get some timestamps + * - a serial link to communicate from MCU to IMU + */ +static int setup_mcu() +{ + int rc = 0; + + platform_io_hal_board_init(); + + /* configure UART */ + config_uart(LOG_UART_ID); + + /* Setup message facility to see internal traces from FW */ + PLATFORM_MSG_SETUP(MSG_LEVEL, msg_printer); + + INV_MSG(INV_MSG_LEVEL_INFO, "######################################"); + INV_MSG(INV_MSG_LEVEL_INFO, "# #XIAN MCU Driver V1.0 SELFTEST EX #"); + INV_MSG(INV_MSG_LEVEL_INFO, "######################################"); + + /* + * Configure input capture mode GPIO connected to pin EXT3-9 (pin PB03). + * This pin is connected to Icm406xx INT1 output and thus will receive interrupts + * enabled on INT1 from the device. + * A callback function is also passed that will be executed each time an interrupt + * fires. + */ + platform_gpio_sensor_irq_init(INV_GPIO_INT1, ext_interrupt_cb, 0); + + /* Init timer peripheral for delay */ + rc |= platform_delay_init(DELAY_TIMER); + + /* + * Configure the timer for the timebase + */ + rc |= platform_timer_configure_timebase(1000000); + platform_timer_enable(TIMEBASE_TIMER); + + inv_imu_set_serif(platform_io_hal_read_reg, platform_io_hal_write_reg); + inv_imu_set_delay(platform_delay_ms,platform_delay_us); //void (*delay_ms)(uint32_t), void (*delay_us)(uint32_t)) + rc |= platform_io_hal_init(); + + return rc; +} + + +/* + * IMU interrupt handler. + * Function is executed when an IMU interrupt rises on MCU. + * This function get a timestamp and store it in the timestamp buffer. + * Note that this function is executed in an interrupt handler and thus no protection + * are implemented for shared variable timestamp_buffer. + */ +static void ext_interrupt_cb(void * context, unsigned int int_num) +{ + (void)context; + +#if !USE_FIFO + /* + * Read timestamp from the timer dedicated to timestamping + */ + uint64_t timestamp = platform_timer_get_counter(TIMEBASE_TIMER); + + if(int_num == INV_GPIO_INT1) { + if (!RINGBUFFER_FULL(×tamp_buffer)) + RINGBUFFER_PUSH(×tamp_buffer, ×tamp); + } +#endif + + irq_from_device |= TO_MASK(int_num); +} + + +/* -------------------------------------------------------------------------------------- + * Functions definition + * -------------------------------------------------------------------------------------- */ + +int setup_imu_device() +{ + int rc = 0; + + /* Init device */ + rc = inv_imu_initialize(); + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize IMU!"); + return rc; + } + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + return rc; +} + + +/* -------------------------------------------------------------------------------------- + * Main + * -------------------------------------------------------------------------------------- */ + +int main(void) +{ + int rc = 0; + uint8_t acc_control = 1; + uint8_t gyro_control = 1; + + struct inv_imu_selftest_output selftest_result; + + rc |= setup_mcu(); + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + /* Init device */ + rc = inv_imu_initialize(); + + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize INV concise driver!"); + return rc; + } + + INV_MSG(INV_MSG_LEVEL_INFO, "IMU device successfully initialized"); + + rc |= inv_imu_run_selftest(acc_control,gyro_control, &selftest_result); + + if (rc != 0) { + INV_LOG(SENSOR_LOG_LEVEL, "selftest run failure. Do nothing %d", rc); + return rc; + } + + if (selftest_result.accel_status == 0x1) + INV_LOG(SENSOR_LOG_LEVEL, "accel Selftest PASS\n"); + else + INV_LOG(SENSOR_LOG_LEVEL, "accel Selftest fail\n"); + + if (selftest_result.gyro_status == 0x1) + INV_LOG(SENSOR_LOG_LEVEL, "gyro Selftest PASS\n"); + else + INV_LOG(SENSOR_LOG_LEVEL, "gyro Selftest fail\n"); + + + do { + + } while(1); +} + +/* + * Printer function for message facility + */ +void msg_printer(int level, const char * str, va_list ap) +{ + static char out_str[256]; /* static to limit stack usage */ + unsigned idx = 0; + const char * s[INV_MSG_LEVEL_MAX] = { + "", // INV_MSG_LEVEL_OFF + "[E] ", // INV_MSG_LEVEL_ERROR + "[W] ", // INV_MSG_LEVEL_WARNING + "[I] ", // INV_MSG_LEVEL_INFO + "[V] ", // INV_MSG_LEVEL_VERBOSE + "[D] ", // INV_MSG_LEVEL_DEBUG + }; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "%s", s[level]); + if(idx >= (sizeof(out_str))) + return; + idx += vsnprintf(&out_str[idx], sizeof(out_str) - idx, str, ap); + if(idx >= (sizeof(out_str))) + return; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "\r\n"); + if(idx >= (sizeof(out_str))) + return; + + platform_uart_mngr_puts(LOG_UART_ID, out_str, idx); +} diff --git a/components/i2c_devices/icm42670/test/example/example_wom.c b/components/i2c_devices/icm42670/test/example/example_wom.c new file mode 100644 index 0000000..2bd825f --- /dev/null +++ b/components/i2c_devices/icm42670/test/example/example_wom.c @@ -0,0 +1,273 @@ +/* + * ________________________________________________________________________________________________________ + * Copyright (c) 2017 InvenSense Inc. All rights reserved. + * + * This software, related documentation and any modifications thereto (collectively Software? is subject + * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright + * and other intellectual property rights laws. + * + * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software + * and any use, reproduction, disclosure or distribution of the Software without an express license agreement + * from InvenSense is strictly prohibited. + * + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS + * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED + * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. + * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL + * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, 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 THE SOFTWARE. + * ________________________________________________________________________________________________________ + */ + +/* std */ +#include + +/* board driver */ +#include "common.h" +#include "uart_mngr.h" +#include "delay.h" +#include "gpio.h" +#include "timer.h" +#include "system_interface.h" + +/* InvenSense utils */ +#include "Invn/EmbUtils/Message.h" +#include "Invn/EmbUtils/RingBuffer.h" + +#include "inv_imu_driver.h" + + +/* + * Select UART port on which INV_MSG() will be printed. + */ +#define LOG_UART_ID INV_UART_SENSOR_CTRL + +/* + * Define msg level + */ +#define MSG_LEVEL INV_MSG_LEVEL_DEBUG + +/* + * Set of timers used throughout standalone applications + */ +#define TIMEBASE_TIMER INV_TIMER1 +#define DELAY_TIMER INV_TIMER2 + +#if !USE_FIFO + /* + * Buffer to keep track of the timestamp when IMU data ready interrupt fires. + * The buffer can contain up to 64 items in order to store one timestamp for each packet in FIFO. + */ + RINGBUFFER(timestamp_buffer, 64, uint64_t); +#endif + +/* -------------------------------------------------------------------------------------- + * Static variables + * -------------------------------------------------------------------------------------- */ + +/* Flag set from IMU device irq handler */ +static volatile int irq_from_device; + + +/* -------------------------------------------------------------------------------------- + * Forward declaration + * -------------------------------------------------------------------------------------- */ +static int setup_mcu(); +static void ext_interrupt_cb(void * context, unsigned int int_num); + +void msg_printer(int level, const char * str, va_list ap); + + +/* -------------------------------------------------------------------------------------- + * Functions definitions + * -------------------------------------------------------------------------------------- */ + +/* + * This function initializes MCU on which this software is running. + * It configures: + * - a UART link used to print some messages + * - interrupt priority group and GPIO so that MCU can receive interrupts from IMU + * - a microsecond timer requested by IMU driver to compute some delay + * - a microsecond timer used to get some timestamps + * - a serial link to communicate from MCU to IMU + */ +static int setup_mcu() +{ + int rc = 0; + + platform_io_hal_board_init(); + + /* configure UART */ + config_uart(LOG_UART_ID); + + /* Setup message facility to see internal traces from FW */ + PLATFORM_MSG_SETUP(MSG_LEVEL, msg_printer); + + INV_MSG(INV_MSG_LEVEL_INFO, "######################################"); + INV_MSG(INV_MSG_LEVEL_INFO, "# #XIAN MCU Driver V1.0 WOM EXAMPLE #"); + INV_MSG(INV_MSG_LEVEL_INFO, "######################################"); + + /* + * Configure input capture mode GPIO connected to pin EXT3-9 (pin PB03). + * This pin is connected to Icm406xx INT1 output and thus will receive interrupts + * enabled on INT1 from the device. + * A callback function is also passed that will be executed each time an interrupt + * fires. + */ + platform_gpio_sensor_irq_init(INV_GPIO_INT1, ext_interrupt_cb, 0); + + /* Init timer peripheral for delay */ + rc |= platform_delay_init(DELAY_TIMER); + + /* + * Configure the timer for the timebase + */ + rc |= platform_timer_configure_timebase(1000000); + platform_timer_enable(TIMEBASE_TIMER); + + inv_imu_set_serif(platform_io_hal_read_reg, platform_io_hal_write_reg); + inv_imu_set_delay(platform_delay_ms,platform_delay_us); //void (*delay_ms)(uint32_t), void (*delay_us)(uint32_t)) + rc |= platform_io_hal_init(); + + return rc; +} + + +/* + * IMU interrupt handler. + * Function is executed when an IMU interrupt rises on MCU. + * This function get a timestamp and store it in the timestamp buffer. + * Note that this function is executed in an interrupt handler and thus no protection + * are implemented for shared variable timestamp_buffer. + */ +static void ext_interrupt_cb(void * context, unsigned int int_num) +{ + (void)context; + +#if !USE_FIFO + /* + * Read timestamp from the timer dedicated to timestamping + */ + uint64_t timestamp = platform_timer_get_counter(TIMEBASE_TIMER); + + if(int_num == INV_GPIO_INT1) { + if (!RINGBUFFER_FULL(×tamp_buffer)) + RINGBUFFER_PUSH(×tamp_buffer, ×tamp); + } +#endif + + irq_from_device |= TO_MASK(int_num); +} + + +/* -------------------------------------------------------------------------------------- + * Functions definition + * -------------------------------------------------------------------------------------- */ + +int setup_imu_device() +{ + int rc = 0; + + /* Init device */ + rc = inv_imu_initialize(); + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize IMU!"); + return rc; + } + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + return rc; +} + +/* -------------------------------------------------------------------------------------- + * Main + * -------------------------------------------------------------------------------------- */ + +int main(void) +{ + int rc = 0; + bool wom_detect_result = false; + int wom_flag; + + rc |= setup_mcu(); + + +#if !USE_FIFO + RINGBUFFER_CLEAR(×tamp_buffer); +#endif + + /* Init device */ + rc = inv_imu_initialize(); + + if (rc != INV_ERROR_SUCCESS) { + INV_MSG(INV_MSG_LEVEL_ERROR, "Failed to initialize INV concise driver!"); + return rc; + } + + INV_MSG(INV_MSG_LEVEL_INFO, "IMU device successfully initialized"); + + rc += inv_imu_wom_enable(WOM_THRESHOLD_X,WOM_THRESHOLD_Y,WOM_THRESHOLD_Z,WOM_CONFIG_WOM_INT_DUR_4_SMPL); + + + if (rc != 0) { + INV_LOG(SENSOR_LOG_LEVEL, "Feature enable Failed. Do nothing %d", rc); + return rc; + } + + int i = 0; + + do { + /* Poll device for data */ + if (irq_from_device & TO_MASK(INV_GPIO_INT1)) { + + inv_disable_irq(); + + wom_flag = inv_imu_wom_get_event(&wom_detect_result); + INV_LOG(SENSOR_LOG_LEVEL, "wom result %d, flag %x",wom_detect_result,wom_flag); + + irq_from_device &= ~TO_MASK(INV_GPIO_INT1); + inv_enable_irq(); + + i++; + } + + if (i == 20) { + rc = inv_imu_wom_disable(); + i++; + } + } while(1); +} + +/* + * Printer function for message facility + */ +void msg_printer(int level, const char * str, va_list ap) +{ + static char out_str[256]; /* static to limit stack usage */ + unsigned idx = 0; + const char * s[INV_MSG_LEVEL_MAX] = { + "", // INV_MSG_LEVEL_OFF + "[E] ", // INV_MSG_LEVEL_ERROR + "[W] ", // INV_MSG_LEVEL_WARNING + "[I] ", // INV_MSG_LEVEL_INFO + "[V] ", // INV_MSG_LEVEL_VERBOSE + "[D] ", // INV_MSG_LEVEL_DEBUG + }; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "%s", s[level]); + if(idx >= (sizeof(out_str))) + return; + idx += vsnprintf(&out_str[idx], sizeof(out_str) - idx, str, ap); + if(idx >= (sizeof(out_str))) + return; + idx += snprintf(&out_str[idx], sizeof(out_str) - idx, "\r\n"); + if(idx >= (sizeof(out_str))) + return; + + platform_uart_mngr_puts(LOG_UART_ID, out_str, idx); +} diff --git a/components/i2c_devices/icm42670/test/icm42670_test.c b/components/i2c_devices/icm42670/test/icm42670_test.c new file mode 100644 index 0000000..89d591e --- /dev/null +++ b/components/i2c_devices/icm42670/test/icm42670_test.c @@ -0,0 +1,50 @@ +// Copyright 2020 Espressif Systems (Shanghai) Co. Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "stdio.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/queue.h" +#include "freertos/timers.h" +#include "esp_log.h" +#include "unity.h" +#include "icm42670.h" + +static const char *TAG = "IMU TEST"; + + +static void icm42670_test(void *args) +{ + AccDataPacket accData; + GyroDataPacket gyroData; + chip_temperature imu_chip_temperature; + + icm42670_init(); + while (1) { + int ret = icm42670_get_raw_data(&accData, &gyroData, &imu_chip_temperature); + if (0 == ret) { + INV_LOG(SENSOR_LOG_LEVEL, "temperature=%.1f", imu_chip_temperature); + INV_LOG(SENSOR_LOG_LEVEL, "ACC %d (%.1f, %.1f, %.1f)", accData.accDataSize, accData.databuff[0].x, accData.databuff[0].y, accData.databuff[0].z); + INV_LOG(SENSOR_LOG_LEVEL, "Gyro%d (%.1f, %.1f, %.1f)", gyroData.gyroDataSize, gyroData.databuff[0].x, gyroData.databuff[0].y, gyroData.databuff[0].z); + } else { + INV_LOG(INV_LOG_LEVEL_ERROR, "read err"); + } + vTaskDelay(pdMS_TO_TICKS(100)); + } +} + +TEST_CASE("ICM42670 test", "[imu][cube]") +{ + icm42670_test(NULL); +} diff --git a/components/i2c_devices/include/ft5x06.h b/components/i2c_devices/touch_panel/include/ft5x06.h similarity index 100% rename from components/i2c_devices/include/ft5x06.h rename to components/i2c_devices/touch_panel/include/ft5x06.h diff --git a/components/i2c_devices/include/tt21100.h b/components/i2c_devices/touch_panel/include/tt21100.h similarity index 100% rename from components/i2c_devices/include/tt21100.h rename to components/i2c_devices/touch_panel/include/tt21100.h