Silicon Labs / ICM20648

Simple driver for the TDK InvenSense ICM20648 6-axis IMU

Dependents:   TBSense2_Sensor_Demo

Information

All examples in this repo are considered EXPERIMENTAL QUALITY, meaning this code has been created as one-off proof-of-concept and is suitable as a demonstration for experimental purposes only. This code will not be regularly maintained by Silicon Labs and there is no guarantee that these projects will work across all environments, SDK versions and hardware.

ICM20648.cpp@0:296308a935f5, 2017-11-12 (annotated)

Committer:
stevew817
Date:
Sun Nov 12 20:09:19 2017 +0100
Revision:
0:296308a935f5
Initial driver code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stevew817 0:296308a935f5 1 /***************************************************************************//**
stevew817 0:296308a935f5 2 * @file ICM20648.cpp
stevew817 0:296308a935f5 3 *******************************************************************************
stevew817 0:296308a935f5 4 * @section License
stevew817 0:296308a935f5 5 * <b>(C) Copyright 2017 Silicon Labs, http://www.silabs.com</b>
stevew817 0:296308a935f5 6 *******************************************************************************
stevew817 0:296308a935f5 7 *
stevew817 0:296308a935f5 8 * SPDX-License-Identifier: Apache-2.0
stevew817 0:296308a935f5 9 *
stevew817 0:296308a935f5 10 * Licensed under the Apache License, Version 2.0 (the "License"); you may
stevew817 0:296308a935f5 11 * not use this file except in compliance with the License.
stevew817 0:296308a935f5 12 * You may obtain a copy of the License at
stevew817 0:296308a935f5 13 *
stevew817 0:296308a935f5 14 * http://www.apache.org/licenses/LICENSE-2.0
stevew817 0:296308a935f5 15 *
stevew817 0:296308a935f5 16 * Unless required by applicable law or agreed to in writing, software
stevew817 0:296308a935f5 17 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
stevew817 0:296308a935f5 18 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
stevew817 0:296308a935f5 19 * See the License for the specific language governing permissions and
stevew817 0:296308a935f5 20 * limitations under the License.
stevew817 0:296308a935f5 21 *
stevew817 0:296308a935f5 22 ******************************************************************************/
stevew817 0:296308a935f5 23
stevew817 0:296308a935f5 24 #include "ICM20648.h"
stevew817 0:296308a935f5 25
stevew817 0:296308a935f5 26 /** @cond DO_NOT_INCLUDE_WITH_DOXYGEN */
stevew817 0:296308a935f5 27
stevew817 0:296308a935f5 28 /**************************************************************************//**
stevew817 0:296308a935f5 29 * @name Error Codes
stevew817 0:296308a935f5 30 * @{
stevew817 0:296308a935f5 31 ******************************************************************************/
stevew817 0:296308a935f5 32 #define ICM20648_OK 0x0000 /**< No errors */
stevew817 0:296308a935f5 33 #define ICM20648_ERROR_INVALID_DEVICE_ID 0x0001 /**< Invalid device ID */
stevew817 0:296308a935f5 34 /**@}*/
stevew817 0:296308a935f5 35
stevew817 0:296308a935f5 36 /**************************************************************************//**
stevew817 0:296308a935f5 37 * @name ICM20648 register banks
stevew817 0:296308a935f5 38 * @{
stevew817 0:296308a935f5 39 ******************************************************************************/
stevew817 0:296308a935f5 40 #define ICM20648_BANK_0 (0 << 7) /**< Register bank 0 */
stevew817 0:296308a935f5 41 #define ICM20648_BANK_1 (1 << 7) /**< Register bank 1 */
stevew817 0:296308a935f5 42 #define ICM20648_BANK_2 (2 << 7) /**< Register bank 2 */
stevew817 0:296308a935f5 43 #define ICM20648_BANK_3 (3 << 7) /**< Register bank 3 */
stevew817 0:296308a935f5 44 /**@}*/
stevew817 0:296308a935f5 45
stevew817 0:296308a935f5 46 /**************************************************************************//**
stevew817 0:296308a935f5 47 * @name Register and associated bit definitions
stevew817 0:296308a935f5 48 * @{
stevew817 0:296308a935f5 49 ******************************************************************************/
stevew817 0:296308a935f5 50 /***********************/
stevew817 0:296308a935f5 51 /* Bank 0 register map */
stevew817 0:296308a935f5 52 /***********************/
stevew817 0:296308a935f5 53 #define ICM20648_REG_WHO_AM_I (ICM20648_BANK_0 | 0x00) /**< Device ID register */
stevew817 0:296308a935f5 54
stevew817 0:296308a935f5 55 #define ICM20648_REG_USER_CTRL (ICM20648_BANK_0 | 0x03) /**< User control register */
stevew817 0:296308a935f5 56 #define ICM20648_BIT_DMP_EN 0x80 /**< DMP enable bit */
stevew817 0:296308a935f5 57 #define ICM20648_BIT_FIFO_EN 0x40 /**< FIFO enable bit */
stevew817 0:296308a935f5 58 #define ICM20648_BIT_I2C_MST_EN 0x20 /**< I2C master I/F enable bit */
stevew817 0:296308a935f5 59 #define ICM20648_BIT_I2C_IF_DIS 0x10 /**< Disable I2C, enable SPI bit */
stevew817 0:296308a935f5 60 #define ICM20648_BIT_DMP_RST 0x08 /**< DMP module reset bit */
stevew817 0:296308a935f5 61 #define ICM20648_BIT_DIAMOND_DMP_RST 0x04 /**< SRAM module reset bit */
stevew817 0:296308a935f5 62
stevew817 0:296308a935f5 63 #define ICM20648_REG_LP_CONFIG (ICM20648_BANK_0 | 0x05) /**< Low Power mode config register */
stevew817 0:296308a935f5 64 #define ICM20648_BIT_I2C_MST_CYCLE 0x40 /**< I2C master cycle mode enable */
stevew817 0:296308a935f5 65 #define ICM20648_BIT_ACCEL_CYCLE 0x20 /**< Accelerometer cycle mode enable */
stevew817 0:296308a935f5 66 #define ICM20648_BIT_GYRO_CYCLE 0x10 /**< Gyroscope cycle mode enable */
stevew817 0:296308a935f5 67
stevew817 0:296308a935f5 68 #define ICM20648_REG_PWR_MGMT_1 (ICM20648_BANK_0 | 0x06) /**< Power Management 1 register */
stevew817 0:296308a935f5 69 #define ICM20648_BIT_H_RESET 0x80 /**< Device reset bit */
stevew817 0:296308a935f5 70 #define ICM20648_BIT_SLEEP 0x40 /**< Sleep mode enable bit */
stevew817 0:296308a935f5 71 #define ICM20648_BIT_LP_EN 0x20 /**< Low Power feature enable bit */
stevew817 0:296308a935f5 72 #define ICM20648_BIT_TEMP_DIS 0x08 /**< Temperature sensor disable bit */
stevew817 0:296308a935f5 73 #define ICM20648_BIT_CLK_PLL 0x01 /**< Auto clock source selection setting */
stevew817 0:296308a935f5 74
stevew817 0:296308a935f5 75 #define ICM20648_REG_PWR_MGMT_2 (ICM20648_BANK_0 | 0x07) /**< Power Management 2 register */
stevew817 0:296308a935f5 76 #define ICM20648_BIT_PWR_ACCEL_STBY 0x38 /**< Disable accelerometer */
stevew817 0:296308a935f5 77 #define ICM20648_BIT_PWR_GYRO_STBY 0x07 /**< Disable gyroscope */
stevew817 0:296308a935f5 78 #define ICM20648_BIT_PWR_ALL_OFF 0x7F /**< Disable both accel and gyro */
stevew817 0:296308a935f5 79
stevew817 0:296308a935f5 80 #define ICM20648_REG_INT_PIN_CFG (ICM20648_BANK_0 | 0x0F) /**< Interrupt Pin Configuration register */
stevew817 0:296308a935f5 81 #define ICM20648_BIT_INT_ACTL 0x80 /**< Active low setting bit */
stevew817 0:296308a935f5 82 #define ICM20648_BIT_INT_OPEN 0x40 /**< Open collector onfiguration bit */
stevew817 0:296308a935f5 83 #define ICM20648_BIT_INT_LATCH_EN 0x20 /**< Latch enable bit */
stevew817 0:296308a935f5 84
stevew817 0:296308a935f5 85 #define ICM20648_REG_INT_ENABLE (ICM20648_BANK_0 | 0x10) /**< Interrupt Enable register */
stevew817 0:296308a935f5 86 #define ICM20648_BIT_WOM_INT_EN 0x08 /**< Wake-up On Motion enable bit */
stevew817 0:296308a935f5 87
stevew817 0:296308a935f5 88 #define ICM20648_REG_INT_ENABLE_1 (ICM20648_BANK_0 | 0x11) /**< Interrupt Enable 1 register */
stevew817 0:296308a935f5 89 #define ICM20648_BIT_RAW_DATA_0_RDY_EN 0x01 /**< Raw data ready interrupt enable bit */
stevew817 0:296308a935f5 90
stevew817 0:296308a935f5 91 #define ICM20648_REG_INT_ENABLE_2 (ICM20648_BANK_0 | 0x12) /**< Interrupt Enable 2 register */
stevew817 0:296308a935f5 92 #define ICM20648_BIT_FIFO_OVERFLOW_EN_0 0x01 /**< FIFO overflow interrupt enable bit */
stevew817 0:296308a935f5 93
stevew817 0:296308a935f5 94 #define ICM20648_REG_INT_ENABLE_3 (ICM20648_BANK_0 | 0x13) /**< Interrupt Enable 2 register */
stevew817 0:296308a935f5 95
stevew817 0:296308a935f5 96 #define ICM20648_REG_INT_STATUS (ICM20648_BANK_0 | 0x19) /**< Interrupt Status register */
stevew817 0:296308a935f5 97 #define ICM20648_BIT_WOM_INT 0x08 /**< Wake-up on motion interrupt occured bit */
stevew817 0:296308a935f5 98 #define ICM20648_BIT_PLL_RDY 0x04 /**< PLL ready interrupt occured bit */
stevew817 0:296308a935f5 99
stevew817 0:296308a935f5 100 #define ICM20648_REG_INT_STATUS_1 (ICM20648_BANK_0 | 0x1A) /**< Interrupt Status 1 register */
stevew817 0:296308a935f5 101 #define ICM20648_BIT_RAW_DATA_0_RDY_INT 0x01 /**< Raw data ready interrupt occured bit */
stevew817 0:296308a935f5 102
stevew817 0:296308a935f5 103 #define ICM20648_REG_INT_STATUS_2 (ICM20648_BANK_0 | 0x1B) /**< Interrupt Status 2 register */
stevew817 0:296308a935f5 104
stevew817 0:296308a935f5 105 #define ICM20648_REG_ACCEL_XOUT_H_SH (ICM20648_BANK_0 | 0x2D) /**< Accelerometer X-axis data high byte */
stevew817 0:296308a935f5 106 #define ICM20648_REG_ACCEL_XOUT_L_SH (ICM20648_BANK_0 | 0x2E) /**< Accelerometer X-axis data low byte */
stevew817 0:296308a935f5 107 #define ICM20648_REG_ACCEL_YOUT_H_SH (ICM20648_BANK_0 | 0x2F) /**< Accelerometer Y-axis data high byte */
stevew817 0:296308a935f5 108 #define ICM20648_REG_ACCEL_YOUT_L_SH (ICM20648_BANK_0 | 0x30) /**< Accelerometer Y-axis data low byte */
stevew817 0:296308a935f5 109 #define ICM20648_REG_ACCEL_ZOUT_H_SH (ICM20648_BANK_0 | 0x31) /**< Accelerometer Z-axis data high byte */
stevew817 0:296308a935f5 110 #define ICM20648_REG_ACCEL_ZOUT_L_SH (ICM20648_BANK_0 | 0x32) /**< Accelerometer Z-axis data low byte */
stevew817 0:296308a935f5 111
stevew817 0:296308a935f5 112 #define ICM20648_REG_GYRO_XOUT_H_SH (ICM20648_BANK_0 | 0x33) /**< Gyroscope X-axis data high byte */
stevew817 0:296308a935f5 113 #define ICM20648_REG_GYRO_XOUT_L_SH (ICM20648_BANK_0 | 0x34) /**< Gyroscope X-axis data low byte */
stevew817 0:296308a935f5 114 #define ICM20648_REG_GYRO_YOUT_H_SH (ICM20648_BANK_0 | 0x35) /**< Gyroscope Y-axis data high byte */
stevew817 0:296308a935f5 115 #define ICM20648_REG_GYRO_YOUT_L_SH (ICM20648_BANK_0 | 0x36) /**< Gyroscope Y-axis data low byte */
stevew817 0:296308a935f5 116 #define ICM20648_REG_GYRO_ZOUT_H_SH (ICM20648_BANK_0 | 0x37) /**< Gyroscope Z-axis data high byte */
stevew817 0:296308a935f5 117 #define ICM20648_REG_GYRO_ZOUT_L_SH (ICM20648_BANK_0 | 0x38) /**< Gyroscope Z-axis data low byte */
stevew817 0:296308a935f5 118
stevew817 0:296308a935f5 119 #define ICM20648_REG_TEMPERATURE_H (ICM20648_BANK_0 | 0x39) /**< Temperature data high byte */
stevew817 0:296308a935f5 120 #define ICM20648_REG_TEMPERATURE_L (ICM20648_BANK_0 | 0x3A) /**< Temperature data low byte */
stevew817 0:296308a935f5 121 #define ICM20648_REG_TEMP_CONFIG (ICM20648_BANK_0 | 0x53) /**< Temperature Configuration register */
stevew817 0:296308a935f5 122
stevew817 0:296308a935f5 123 #define ICM20648_REG_FIFO_EN_1 (ICM20648_BANK_0 | 0x66) /**< FIFO Enable 1 register */
stevew817 0:296308a935f5 124
stevew817 0:296308a935f5 125 #define ICM20648_REG_FIFO_EN_2 (ICM20648_BANK_0 | 0x67) /**< FIFO Enable 2 register */
stevew817 0:296308a935f5 126 #define ICM20648_BIT_ACCEL_FIFO_EN 0x10 /**< Enable writing acceleration data to FIFO bit */
stevew817 0:296308a935f5 127 #define ICM20648_BITS_GYRO_FIFO_EN 0x0E /**< Enable writing gyroscope data to FIFO bit */
stevew817 0:296308a935f5 128
stevew817 0:296308a935f5 129 #define ICM20648_REG_FIFO_RST (ICM20648_BANK_0 | 0x68) /**< FIFO Reset register */
stevew817 0:296308a935f5 130 #define ICM20648_REG_FIFO_MODE (ICM20648_BANK_0 | 0x69) /**< FIFO Mode register */
stevew817 0:296308a935f5 131
stevew817 0:296308a935f5 132 #define ICM20648_REG_FIFO_COUNT_H (ICM20648_BANK_0 | 0x70) /**< FIFO data count high byte */
stevew817 0:296308a935f5 133 #define ICM20648_REG_FIFO_COUNT_L (ICM20648_BANK_0 | 0x71) /**< FIFO data count low byte */
stevew817 0:296308a935f5 134 #define ICM20648_REG_FIFO_R_W (ICM20648_BANK_0 | 0x72) /**< FIFO Read/Write register */
stevew817 0:296308a935f5 135
stevew817 0:296308a935f5 136 #define ICM20648_REG_DATA_RDY_STATUS (ICM20648_BANK_0 | 0x74) /**< Data Ready Status register */
stevew817 0:296308a935f5 137 #define ICM20648_BIT_RAW_DATA_0_RDY 0x01 /**< Raw Data Ready bit */
stevew817 0:296308a935f5 138
stevew817 0:296308a935f5 139 #define ICM20648_REG_FIFO_CFG (ICM20648_BANK_0 | 0x76) /**< FIFO Configuration register */
stevew817 0:296308a935f5 140 #define ICM20648_BIT_MULTI_FIFO_CFG 0x01 /**< Interrupt status for each sensor is required */
stevew817 0:296308a935f5 141 #define ICM20648_BIT_SINGLE_FIFO_CFG 0x00 /**< Interrupt status for only a single sensor is required */
stevew817 0:296308a935f5 142
stevew817 0:296308a935f5 143 /***********************/
stevew817 0:296308a935f5 144 /* Bank 1 register map */
stevew817 0:296308a935f5 145 /***********************/
stevew817 0:296308a935f5 146 #define ICM20648_REG_XA_OFFSET_H (ICM20648_BANK_1 | 0x14) /**< Acceleration sensor X-axis offset cancellation high byte */
stevew817 0:296308a935f5 147 #define ICM20648_REG_XA_OFFSET_L (ICM20648_BANK_1 | 0x15) /**< Acceleration sensor X-axis offset cancellation low byte */
stevew817 0:296308a935f5 148 #define ICM20648_REG_YA_OFFSET_H (ICM20648_BANK_1 | 0x17) /**< Acceleration sensor Y-axis offset cancellation high byte */
stevew817 0:296308a935f5 149 #define ICM20648_REG_YA_OFFSET_L (ICM20648_BANK_1 | 0x18) /**< Acceleration sensor Y-axis offset cancellation low byte */
stevew817 0:296308a935f5 150 #define ICM20648_REG_ZA_OFFSET_H (ICM20648_BANK_1 | 0x1A) /**< Acceleration sensor Z-axis offset cancellation high byte */
stevew817 0:296308a935f5 151 #define ICM20648_REG_ZA_OFFSET_L (ICM20648_BANK_1 | 0x1B) /**< Acceleration sensor Z-axis offset cancellation low byte */
stevew817 0:296308a935f5 152
stevew817 0:296308a935f5 153 #define ICM20648_REG_TIMEBASE_CORR_PLL (ICM20648_BANK_1 | 0x28) /**< PLL Timebase Correction register */
stevew817 0:296308a935f5 154
stevew817 0:296308a935f5 155 /***********************/
stevew817 0:296308a935f5 156 /* Bank 2 register map */
stevew817 0:296308a935f5 157 /***********************/
stevew817 0:296308a935f5 158 #define ICM20648_REG_GYRO_SMPLRT_DIV (ICM20648_BANK_2 | 0x00) /**< Gyroscope Sample Rate Divider regiser */
stevew817 0:296308a935f5 159
stevew817 0:296308a935f5 160 #define ICM20648_REG_GYRO_CONFIG_1 (ICM20648_BANK_2 | 0x01) /**< Gyroscope Configuration 1 register */
stevew817 0:296308a935f5 161 #define ICM20648_BIT_GYRO_FCHOICE 0x01 /**< Gyro Digital Low-Pass Filter enable bit */
stevew817 0:296308a935f5 162 #define ICM20648_SHIFT_GYRO_FS_SEL 1 /**< Gyro Full Scale Select bit shift */
stevew817 0:296308a935f5 163 #define ICM20648_SHIFT_GYRO_DLPCFG 3 /**< Gyro DLPF Config bit shift */
stevew817 0:296308a935f5 164 #define ICM20648_MASK_GYRO_FULLSCALE 0x06 /**< Gyro Full Scale Select bitmask */
stevew817 0:296308a935f5 165 #define ICM20648_MASK_GYRO_BW 0x39 /**< Gyro Bandwidth Select bitmask */
stevew817 0:296308a935f5 166 #define ICM20648_GYRO_FULLSCALE_250DPS (0x00 << ICM20648_SHIFT_GYRO_FS_SEL) /**< Gyro Full Scale = 250 deg/sec */
stevew817 0:296308a935f5 167 #define ICM20648_GYRO_FULLSCALE_500DPS (0x01 << ICM20648_SHIFT_GYRO_FS_SEL) /**< Gyro Full Scale = 500 deg/sec */
stevew817 0:296308a935f5 168 #define ICM20648_GYRO_FULLSCALE_1000DPS (0x02 << ICM20648_SHIFT_GYRO_FS_SEL) /**< Gyro Full Scale = 1000 deg/sec */
stevew817 0:296308a935f5 169 #define ICM20648_GYRO_FULLSCALE_2000DPS (0x03 << ICM20648_SHIFT_GYRO_FS_SEL) /**< Gyro Full Scale = 2000 deg/sec */
stevew817 0:296308a935f5 170 #define ICM20648_GYRO_BW_12100HZ (0x00 << ICM20648_SHIFT_GYRO_DLPCFG) /**< Gyro Bandwidth = 12100 Hz */
stevew817 0:296308a935f5 171 #define ICM20648_GYRO_BW_360HZ ( (0x07 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 360 Hz */
stevew817 0:296308a935f5 172 #define ICM20648_GYRO_BW_200HZ ( (0x00 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 200 Hz */
stevew817 0:296308a935f5 173 #define ICM20648_GYRO_BW_150HZ ( (0x01 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 150 Hz */
stevew817 0:296308a935f5 174 #define ICM20648_GYRO_BW_120HZ ( (0x02 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 120 Hz */
stevew817 0:296308a935f5 175 #define ICM20648_GYRO_BW_51HZ ( (0x03 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 51 Hz */
stevew817 0:296308a935f5 176 #define ICM20648_GYRO_BW_24HZ ( (0x04 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 24 Hz */
stevew817 0:296308a935f5 177 #define ICM20648_GYRO_BW_12HZ ( (0x05 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 12 Hz */
stevew817 0:296308a935f5 178 #define ICM20648_GYRO_BW_6HZ ( (0x06 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 6 Hz */
stevew817 0:296308a935f5 179
stevew817 0:296308a935f5 180 #define ICM20648_REG_GYRO_CONFIG_2 (ICM20648_BANK_2 | 0x02) /**< Gyroscope Configuration 2 register */
stevew817 0:296308a935f5 181 #define ICM20648_BIT_GYRO_CTEN 0x38 /**< Gyroscope Self-Test Enable bits */
stevew817 0:296308a935f5 182
stevew817 0:296308a935f5 183 #define ICM20648_REG_XG_OFFS_USRH (ICM20648_BANK_2 | 0x03) /**< Gyroscope sensor X-axis offset cancellation high byte */
stevew817 0:296308a935f5 184 #define ICM20648_REG_XG_OFFS_USRL (ICM20648_BANK_2 | 0x04) /**< Gyroscope sensor X-axis offset cancellation low byte */
stevew817 0:296308a935f5 185 #define ICM20648_REG_YG_OFFS_USRH (ICM20648_BANK_2 | 0x05) /**< Gyroscope sensor Y-axis offset cancellation high byte */
stevew817 0:296308a935f5 186 #define ICM20648_REG_YG_OFFS_USRL (ICM20648_BANK_2 | 0x06) /**< Gyroscope sensor Y-axis offset cancellation low byte */
stevew817 0:296308a935f5 187 #define ICM20648_REG_ZG_OFFS_USRH (ICM20648_BANK_2 | 0x07) /**< Gyroscope sensor Z-axis offset cancellation high byte */
stevew817 0:296308a935f5 188 #define ICM20648_REG_ZG_OFFS_USRL (ICM20648_BANK_2 | 0x08) /**< Gyroscope sensor Z-axis offset cancellation low byte */
stevew817 0:296308a935f5 189
stevew817 0:296308a935f5 190 #define ICM20648_REG_ODR_ALIGN_EN (ICM20648_BANK_2 | 0x09) /**< Output Data Rate start time alignment */
stevew817 0:296308a935f5 191
stevew817 0:296308a935f5 192 #define ICM20648_REG_ACCEL_SMPLRT_DIV_1 (ICM20648_BANK_2 | 0x10) /**< Acceleration Sensor Sample Rate Divider 1 register */
stevew817 0:296308a935f5 193 #define ICM20648_REG_ACCEL_SMPLRT_DIV_2 (ICM20648_BANK_2 | 0x11) /**< Acceleration Sensor Sample Rate Divider 2 register */
stevew817 0:296308a935f5 194
stevew817 0:296308a935f5 195 #define ICM20648_REG_ACCEL_INTEL_CTRL (ICM20648_BANK_2 | 0x12) /**< Accelerometer Hardware Intelligence Control register */
stevew817 0:296308a935f5 196 #define ICM20648_BIT_ACCEL_INTEL_EN 0x02 /**< Wake-up On Motion enable bit */
stevew817 0:296308a935f5 197 #define ICM20648_BIT_ACCEL_INTEL_MODE 0x01 /**< WOM algorithm selection bit */
stevew817 0:296308a935f5 198
stevew817 0:296308a935f5 199 #define ICM20648_REG_ACCEL_WOM_THR (ICM20648_BANK_2 | 0x13) /**< Wake-up On Motion Threshold register */
stevew817 0:296308a935f5 200
stevew817 0:296308a935f5 201 #define ICM20648_REG_ACCEL_CONFIG (ICM20648_BANK_2 | 0x14) /**< Accelerometer Configuration register */
stevew817 0:296308a935f5 202 #define ICM20648_BIT_ACCEL_FCHOICE 0x01 /**< Accel Digital Low-Pass Filter enable bit */
stevew817 0:296308a935f5 203 #define ICM20648_SHIFT_ACCEL_FS 1 /**< Accel Full Scale Select bit shift */
stevew817 0:296308a935f5 204 #define ICM20648_SHIFT_ACCEL_DLPCFG 3 /**< Accel DLPF Config bit shift */
stevew817 0:296308a935f5 205 #define ICM20648_MASK_ACCEL_FULLSCALE 0x06 /**< Accel Full Scale Select bitmask */
stevew817 0:296308a935f5 206 #define ICM20648_MASK_ACCEL_BW 0x39 /**< Accel Bandwidth Select bitmask */
stevew817 0:296308a935f5 207 #define ICM20648_ACCEL_FULLSCALE_2G (0x00 << ICM20648_SHIFT_ACCEL_FS) /**< Accel Full Scale = 2 g */
stevew817 0:296308a935f5 208 #define ICM20648_ACCEL_FULLSCALE_4G (0x01 << ICM20648_SHIFT_ACCEL_FS) /**< Accel Full Scale = 4 g */
stevew817 0:296308a935f5 209 #define ICM20648_ACCEL_FULLSCALE_8G (0x02 << ICM20648_SHIFT_ACCEL_FS) /**< Accel Full Scale = 8 g */
stevew817 0:296308a935f5 210 #define ICM20648_ACCEL_FULLSCALE_16G (0x03 << ICM20648_SHIFT_ACCEL_FS) /**< Accel Full Scale = 16 g */
stevew817 0:296308a935f5 211 #define ICM20648_ACCEL_BW_1210HZ (0x00 << ICM20648_SHIFT_ACCEL_DLPCFG) /**< Accel Bandwidth = 1210 Hz */
stevew817 0:296308a935f5 212 #define ICM20648_ACCEL_BW_470HZ ( (0x07 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 470 Hz */
stevew817 0:296308a935f5 213 #define ICM20648_ACCEL_BW_246HZ ( (0x00 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 246 Hz */
stevew817 0:296308a935f5 214 #define ICM20648_ACCEL_BW_111HZ ( (0x02 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 111 Hz */
stevew817 0:296308a935f5 215 #define ICM20648_ACCEL_BW_50HZ ( (0x03 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 50 Hz */
stevew817 0:296308a935f5 216 #define ICM20648_ACCEL_BW_24HZ ( (0x04 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 24 Hz */
stevew817 0:296308a935f5 217 #define ICM20648_ACCEL_BW_12HZ ( (0x05 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 12 Hz */
stevew817 0:296308a935f5 218 #define ICM20648_ACCEL_BW_6HZ ( (0x06 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 6 Hz */
stevew817 0:296308a935f5 219
stevew817 0:296308a935f5 220 #define ICM20648_REG_ACCEL_CONFIG_2 (ICM20648_BANK_2 | 0x15) /**< Accelerometer Configuration 2 register */
stevew817 0:296308a935f5 221 #define ICM20648_BIT_ACCEL_CTEN 0x1C /**< Accelerometer Self-Test Enable bits */
stevew817 0:296308a935f5 222
stevew817 0:296308a935f5 223 /***********************/
stevew817 0:296308a935f5 224 /* Bank 3 register map */
stevew817 0:296308a935f5 225 /***********************/
stevew817 0:296308a935f5 226 #define ICM20648_REG_I2C_MST_ODR_CONFIG (ICM20648_BANK_3 | 0x00) /**< I2C Master Output Data Rate Configuration register */
stevew817 0:296308a935f5 227
stevew817 0:296308a935f5 228 #define ICM20648_REG_I2C_MST_CTRL (ICM20648_BANK_3 | 0x01) /**< I2C Master Control register */
stevew817 0:296308a935f5 229 #define ICM20648_BIT_I2C_MST_P_NSR 0x10 /**< Stop between reads enabling bit */
stevew817 0:296308a935f5 230
stevew817 0:296308a935f5 231 #define ICM20648_REG_I2C_MST_DELAY_CTRL (ICM20648_BANK_3 | 0x02) /**< I2C Master Delay Control register */
stevew817 0:296308a935f5 232 #define ICM20648_BIT_SLV0_DLY_EN 0x01 /**< I2C Slave0 Delay Enable bit */
stevew817 0:296308a935f5 233 #define ICM20648_BIT_SLV1_DLY_EN 0x02 /**< I2C Slave1 Delay Enable bit */
stevew817 0:296308a935f5 234 #define ICM20648_BIT_SLV2_DLY_EN 0x04 /**< I2C Slave2 Delay Enable bit */
stevew817 0:296308a935f5 235 #define ICM20648_BIT_SLV3_DLY_EN 0x08 /**< I2C Slave3 Delay Enable bit */
stevew817 0:296308a935f5 236
stevew817 0:296308a935f5 237 #define ICM20648_REG_I2C_SLV0_ADDR (ICM20648_BANK_3 | 0x03) /**< I2C Slave0 Physical Address register */
stevew817 0:296308a935f5 238 #define ICM20648_REG_I2C_SLV0_REG (ICM20648_BANK_3 | 0x04) /**< I2C Slave0 Register Address register */
stevew817 0:296308a935f5 239 #define ICM20648_REG_I2C_SLV0_CTRL (ICM20648_BANK_3 | 0x05) /**< I2C Slave0 Control register */
stevew817 0:296308a935f5 240 #define ICM20648_REG_I2C_SLV0_DO (ICM20648_BANK_3 | 0x06) /**< I2C Slave0 Data Out register */
stevew817 0:296308a935f5 241
stevew817 0:296308a935f5 242 #define ICM20648_REG_I2C_SLV1_ADDR (ICM20648_BANK_3 | 0x07) /**< I2C Slave1 Physical Address register */
stevew817 0:296308a935f5 243 #define ICM20648_REG_I2C_SLV1_REG (ICM20648_BANK_3 | 0x08) /**< I2C Slave1 Register Address register */
stevew817 0:296308a935f5 244 #define ICM20648_REG_I2C_SLV1_CTRL (ICM20648_BANK_3 | 0x09) /**< I2C Slave1 Control register */
stevew817 0:296308a935f5 245 #define ICM20648_REG_I2C_SLV1_DO (ICM20648_BANK_3 | 0x0A) /**< I2C Slave1 Data Out register */
stevew817 0:296308a935f5 246
stevew817 0:296308a935f5 247 #define ICM20648_REG_I2C_SLV2_ADDR (ICM20648_BANK_3 | 0x0B) /**< I2C Slave2 Physical Address register */
stevew817 0:296308a935f5 248 #define ICM20648_REG_I2C_SLV2_REG (ICM20648_BANK_3 | 0x0C) /**< I2C Slave2 Register Address register */
stevew817 0:296308a935f5 249 #define ICM20648_REG_I2C_SLV2_CTRL (ICM20648_BANK_3 | 0x0D) /**< I2C Slave2 Control register */
stevew817 0:296308a935f5 250 #define ICM20648_REG_I2C_SLV2_DO (ICM20648_BANK_3 | 0x0E) /**< I2C Slave2 Data Out register */
stevew817 0:296308a935f5 251
stevew817 0:296308a935f5 252 #define ICM20648_REG_I2C_SLV3_ADDR (ICM20648_BANK_3 | 0x0F) /**< I2C Slave3 Physical Address register */
stevew817 0:296308a935f5 253 #define ICM20648_REG_I2C_SLV3_REG (ICM20648_BANK_3 | 0x10) /**< I2C Slave3 Register Address register */
stevew817 0:296308a935f5 254 #define ICM20648_REG_I2C_SLV3_CTRL (ICM20648_BANK_3 | 0x11) /**< I2C Slave3 Control register */
stevew817 0:296308a935f5 255 #define ICM20648_REG_I2C_SLV3_DO (ICM20648_BANK_3 | 0x12) /**< I2C Slave3 Data Out register */
stevew817 0:296308a935f5 256
stevew817 0:296308a935f5 257 #define ICM20648_REG_I2C_SLV4_ADDR (ICM20648_BANK_3 | 0x13) /**< I2C Slave4 Physical Address register */
stevew817 0:296308a935f5 258 #define ICM20648_REG_I2C_SLV4_REG (ICM20648_BANK_3 | 0x14) /**< I2C Slave4 Register Address register */
stevew817 0:296308a935f5 259 #define ICM20648_REG_I2C_SLV4_CTRL (ICM20648_BANK_3 | 0x15) /**< I2C Slave4 Control register */
stevew817 0:296308a935f5 260 #define ICM20648_REG_I2C_SLV4_DO (ICM20648_BANK_3 | 0x16) /**< I2C Slave4 Data Out register */
stevew817 0:296308a935f5 261 #define ICM20648_REG_I2C_SLV4_DI (ICM20648_BANK_3 | 0x17) /**< I2C Slave4 Data In register */
stevew817 0:296308a935f5 262
stevew817 0:296308a935f5 263 #define ICM20648_BIT_I2C_SLV_EN 0x80 /**< I2C Slave Enable bit */
stevew817 0:296308a935f5 264 #define ICM20648_BIT_I2C_BYTE_SW 0x40 /**< I2C Slave Byte Swap enable bit */
stevew817 0:296308a935f5 265 #define ICM20648_BIT_I2C_REG_DIS 0x20 /**< I2C Slave Do Not Write Register Value bit */
stevew817 0:296308a935f5 266 #define ICM20648_BIT_I2C_GRP 0x10 /**< I2C Slave Group bit */
stevew817 0:296308a935f5 267 #define ICM20648_BIT_I2C_READ 0x80 /**< I2C Slave R/W bit */
stevew817 0:296308a935f5 268
stevew817 0:296308a935f5 269 /* Register common for all banks */
stevew817 0:296308a935f5 270 #define ICM20648_REG_BANK_SEL 0x7F /**< Bank Select register */
stevew817 0:296308a935f5 271
stevew817 0:296308a935f5 272 #define ICM20648_DEVICE_ID 0xE0 /**< ICM20648 Device ID value */
stevew817 0:296308a935f5 273 #define ICM20948_DEVICE_ID 0xEA /**< ICM20948 Device ID value */
stevew817 0:296308a935f5 274 /**@}*/
stevew817 0:296308a935f5 275
stevew817 0:296308a935f5 276 /** @endcond */
stevew817 0:296308a935f5 277
stevew817 0:296308a935f5 278
stevew817 0:296308a935f5 279 ICM20648::ICM20648(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName irq) : m_SPI(mosi, miso, sclk), m_CS(cs, 1), m_IRQ(irq)
stevew817 0:296308a935f5 280 {
stevew817 0:296308a935f5 281 m_IRQ.disable_irq();
stevew817 0:296308a935f5 282 m_IRQ.fall(Callback<void(void)>(this, &ICM20648::irq_handler));
stevew817 0:296308a935f5 283 }
stevew817 0:296308a935f5 284
stevew817 0:296308a935f5 285 ICM20648::~ICM20648(void)
stevew817 0:296308a935f5 286 {
stevew817 0:296308a935f5 287 }
stevew817 0:296308a935f5 288
stevew817 0:296308a935f5 289 bool ICM20648::open()
stevew817 0:296308a935f5 290 {
stevew817 0:296308a935f5 291 uint8_t data;
stevew817 0:296308a935f5 292
stevew817 0:296308a935f5 293 reset();
stevew817 0:296308a935f5 294
stevew817 0:296308a935f5 295 /* Disable I2C interface, use SPI */
stevew817 0:296308a935f5 296 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_I2C_IF_DIS);
stevew817 0:296308a935f5 297
stevew817 0:296308a935f5 298 /* Read Who am I register, should get 0x71 */
stevew817 0:296308a935f5 299 read_register(ICM20648_REG_WHO_AM_I, 1, &data);
stevew817 0:296308a935f5 300
stevew817 0:296308a935f5 301 /* If not - return */
stevew817 0:296308a935f5 302 if ( (data != ICM20648_DEVICE_ID) && (data != ICM20948_DEVICE_ID) ) {
stevew817 0:296308a935f5 303 return false;
stevew817 0:296308a935f5 304 }
stevew817 0:296308a935f5 305
stevew817 0:296308a935f5 306 /* Auto selects the best available clock source – PLL if ready, else use the Internal oscillator */
stevew817 0:296308a935f5 307 write_register(ICM20648_REG_PWR_MGMT_1, ICM20648_BIT_CLK_PLL);
stevew817 0:296308a935f5 308
stevew817 0:296308a935f5 309 /* PLL startup time - maybe it is too long but better be on the safe side, no spec in the datasheet */
stevew817 0:296308a935f5 310 wait_ms(30);
stevew817 0:296308a935f5 311
stevew817 0:296308a935f5 312 /* INT pin: active low, open drain, IT status read clears. It seems that latched mode does not work, the INT pin cannot be cleared if set */
stevew817 0:296308a935f5 313 write_register(ICM20648_REG_INT_PIN_CFG, ICM20648_BIT_INT_ACTL | ICM20648_BIT_INT_OPEN);
stevew817 0:296308a935f5 314
stevew817 0:296308a935f5 315 return true;
stevew817 0:296308a935f5 316 }
stevew817 0:296308a935f5 317
stevew817 0:296308a935f5 318 /** Perform a measurement
stevew817 0:296308a935f5 319 *
stevew817 0:296308a935f5 320 * @returns true if measurement was successful
stevew817 0:296308a935f5 321 */
stevew817 0:296308a935f5 322 bool ICM20648::measure()
stevew817 0:296308a935f5 323 {
stevew817 0:296308a935f5 324
stevew817 0:296308a935f5 325 }
stevew817 0:296308a935f5 326
stevew817 0:296308a935f5 327 /** Do a measurement on the gyroscope
stevew817 0:296308a935f5 328 *
stevew817 0:296308a935f5 329 * @param[out] gyr_x Gyroscope measurement on X axis
stevew817 0:296308a935f5 330 * @param[out] gyr_y Gyroscope measurement on Y axis
stevew817 0:296308a935f5 331 * @param[out] gyr_z Gyroscope measurement on Z axis
stevew817 0:296308a935f5 332 *
stevew817 0:296308a935f5 333 * @returns true if measurement was successful
stevew817 0:296308a935f5 334 */
stevew817 0:296308a935f5 335 bool ICM20648::get_gyroscope(float *gyr_x, float *gyr_y, float *gyr_z)
stevew817 0:296308a935f5 336 {
stevew817 0:296308a935f5 337 float buf[3];
stevew817 0:296308a935f5 338 if(read_gyro_data(buf)) {
stevew817 0:296308a935f5 339 return false;
stevew817 0:296308a935f5 340 }
stevew817 0:296308a935f5 341
stevew817 0:296308a935f5 342 *gyr_x = buf[0];
stevew817 0:296308a935f5 343 *gyr_y = buf[1];
stevew817 0:296308a935f5 344 *gyr_z = buf[2];
stevew817 0:296308a935f5 345 }
stevew817 0:296308a935f5 346
stevew817 0:296308a935f5 347 /** Do a measurement on the accelerometer
stevew817 0:296308a935f5 348 *
stevew817 0:296308a935f5 349 * @param[out] acc_x Accelerometer measurement on X axis
stevew817 0:296308a935f5 350 * @param[out] acc_y Accelerometer measurement on Y axis
stevew817 0:296308a935f5 351 * @param[out] acc_z Accelerometer measurement on Z axis
stevew817 0:296308a935f5 352 *
stevew817 0:296308a935f5 353 * @returns true if measurement was successful
stevew817 0:296308a935f5 354 */
stevew817 0:296308a935f5 355 bool ICM20648::get_accelerometer(float *acc_x, float *acc_y, float *acc_z)
stevew817 0:296308a935f5 356 {
stevew817 0:296308a935f5 357 float buf[3];
stevew817 0:296308a935f5 358 if(read_accel_data(buf)) {
stevew817 0:296308a935f5 359 return false;
stevew817 0:296308a935f5 360 }
stevew817 0:296308a935f5 361
stevew817 0:296308a935f5 362 *acc_x = buf[0];
stevew817 0:296308a935f5 363 *acc_y = buf[1];
stevew817 0:296308a935f5 364 *acc_z = buf[2];
stevew817 0:296308a935f5 365 }
stevew817 0:296308a935f5 366
stevew817 0:296308a935f5 367 bool ICM20648::get_temperature(float *temperature)
stevew817 0:296308a935f5 368 {
stevew817 0:296308a935f5 369 read_temperature(temperature);
stevew817 0:296308a935f5 370 return true;
stevew817 0:296308a935f5 371 }
stevew817 0:296308a935f5 372
stevew817 0:296308a935f5 373 /***************************************************************************//**
stevew817 0:296308a935f5 374 * @brief
stevew817 0:296308a935f5 375 * Reads register from the ICM20648 device
stevew817 0:296308a935f5 376 *
stevew817 0:296308a935f5 377 * @param[in] addr
stevew817 0:296308a935f5 378 * The register address to read from in the sensor
stevew817 0:296308a935f5 379 * Bit[8:7] - bank address
stevew817 0:296308a935f5 380 * Bit[6:0] - register address
stevew817 0:296308a935f5 381 *
stevew817 0:296308a935f5 382 * @param[in] numBytes
stevew817 0:296308a935f5 383 * The number of bytes to read
stevew817 0:296308a935f5 384 *
stevew817 0:296308a935f5 385 * @param[out] data
stevew817 0:296308a935f5 386 * The data read from the register
stevew817 0:296308a935f5 387 *
stevew817 0:296308a935f5 388 * @return
stevew817 0:296308a935f5 389 * None
stevew817 0:296308a935f5 390 ******************************************************************************/
stevew817 0:296308a935f5 391 void ICM20648::read_register(uint16_t addr, int numBytes, uint8_t *data)
stevew817 0:296308a935f5 392 {
stevew817 0:296308a935f5 393 uint8_t regAddr;
stevew817 0:296308a935f5 394 uint8_t bank;
stevew817 0:296308a935f5 395
stevew817 0:296308a935f5 396 regAddr = (uint8_t) (addr & 0x7F);
stevew817 0:296308a935f5 397 bank = (uint8_t) (addr >> 7);
stevew817 0:296308a935f5 398
stevew817 0:296308a935f5 399 select_bank(bank);
stevew817 0:296308a935f5 400
stevew817 0:296308a935f5 401 /* Enable chip select */
stevew817 0:296308a935f5 402 m_CS = 0;
stevew817 0:296308a935f5 403
stevew817 0:296308a935f5 404 /* Set R/W bit to 1 - read */
stevew817 0:296308a935f5 405 m_SPI.write(regAddr | 0x80);
stevew817 0:296308a935f5 406 /* Transmit 0's to provide clock and read the data */
stevew817 0:296308a935f5 407 m_SPI.write(NULL, 0, (char*)data, numBytes);
stevew817 0:296308a935f5 408
stevew817 0:296308a935f5 409 /* Disable chip select */
stevew817 0:296308a935f5 410 m_CS = 1;
stevew817 0:296308a935f5 411
stevew817 0:296308a935f5 412 return;
stevew817 0:296308a935f5 413 }
stevew817 0:296308a935f5 414
stevew817 0:296308a935f5 415 /***************************************************************************//**
stevew817 0:296308a935f5 416 * @brief
stevew817 0:296308a935f5 417 * Writes a register in the ICM20648 device
stevew817 0:296308a935f5 418 *
stevew817 0:296308a935f5 419 * @param[in] addr
stevew817 0:296308a935f5 420 * The register address to write
stevew817 0:296308a935f5 421 * Bit[8:7] - bank address
stevew817 0:296308a935f5 422 * Bit[6:0] - register address
stevew817 0:296308a935f5 423 *
stevew817 0:296308a935f5 424 * @param[in] data
stevew817 0:296308a935f5 425 * The data to write to the register
stevew817 0:296308a935f5 426 *
stevew817 0:296308a935f5 427 * @return
stevew817 0:296308a935f5 428 * None
stevew817 0:296308a935f5 429 ******************************************************************************/
stevew817 0:296308a935f5 430 void ICM20648::write_register(uint16_t addr, uint8_t data)
stevew817 0:296308a935f5 431 {
stevew817 0:296308a935f5 432 uint8_t regAddr;
stevew817 0:296308a935f5 433 uint8_t bank;
stevew817 0:296308a935f5 434
stevew817 0:296308a935f5 435 regAddr = (uint8_t) (addr & 0x7F);
stevew817 0:296308a935f5 436 bank = (uint8_t) (addr >> 7);
stevew817 0:296308a935f5 437
stevew817 0:296308a935f5 438 select_bank(bank);
stevew817 0:296308a935f5 439
stevew817 0:296308a935f5 440 /* Enable chip select */
stevew817 0:296308a935f5 441 m_CS = 0;
stevew817 0:296308a935f5 442
stevew817 0:296308a935f5 443 /* clear R/W bit - write, send the address */
stevew817 0:296308a935f5 444 m_SPI.write(regAddr & 0x7F);
stevew817 0:296308a935f5 445 m_SPI.write(data);
stevew817 0:296308a935f5 446
stevew817 0:296308a935f5 447 /* Disable chip select */
stevew817 0:296308a935f5 448 m_CS = 1;
stevew817 0:296308a935f5 449
stevew817 0:296308a935f5 450 return;
stevew817 0:296308a935f5 451 }
stevew817 0:296308a935f5 452
stevew817 0:296308a935f5 453 /***************************************************************************//**
stevew817 0:296308a935f5 454 * @brief
stevew817 0:296308a935f5 455 * Select the desired register bank
stevew817 0:296308a935f5 456 *
stevew817 0:296308a935f5 457 * @param[in] bank
stevew817 0:296308a935f5 458 * The address of the register bank (0..3)
stevew817 0:296308a935f5 459 *
stevew817 0:296308a935f5 460 * @return
stevew817 0:296308a935f5 461 * None
stevew817 0:296308a935f5 462 ******************************************************************************/
stevew817 0:296308a935f5 463 void ICM20648::select_bank(uint8_t bank)
stevew817 0:296308a935f5 464 {
stevew817 0:296308a935f5 465 /* Enable chip select */
stevew817 0:296308a935f5 466 m_CS = 0;
stevew817 0:296308a935f5 467
stevew817 0:296308a935f5 468 /* clear R/W bit - write, send the address */
stevew817 0:296308a935f5 469 m_SPI.write(ICM20648_REG_BANK_SEL);
stevew817 0:296308a935f5 470 m_SPI.write((uint8_t)(bank << 4));
stevew817 0:296308a935f5 471
stevew817 0:296308a935f5 472 /* Disable chip select */
stevew817 0:296308a935f5 473 m_CS = 1;
stevew817 0:296308a935f5 474
stevew817 0:296308a935f5 475 return;
stevew817 0:296308a935f5 476 }
stevew817 0:296308a935f5 477
stevew817 0:296308a935f5 478 /***************************************************************************//**
stevew817 0:296308a935f5 479 * @brief
stevew817 0:296308a935f5 480 * Performs soft reset on the ICM20648 chip
stevew817 0:296308a935f5 481 *
stevew817 0:296308a935f5 482 * @return
stevew817 0:296308a935f5 483 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 484 ******************************************************************************/
stevew817 0:296308a935f5 485 uint32_t ICM20648::reset(void)
stevew817 0:296308a935f5 486 {
stevew817 0:296308a935f5 487 /* Set H_RESET bit to initiate soft reset */
stevew817 0:296308a935f5 488 write_register(ICM20648_REG_PWR_MGMT_1, ICM20648_BIT_H_RESET);
stevew817 0:296308a935f5 489
stevew817 0:296308a935f5 490 /* Wait 100ms to complete the reset sequence */
stevew817 0:296308a935f5 491 wait_ms(100);
stevew817 0:296308a935f5 492
stevew817 0:296308a935f5 493 return ICM20648_OK;
stevew817 0:296308a935f5 494 }
stevew817 0:296308a935f5 495
stevew817 0:296308a935f5 496 /***************************************************************************//**
stevew817 0:296308a935f5 497 * @brief
stevew817 0:296308a935f5 498 * Sets the sample rate both of the accelerometer and the gyroscope.
stevew817 0:296308a935f5 499 *
stevew817 0:296308a935f5 500 * @param[in] sampleRate
stevew817 0:296308a935f5 501 * The desired sample rate in Hz. Since the resolution of the sample rate
stevew817 0:296308a935f5 502 * divider is different in the accel and gyro stages it is possible that
stevew817 0:296308a935f5 503 * the two sensor will have different sample rate set.
stevew817 0:296308a935f5 504 *
stevew817 0:296308a935f5 505 * @return
stevew817 0:296308a935f5 506 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 507 ******************************************************************************/
stevew817 0:296308a935f5 508 uint32_t ICM20648::set_sample_rate(float sampleRate)
stevew817 0:296308a935f5 509 {
stevew817 0:296308a935f5 510 set_gyro_sample_rate(sampleRate);
stevew817 0:296308a935f5 511 set_accel_sample_rate(sampleRate);
stevew817 0:296308a935f5 512
stevew817 0:296308a935f5 513 return ICM20648_OK;
stevew817 0:296308a935f5 514 }
stevew817 0:296308a935f5 515
stevew817 0:296308a935f5 516 /***************************************************************************//**
stevew817 0:296308a935f5 517 * @brief
stevew817 0:296308a935f5 518 * Sets the sample rate of the accelerometer
stevew817 0:296308a935f5 519 *
stevew817 0:296308a935f5 520 * @param[in] sampleRate
stevew817 0:296308a935f5 521 * The desired sample rate in Hz
stevew817 0:296308a935f5 522 *
stevew817 0:296308a935f5 523 * @return
stevew817 0:296308a935f5 524 * The actual sample rate. May be different from the desired value because
stevew817 0:296308a935f5 525 * of the finite and discrete number of divider settings
stevew817 0:296308a935f5 526 ******************************************************************************/
stevew817 0:296308a935f5 527 float ICM20648::set_gyro_sample_rate(float sampleRate)
stevew817 0:296308a935f5 528 {
stevew817 0:296308a935f5 529 uint8_t gyroDiv;
stevew817 0:296308a935f5 530 float gyroSampleRate;
stevew817 0:296308a935f5 531
stevew817 0:296308a935f5 532 /* Calculate the sample rate divider */
stevew817 0:296308a935f5 533 gyroSampleRate = (1125.0 / sampleRate) - 1.0;
stevew817 0:296308a935f5 534
stevew817 0:296308a935f5 535 /* Check if it fits in the divider register */
stevew817 0:296308a935f5 536 if ( gyroSampleRate > 255.0 ) {
stevew817 0:296308a935f5 537 gyroSampleRate = 255.0;
stevew817 0:296308a935f5 538 }
stevew817 0:296308a935f5 539
stevew817 0:296308a935f5 540 if ( gyroSampleRate < 0 ) {
stevew817 0:296308a935f5 541 gyroSampleRate = 0.0;
stevew817 0:296308a935f5 542 }
stevew817 0:296308a935f5 543
stevew817 0:296308a935f5 544 /* Write the value to the register */
stevew817 0:296308a935f5 545 gyroDiv = (uint8_t) gyroSampleRate;
stevew817 0:296308a935f5 546 write_register(ICM20648_REG_GYRO_SMPLRT_DIV, gyroDiv);
stevew817 0:296308a935f5 547
stevew817 0:296308a935f5 548 /* Calculate the actual sample rate from the divider value */
stevew817 0:296308a935f5 549 gyroSampleRate = 1125.0 / (gyroDiv + 1);
stevew817 0:296308a935f5 550
stevew817 0:296308a935f5 551 return gyroSampleRate;
stevew817 0:296308a935f5 552 }
stevew817 0:296308a935f5 553
stevew817 0:296308a935f5 554 /***************************************************************************//**
stevew817 0:296308a935f5 555 * @brief
stevew817 0:296308a935f5 556 * Sets the sample rate of the gyroscope
stevew817 0:296308a935f5 557 *
stevew817 0:296308a935f5 558 * @param[in] sampleRate
stevew817 0:296308a935f5 559 * The desired sample rate in Hz
stevew817 0:296308a935f5 560 *
stevew817 0:296308a935f5 561 * @return
stevew817 0:296308a935f5 562 * The actual sample rate. May be different from the desired value because
stevew817 0:296308a935f5 563 * of the finite and discrete number of divider settings
stevew817 0:296308a935f5 564 ******************************************************************************/
stevew817 0:296308a935f5 565 float ICM20648::set_accel_sample_rate(float sampleRate)
stevew817 0:296308a935f5 566 {
stevew817 0:296308a935f5 567 uint16_t accelDiv;
stevew817 0:296308a935f5 568 float accelSampleRate;
stevew817 0:296308a935f5 569
stevew817 0:296308a935f5 570 /* Calculate the sample rate divider */
stevew817 0:296308a935f5 571 accelSampleRate = (1125.0 / sampleRate) - 1.0;
stevew817 0:296308a935f5 572
stevew817 0:296308a935f5 573 /* Check if it fits in the divider registers */
stevew817 0:296308a935f5 574 if ( accelSampleRate > 4095.0 ) {
stevew817 0:296308a935f5 575 accelSampleRate = 4095.0;
stevew817 0:296308a935f5 576 }
stevew817 0:296308a935f5 577
stevew817 0:296308a935f5 578 if ( accelSampleRate < 0 ) {
stevew817 0:296308a935f5 579 accelSampleRate = 0.0;
stevew817 0:296308a935f5 580 }
stevew817 0:296308a935f5 581
stevew817 0:296308a935f5 582 /* Write the value to the registers */
stevew817 0:296308a935f5 583 accelDiv = (uint16_t) accelSampleRate;
stevew817 0:296308a935f5 584 write_register(ICM20648_REG_ACCEL_SMPLRT_DIV_1, (uint8_t) (accelDiv >> 8) );
stevew817 0:296308a935f5 585 write_register(ICM20648_REG_ACCEL_SMPLRT_DIV_2, (uint8_t) (accelDiv & 0xFF) );
stevew817 0:296308a935f5 586
stevew817 0:296308a935f5 587 /* Calculate the actual sample rate from the divider value */
stevew817 0:296308a935f5 588 accelSampleRate = 1125.0 / (accelDiv + 1);
stevew817 0:296308a935f5 589
stevew817 0:296308a935f5 590 return accelSampleRate;
stevew817 0:296308a935f5 591 }
stevew817 0:296308a935f5 592
stevew817 0:296308a935f5 593 /***************************************************************************//**
stevew817 0:296308a935f5 594 * @brief
stevew817 0:296308a935f5 595 * Sets the bandwidth of the gyroscope
stevew817 0:296308a935f5 596 *
stevew817 0:296308a935f5 597 * @param[in] gyroBw
stevew817 0:296308a935f5 598 * The desired bandwidth value. Use the ICM20648_GYRO_BW_xHZ macros, which
stevew817 0:296308a935f5 599 * are defined in the icm20648.h file. The value of x can be
stevew817 0:296308a935f5 600 * 6, 12, 24, 51, 120, 150, 200, 360 or 12100.
stevew817 0:296308a935f5 601 *
stevew817 0:296308a935f5 602 * @return
stevew817 0:296308a935f5 603 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 604 ******************************************************************************/
stevew817 0:296308a935f5 605 uint32_t ICM20648::set_gyro_bandwidth(uint8_t gyroBw)
stevew817 0:296308a935f5 606 {
stevew817 0:296308a935f5 607 uint8_t reg;
stevew817 0:296308a935f5 608
stevew817 0:296308a935f5 609 /* Read the GYRO_CONFIG_1 register */
stevew817 0:296308a935f5 610 read_register(ICM20648_REG_GYRO_CONFIG_1, 1, &reg);
stevew817 0:296308a935f5 611 reg &= ~(ICM20648_MASK_GYRO_BW);
stevew817 0:296308a935f5 612
stevew817 0:296308a935f5 613 /* Write the new bandwidth value to the gyro config register */
stevew817 0:296308a935f5 614 reg |= (gyroBw & ICM20648_MASK_GYRO_BW);
stevew817 0:296308a935f5 615 write_register(ICM20648_REG_GYRO_CONFIG_1, reg);
stevew817 0:296308a935f5 616
stevew817 0:296308a935f5 617 return ICM20648_OK;
stevew817 0:296308a935f5 618 }
stevew817 0:296308a935f5 619
stevew817 0:296308a935f5 620 /***************************************************************************//**
stevew817 0:296308a935f5 621 * @brief
stevew817 0:296308a935f5 622 * Sets the bandwidth of the accelerometer
stevew817 0:296308a935f5 623 *
stevew817 0:296308a935f5 624 * @param[in] accelBw
stevew817 0:296308a935f5 625 * The desired bandwidth value. Use the ICM20648_ACCEL_BW_yHZ macros, which
stevew817 0:296308a935f5 626 * are defined in the icm20648.h file. The value of y can be
stevew817 0:296308a935f5 627 * 6, 12, 24, 50, 111, 246, 470 or 1210.
stevew817 0:296308a935f5 628 *
stevew817 0:296308a935f5 629 * @return
stevew817 0:296308a935f5 630 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 631 ******************************************************************************/
stevew817 0:296308a935f5 632 uint32_t ICM20648::set_accel_bandwidth(uint8_t accelBw)
stevew817 0:296308a935f5 633 {
stevew817 0:296308a935f5 634 uint8_t reg;
stevew817 0:296308a935f5 635
stevew817 0:296308a935f5 636 /* Read the GYRO_CONFIG_1 register */
stevew817 0:296308a935f5 637 read_register(ICM20648_REG_ACCEL_CONFIG, 1, &reg);
stevew817 0:296308a935f5 638 reg &= ~(ICM20648_MASK_ACCEL_BW);
stevew817 0:296308a935f5 639
stevew817 0:296308a935f5 640 /* Write the new bandwidth value to the gyro config register */
stevew817 0:296308a935f5 641 reg |= (accelBw & ICM20648_MASK_ACCEL_BW);
stevew817 0:296308a935f5 642 write_register(ICM20648_REG_ACCEL_CONFIG, reg);
stevew817 0:296308a935f5 643
stevew817 0:296308a935f5 644 return ICM20648_OK;
stevew817 0:296308a935f5 645 }
stevew817 0:296308a935f5 646
stevew817 0:296308a935f5 647 /***************************************************************************//**
stevew817 0:296308a935f5 648 * @brief
stevew817 0:296308a935f5 649 * Reads the raw acceleration value and converts to g value based on
stevew817 0:296308a935f5 650 * the actual resolution
stevew817 0:296308a935f5 651 *
stevew817 0:296308a935f5 652 * @param[out] accel
stevew817 0:296308a935f5 653 * A 3-element array of float numbers containing the acceleration values
stevew817 0:296308a935f5 654 * for the x, y and z axes in g units.
stevew817 0:296308a935f5 655 *
stevew817 0:296308a935f5 656 * @return
stevew817 0:296308a935f5 657 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 658 ******************************************************************************/
stevew817 0:296308a935f5 659 uint32_t ICM20648::read_accel_data(float *accel)
stevew817 0:296308a935f5 660 {
stevew817 0:296308a935f5 661 uint8_t rawData[6];
stevew817 0:296308a935f5 662 float accelRes;
stevew817 0:296308a935f5 663 int16_t temp;
stevew817 0:296308a935f5 664
stevew817 0:296308a935f5 665 /* Retrieve the current resolution */
stevew817 0:296308a935f5 666 get_accel_resolution(&accelRes);
stevew817 0:296308a935f5 667
stevew817 0:296308a935f5 668 /* Read the six raw data registers into data array */
stevew817 0:296308a935f5 669 read_register(ICM20648_REG_ACCEL_XOUT_H_SH, 6, &rawData[0]);
stevew817 0:296308a935f5 670
stevew817 0:296308a935f5 671 /* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the G value */
stevew817 0:296308a935f5 672 temp = ( (int16_t) rawData[0] << 8) | rawData[1];
stevew817 0:296308a935f5 673 accel[0] = (float) temp * accelRes;
stevew817 0:296308a935f5 674 temp = ( (int16_t) rawData[2] << 8) | rawData[3];
stevew817 0:296308a935f5 675 accel[1] = (float) temp * accelRes;
stevew817 0:296308a935f5 676 temp = ( (int16_t) rawData[4] << 8) | rawData[5];
stevew817 0:296308a935f5 677 accel[2] = (float) temp * accelRes;
stevew817 0:296308a935f5 678
stevew817 0:296308a935f5 679 return ICM20648_OK;
stevew817 0:296308a935f5 680 }
stevew817 0:296308a935f5 681
stevew817 0:296308a935f5 682 /***************************************************************************//**
stevew817 0:296308a935f5 683 * @brief
stevew817 0:296308a935f5 684 * Reads the raw gyroscope value and converts to deg/sec value based on
stevew817 0:296308a935f5 685 * the actual resolution
stevew817 0:296308a935f5 686 *
stevew817 0:296308a935f5 687 * @param[out] gyro
stevew817 0:296308a935f5 688 * A 3-element array of float numbers containing the gyroscope values
stevew817 0:296308a935f5 689 * for the x, y and z axes in deg/sec units.
stevew817 0:296308a935f5 690 *
stevew817 0:296308a935f5 691 * @return
stevew817 0:296308a935f5 692 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 693 ******************************************************************************/
stevew817 0:296308a935f5 694 uint32_t ICM20648::read_gyro_data(float *gyro)
stevew817 0:296308a935f5 695 {
stevew817 0:296308a935f5 696 uint8_t rawData[6];
stevew817 0:296308a935f5 697 float gyroRes;
stevew817 0:296308a935f5 698 int16_t temp;
stevew817 0:296308a935f5 699
stevew817 0:296308a935f5 700 /* Retrieve the current resolution */
stevew817 0:296308a935f5 701 get_gyro_resolution(&gyroRes);
stevew817 0:296308a935f5 702
stevew817 0:296308a935f5 703 /* Read the six raw data registers into data array */
stevew817 0:296308a935f5 704 read_register(ICM20648_REG_GYRO_XOUT_H_SH, 6, &rawData[0]);
stevew817 0:296308a935f5 705
stevew817 0:296308a935f5 706 /* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the dps value */
stevew817 0:296308a935f5 707 temp = ( (int16_t) rawData[0] << 8) | rawData[1];
stevew817 0:296308a935f5 708 gyro[0] = (float) temp * gyroRes;
stevew817 0:296308a935f5 709 temp = ( (int16_t) rawData[2] << 8) | rawData[3];
stevew817 0:296308a935f5 710 gyro[1] = (float) temp * gyroRes;
stevew817 0:296308a935f5 711 temp = ( (int16_t) rawData[4] << 8) | rawData[5];
stevew817 0:296308a935f5 712 gyro[2] = (float) temp * gyroRes;
stevew817 0:296308a935f5 713
stevew817 0:296308a935f5 714 return ICM20648_OK;
stevew817 0:296308a935f5 715 }
stevew817 0:296308a935f5 716
stevew817 0:296308a935f5 717 /***************************************************************************//**
stevew817 0:296308a935f5 718 * @brief
stevew817 0:296308a935f5 719 * Gets the actual resolution of the accelerometer
stevew817 0:296308a935f5 720 *
stevew817 0:296308a935f5 721 * @param[out] accelRes
stevew817 0:296308a935f5 722 * The resolution in g/bit units
stevew817 0:296308a935f5 723 *
stevew817 0:296308a935f5 724 * @return
stevew817 0:296308a935f5 725 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 726 ******************************************************************************/
stevew817 0:296308a935f5 727 uint32_t ICM20648::get_accel_resolution(float *accelRes)
stevew817 0:296308a935f5 728 {
stevew817 0:296308a935f5 729 uint8_t reg;
stevew817 0:296308a935f5 730
stevew817 0:296308a935f5 731 /* Read the actual acceleration full scale setting */
stevew817 0:296308a935f5 732 read_register(ICM20648_REG_ACCEL_CONFIG, 1, &reg);
stevew817 0:296308a935f5 733 reg &= ICM20648_MASK_ACCEL_FULLSCALE;
stevew817 0:296308a935f5 734
stevew817 0:296308a935f5 735 /* Calculate the resolution */
stevew817 0:296308a935f5 736 switch ( reg ) {
stevew817 0:296308a935f5 737 case ICM20648_ACCEL_FULLSCALE_2G:
stevew817 0:296308a935f5 738 *accelRes = 2.0 / 32768.0;
stevew817 0:296308a935f5 739 break;
stevew817 0:296308a935f5 740
stevew817 0:296308a935f5 741 case ICM20648_ACCEL_FULLSCALE_4G:
stevew817 0:296308a935f5 742 *accelRes = 4.0 / 32768.0;
stevew817 0:296308a935f5 743 break;
stevew817 0:296308a935f5 744
stevew817 0:296308a935f5 745 case ICM20648_ACCEL_FULLSCALE_8G:
stevew817 0:296308a935f5 746 *accelRes = 8.0 / 32768.0;
stevew817 0:296308a935f5 747 break;
stevew817 0:296308a935f5 748
stevew817 0:296308a935f5 749 case ICM20648_ACCEL_FULLSCALE_16G:
stevew817 0:296308a935f5 750 *accelRes = 16.0 / 32768.0;
stevew817 0:296308a935f5 751 break;
stevew817 0:296308a935f5 752 }
stevew817 0:296308a935f5 753
stevew817 0:296308a935f5 754 return ICM20648_OK;
stevew817 0:296308a935f5 755 }
stevew817 0:296308a935f5 756
stevew817 0:296308a935f5 757 /***************************************************************************//**
stevew817 0:296308a935f5 758 * @brief
stevew817 0:296308a935f5 759 * Gets the actual resolution of the gyroscope
stevew817 0:296308a935f5 760 *
stevew817 0:296308a935f5 761 * @param[out] gyroRes
stevew817 0:296308a935f5 762 * The actual resolution in (deg/sec)/bit units
stevew817 0:296308a935f5 763 *
stevew817 0:296308a935f5 764 * @return
stevew817 0:296308a935f5 765 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 766 ******************************************************************************/
stevew817 0:296308a935f5 767 uint32_t ICM20648::get_gyro_resolution(float *gyroRes)
stevew817 0:296308a935f5 768 {
stevew817 0:296308a935f5 769 uint8_t reg;
stevew817 0:296308a935f5 770
stevew817 0:296308a935f5 771 /* Read the actual gyroscope full scale setting */
stevew817 0:296308a935f5 772 read_register(ICM20648_REG_GYRO_CONFIG_1, 1, &reg);
stevew817 0:296308a935f5 773 reg &= ICM20648_MASK_GYRO_FULLSCALE;
stevew817 0:296308a935f5 774
stevew817 0:296308a935f5 775 /* Calculate the resolution */
stevew817 0:296308a935f5 776 switch ( reg ) {
stevew817 0:296308a935f5 777 case ICM20648_GYRO_FULLSCALE_250DPS:
stevew817 0:296308a935f5 778 *gyroRes = 250.0 / 32768.0;
stevew817 0:296308a935f5 779 break;
stevew817 0:296308a935f5 780
stevew817 0:296308a935f5 781 case ICM20648_GYRO_FULLSCALE_500DPS:
stevew817 0:296308a935f5 782 *gyroRes = 500.0 / 32768.0;
stevew817 0:296308a935f5 783 break;
stevew817 0:296308a935f5 784
stevew817 0:296308a935f5 785 case ICM20648_GYRO_FULLSCALE_1000DPS:
stevew817 0:296308a935f5 786 *gyroRes = 1000.0 / 32768.0;
stevew817 0:296308a935f5 787 break;
stevew817 0:296308a935f5 788
stevew817 0:296308a935f5 789 case ICM20648_GYRO_FULLSCALE_2000DPS:
stevew817 0:296308a935f5 790 *gyroRes = 2000.0 / 32768.0;
stevew817 0:296308a935f5 791 break;
stevew817 0:296308a935f5 792 }
stevew817 0:296308a935f5 793
stevew817 0:296308a935f5 794 return ICM20648_OK;
stevew817 0:296308a935f5 795 }
stevew817 0:296308a935f5 796
stevew817 0:296308a935f5 797 /***************************************************************************//**
stevew817 0:296308a935f5 798 * @brief
stevew817 0:296308a935f5 799 * Sets the full scale value of the accelerometer
stevew817 0:296308a935f5 800 *
stevew817 0:296308a935f5 801 * @param[in] accelFs
stevew817 0:296308a935f5 802 * The desired full scale value. Use the ICM20648_ACCEL_FULLSCALE_xG
stevew817 0:296308a935f5 803 * macros, which are defined in the icm20648.h file. The value of x can be
stevew817 0:296308a935f5 804 * 2, 4, 8 or 16.
stevew817 0:296308a935f5 805 *
stevew817 0:296308a935f5 806 * @return
stevew817 0:296308a935f5 807 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 808 ******************************************************************************/
stevew817 0:296308a935f5 809 uint32_t ICM20648::set_accel_fullscale(uint8_t accelFs)
stevew817 0:296308a935f5 810 {
stevew817 0:296308a935f5 811 uint8_t reg;
stevew817 0:296308a935f5 812
stevew817 0:296308a935f5 813 accelFs &= ICM20648_MASK_ACCEL_FULLSCALE;
stevew817 0:296308a935f5 814 read_register(ICM20648_REG_ACCEL_CONFIG, 1, &reg);
stevew817 0:296308a935f5 815 reg &= ~(ICM20648_MASK_ACCEL_FULLSCALE);
stevew817 0:296308a935f5 816 reg |= accelFs;
stevew817 0:296308a935f5 817 write_register(ICM20648_REG_ACCEL_CONFIG, reg);
stevew817 0:296308a935f5 818
stevew817 0:296308a935f5 819 return ICM20648_OK;
stevew817 0:296308a935f5 820 }
stevew817 0:296308a935f5 821
stevew817 0:296308a935f5 822 /***************************************************************************//**
stevew817 0:296308a935f5 823 * @brief
stevew817 0:296308a935f5 824 * Sets the full scale value of the gyroscope
stevew817 0:296308a935f5 825 *
stevew817 0:296308a935f5 826 * @param[in] gyroFs
stevew817 0:296308a935f5 827 * The desired full scale value. Use the ICM20648_GYRO_FULLSCALE_yDPS
stevew817 0:296308a935f5 828 * macros, which are defined in the icm20648.h file. The value of y can be
stevew817 0:296308a935f5 829 * 250, 500, 1000 or 2000.
stevew817 0:296308a935f5 830 *
stevew817 0:296308a935f5 831 * @return
stevew817 0:296308a935f5 832 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 833 ******************************************************************************/
stevew817 0:296308a935f5 834 uint32_t ICM20648::set_gyro_fullscale(uint8_t gyroFs)
stevew817 0:296308a935f5 835 {
stevew817 0:296308a935f5 836 uint8_t reg;
stevew817 0:296308a935f5 837
stevew817 0:296308a935f5 838 gyroFs &= ICM20648_MASK_GYRO_FULLSCALE;
stevew817 0:296308a935f5 839 read_register(ICM20648_REG_GYRO_CONFIG_1, 1, &reg);
stevew817 0:296308a935f5 840 reg &= ~(ICM20648_MASK_GYRO_FULLSCALE);
stevew817 0:296308a935f5 841 reg |= gyroFs;
stevew817 0:296308a935f5 842 write_register(ICM20648_REG_GYRO_CONFIG_1, reg);
stevew817 0:296308a935f5 843
stevew817 0:296308a935f5 844 return ICM20648_OK;
stevew817 0:296308a935f5 845 }
stevew817 0:296308a935f5 846
stevew817 0:296308a935f5 847 /***************************************************************************//**
stevew817 0:296308a935f5 848 * @brief
stevew817 0:296308a935f5 849 * Enables or disables the sleep mode of the device
stevew817 0:296308a935f5 850 *
stevew817 0:296308a935f5 851 * @param[in] enable
stevew817 0:296308a935f5 852 * If true, sleep mode is enabled. Set to false to disable sleep mode.
stevew817 0:296308a935f5 853 *
stevew817 0:296308a935f5 854 * @return
stevew817 0:296308a935f5 855 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 856 ******************************************************************************/
stevew817 0:296308a935f5 857 uint32_t ICM20648::enable_sleepmode(bool enable)
stevew817 0:296308a935f5 858 {
stevew817 0:296308a935f5 859 uint8_t reg;
stevew817 0:296308a935f5 860
stevew817 0:296308a935f5 861 read_register(ICM20648_REG_PWR_MGMT_1, 1, &reg);
stevew817 0:296308a935f5 862
stevew817 0:296308a935f5 863 if ( enable ) {
stevew817 0:296308a935f5 864 /* Sleep: set the SLEEP bit */
stevew817 0:296308a935f5 865 reg |= ICM20648_BIT_SLEEP;
stevew817 0:296308a935f5 866 } else {
stevew817 0:296308a935f5 867 /* Wake up: clear the SLEEP bit */
stevew817 0:296308a935f5 868 reg &= ~(ICM20648_BIT_SLEEP);
stevew817 0:296308a935f5 869 }
stevew817 0:296308a935f5 870
stevew817 0:296308a935f5 871 write_register(ICM20648_REG_PWR_MGMT_1, reg);
stevew817 0:296308a935f5 872
stevew817 0:296308a935f5 873 return ICM20648_OK;
stevew817 0:296308a935f5 874 }
stevew817 0:296308a935f5 875
stevew817 0:296308a935f5 876 /***************************************************************************//**
stevew817 0:296308a935f5 877 * @brief
stevew817 0:296308a935f5 878 * Enables or disables the cycle mode operation of the accel and gyro
stevew817 0:296308a935f5 879 *
stevew817 0:296308a935f5 880 * @param[in] enable
stevew817 0:296308a935f5 881 * If true both the accel and gyro sensors will operate in cycle mode. If
stevew817 0:296308a935f5 882 * false the senors working in continuous mode.
stevew817 0:296308a935f5 883 *
stevew817 0:296308a935f5 884 * @return
stevew817 0:296308a935f5 885 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 886 ******************************************************************************/
stevew817 0:296308a935f5 887 uint32_t ICM20648::enable_cyclemode(bool enable)
stevew817 0:296308a935f5 888 {
stevew817 0:296308a935f5 889 uint8_t reg;
stevew817 0:296308a935f5 890
stevew817 0:296308a935f5 891 reg = 0x00;
stevew817 0:296308a935f5 892
stevew817 0:296308a935f5 893 if ( enable ) {
stevew817 0:296308a935f5 894 reg = ICM20648_BIT_ACCEL_CYCLE | ICM20648_BIT_GYRO_CYCLE;
stevew817 0:296308a935f5 895 }
stevew817 0:296308a935f5 896
stevew817 0:296308a935f5 897 write_register(ICM20648_REG_LP_CONFIG, reg);
stevew817 0:296308a935f5 898
stevew817 0:296308a935f5 899 return ICM20648_OK;
stevew817 0:296308a935f5 900 }
stevew817 0:296308a935f5 901
stevew817 0:296308a935f5 902 /***************************************************************************//**
stevew817 0:296308a935f5 903 * @brief
stevew817 0:296308a935f5 904 * Enables or disables the sensors in the ICM20648 chip
stevew817 0:296308a935f5 905 *
stevew817 0:296308a935f5 906 * @param[in] accel
stevew817 0:296308a935f5 907 * If true enables the acceleration sensor
stevew817 0:296308a935f5 908 *
stevew817 0:296308a935f5 909 * @param[in] gyro
stevew817 0:296308a935f5 910 * If true enables the gyroscope sensor
stevew817 0:296308a935f5 911 *
stevew817 0:296308a935f5 912 * @param[in] temp
stevew817 0:296308a935f5 913 * If true enables the temperature sensor
stevew817 0:296308a935f5 914 *
stevew817 0:296308a935f5 915 * @return
stevew817 0:296308a935f5 916 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 917 ******************************************************************************/
stevew817 0:296308a935f5 918 uint32_t ICM20648::enable_sensor(bool accel, bool gyro, bool temp)
stevew817 0:296308a935f5 919 {
stevew817 0:296308a935f5 920 uint8_t pwrManagement1;
stevew817 0:296308a935f5 921 uint8_t pwrManagement2;
stevew817 0:296308a935f5 922
stevew817 0:296308a935f5 923 read_register(ICM20648_REG_PWR_MGMT_1, 1, &pwrManagement1);
stevew817 0:296308a935f5 924 pwrManagement2 = 0;
stevew817 0:296308a935f5 925
stevew817 0:296308a935f5 926 /* To enable the accelerometer clear the DISABLE_ACCEL bits in PWR_MGMT_2 */
stevew817 0:296308a935f5 927 if ( accel ) {
stevew817 0:296308a935f5 928 pwrManagement2 &= ~(ICM20648_BIT_PWR_ACCEL_STBY);
stevew817 0:296308a935f5 929 } else {
stevew817 0:296308a935f5 930 pwrManagement2 |= ICM20648_BIT_PWR_ACCEL_STBY;
stevew817 0:296308a935f5 931 }
stevew817 0:296308a935f5 932
stevew817 0:296308a935f5 933 /* To enable gyro clear the DISABLE_GYRO bits in PWR_MGMT_2 */
stevew817 0:296308a935f5 934 if ( gyro ) {
stevew817 0:296308a935f5 935 pwrManagement2 &= ~(ICM20648_BIT_PWR_GYRO_STBY);
stevew817 0:296308a935f5 936 } else {
stevew817 0:296308a935f5 937 pwrManagement2 |= ICM20648_BIT_PWR_GYRO_STBY;
stevew817 0:296308a935f5 938 }
stevew817 0:296308a935f5 939
stevew817 0:296308a935f5 940 /* To enable the temperature sensor clear the TEMP_DIS bit in PWR_MGMT_1 */
stevew817 0:296308a935f5 941 if ( temp ) {
stevew817 0:296308a935f5 942 pwrManagement1 &= ~(ICM20648_BIT_TEMP_DIS);
stevew817 0:296308a935f5 943 } else {
stevew817 0:296308a935f5 944 pwrManagement1 |= ICM20648_BIT_TEMP_DIS;
stevew817 0:296308a935f5 945 }
stevew817 0:296308a935f5 946
stevew817 0:296308a935f5 947 /* Write back the modified values */
stevew817 0:296308a935f5 948 write_register(ICM20648_REG_PWR_MGMT_1, pwrManagement1);
stevew817 0:296308a935f5 949 write_register(ICM20648_REG_PWR_MGMT_2, pwrManagement2);
stevew817 0:296308a935f5 950
stevew817 0:296308a935f5 951 return ICM20648_OK;
stevew817 0:296308a935f5 952 }
stevew817 0:296308a935f5 953
stevew817 0:296308a935f5 954 /***************************************************************************//**
stevew817 0:296308a935f5 955 * @brief
stevew817 0:296308a935f5 956 * Enables or disables the sensors in low power mode in the ICM20648 chip
stevew817 0:296308a935f5 957 *
stevew817 0:296308a935f5 958 * @param[in] enAccel
stevew817 0:296308a935f5 959 * If true enables the acceleration sensor in low power mode
stevew817 0:296308a935f5 960 *
stevew817 0:296308a935f5 961 * @param[in] enGyro
stevew817 0:296308a935f5 962 * If true enables the gyroscope sensor in low power mode
stevew817 0:296308a935f5 963 *
stevew817 0:296308a935f5 964 * @param[in] enTemp
stevew817 0:296308a935f5 965 * If true enables the temperature sensor in low power mode
stevew817 0:296308a935f5 966 *
stevew817 0:296308a935f5 967 * @return
stevew817 0:296308a935f5 968 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 969 ******************************************************************************/
stevew817 0:296308a935f5 970 uint32_t ICM20648::enter_lowpowermode(bool enAccel, bool enGyro, bool enTemp)
stevew817 0:296308a935f5 971 {
stevew817 0:296308a935f5 972 uint8_t data;
stevew817 0:296308a935f5 973
stevew817 0:296308a935f5 974 read_register(ICM20648_REG_PWR_MGMT_1, 1, &data);
stevew817 0:296308a935f5 975
stevew817 0:296308a935f5 976 if ( enAccel || enGyro || enTemp ) {
stevew817 0:296308a935f5 977 /* Make sure that the chip is not in sleep */
stevew817 0:296308a935f5 978 enable_sleepmode(false);
stevew817 0:296308a935f5 979
stevew817 0:296308a935f5 980 /* And in continuous mode */
stevew817 0:296308a935f5 981 enable_cyclemode(false);
stevew817 0:296308a935f5 982
stevew817 0:296308a935f5 983 /* Enable the accelerometer and the gyroscope*/
stevew817 0:296308a935f5 984 enable_sensor(enAccel, enGyro, enTemp);
stevew817 0:296308a935f5 985 wait_ms(50);
stevew817 0:296308a935f5 986
stevew817 0:296308a935f5 987 /* Enable cycle mode */
stevew817 0:296308a935f5 988 enable_cyclemode(true);
stevew817 0:296308a935f5 989
stevew817 0:296308a935f5 990 /* Set the LP_EN bit to enable low power mode */
stevew817 0:296308a935f5 991 data |= ICM20648_BIT_LP_EN;
stevew817 0:296308a935f5 992 } else {
stevew817 0:296308a935f5 993 /* Enable continuous mode */
stevew817 0:296308a935f5 994 enable_cyclemode(false);
stevew817 0:296308a935f5 995
stevew817 0:296308a935f5 996 /* Clear the LP_EN bit to disable low power mode */
stevew817 0:296308a935f5 997 data &= ~ICM20648_BIT_LP_EN;
stevew817 0:296308a935f5 998 }
stevew817 0:296308a935f5 999
stevew817 0:296308a935f5 1000 /* Write the updated value to the PWR_MGNT_1 register */
stevew817 0:296308a935f5 1001 write_register(ICM20648_REG_PWR_MGMT_1, data);
stevew817 0:296308a935f5 1002
stevew817 0:296308a935f5 1003 return ICM20648_OK;
stevew817 0:296308a935f5 1004 }
stevew817 0:296308a935f5 1005
stevew817 0:296308a935f5 1006 /***************************************************************************//**
stevew817 0:296308a935f5 1007 * @brief
stevew817 0:296308a935f5 1008 * Enables or disables the interrupts in the ICM20648 chip
stevew817 0:296308a935f5 1009 *
stevew817 0:296308a935f5 1010 * @param[in] dataReadyEnable
stevew817 0:296308a935f5 1011 * If true enables the Raw Data Ready interrupt, otherwise disables.
stevew817 0:296308a935f5 1012 *
stevew817 0:296308a935f5 1013 * @param[in] womEnable
stevew817 0:296308a935f5 1014 * If true enables the Wake-up On Motion interrupt, otherwise disables.
stevew817 0:296308a935f5 1015 *
stevew817 0:296308a935f5 1016 * @return
stevew817 0:296308a935f5 1017 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 1018 ******************************************************************************/
stevew817 0:296308a935f5 1019 uint32_t ICM20648::enable_irq(bool dataReadyEnable, bool womEnable)
stevew817 0:296308a935f5 1020 {
stevew817 0:296308a935f5 1021 uint8_t intEnable;
stevew817 0:296308a935f5 1022
stevew817 0:296308a935f5 1023 /* All interrupts disabled by default */
stevew817 0:296308a935f5 1024 intEnable = 0;
stevew817 0:296308a935f5 1025
stevew817 0:296308a935f5 1026 /* Enable one or both of the interrupt sources if required */
stevew817 0:296308a935f5 1027 if ( womEnable ) {
stevew817 0:296308a935f5 1028 intEnable = ICM20648_BIT_WOM_INT_EN;
stevew817 0:296308a935f5 1029 }
stevew817 0:296308a935f5 1030 /* Write value to register */
stevew817 0:296308a935f5 1031 write_register(ICM20648_REG_INT_ENABLE, intEnable);
stevew817 0:296308a935f5 1032
stevew817 0:296308a935f5 1033 /* All interrupts disabled by default */
stevew817 0:296308a935f5 1034 intEnable = 0;
stevew817 0:296308a935f5 1035
stevew817 0:296308a935f5 1036 if ( dataReadyEnable ) {
stevew817 0:296308a935f5 1037 intEnable = ICM20648_BIT_RAW_DATA_0_RDY_EN;
stevew817 0:296308a935f5 1038 }
stevew817 0:296308a935f5 1039
stevew817 0:296308a935f5 1040 /* Write value to register */
stevew817 0:296308a935f5 1041 write_register(ICM20648_REG_INT_ENABLE_1, intEnable);
stevew817 0:296308a935f5 1042
stevew817 0:296308a935f5 1043 return ICM20648_OK;
stevew817 0:296308a935f5 1044 }
stevew817 0:296308a935f5 1045
stevew817 0:296308a935f5 1046 /***************************************************************************//**
stevew817 0:296308a935f5 1047 * @brief
stevew817 0:296308a935f5 1048 * Reads the interrupt status registers of the ICM20648 chip
stevew817 0:296308a935f5 1049 *
stevew817 0:296308a935f5 1050 * @param[out] intStatus
stevew817 0:296308a935f5 1051 * The content the four interrupt registers. LSByte is INT_STATUS, MSByte is
stevew817 0:296308a935f5 1052 * INT_STATUS_3
stevew817 0:296308a935f5 1053 *
stevew817 0:296308a935f5 1054 * @return
stevew817 0:296308a935f5 1055 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 1056 ******************************************************************************/
stevew817 0:296308a935f5 1057 uint32_t ICM20648::read_irqstatus(uint32_t *int_status)
stevew817 0:296308a935f5 1058 {
stevew817 0:296308a935f5 1059 uint8_t reg[4];
stevew817 0:296308a935f5 1060
stevew817 0:296308a935f5 1061 read_register(ICM20648_REG_INT_STATUS, 4, reg);
stevew817 0:296308a935f5 1062 *int_status = (uint32_t) reg[0];
stevew817 0:296308a935f5 1063 *int_status |= ( ( (uint32_t) reg[1]) << 8);
stevew817 0:296308a935f5 1064 *int_status |= ( ( (uint32_t) reg[2]) << 16);
stevew817 0:296308a935f5 1065 *int_status |= ( ( (uint32_t) reg[3]) << 24);
stevew817 0:296308a935f5 1066
stevew817 0:296308a935f5 1067 return ICM20648_OK;
stevew817 0:296308a935f5 1068 }
stevew817 0:296308a935f5 1069
stevew817 0:296308a935f5 1070 /***************************************************************************//**
stevew817 0:296308a935f5 1071 * @brief
stevew817 0:296308a935f5 1072 * Checks if new data is available for read
stevew817 0:296308a935f5 1073 *
stevew817 0:296308a935f5 1074 * @return
stevew817 0:296308a935f5 1075 * Returns true if the Raw Data Ready interrupt bit set, false otherwise
stevew817 0:296308a935f5 1076 ******************************************************************************/
stevew817 0:296308a935f5 1077 bool ICM20648::is_data_ready(void)
stevew817 0:296308a935f5 1078 {
stevew817 0:296308a935f5 1079 uint8_t status;
stevew817 0:296308a935f5 1080 bool ret;
stevew817 0:296308a935f5 1081
stevew817 0:296308a935f5 1082 ret = false;
stevew817 0:296308a935f5 1083 read_register(ICM20648_REG_INT_STATUS_1, 1, &status);
stevew817 0:296308a935f5 1084
stevew817 0:296308a935f5 1085 if ( status & ICM20648_BIT_RAW_DATA_0_RDY_INT ) {
stevew817 0:296308a935f5 1086 ret = true;
stevew817 0:296308a935f5 1087 }
stevew817 0:296308a935f5 1088
stevew817 0:296308a935f5 1089 return ret;
stevew817 0:296308a935f5 1090 }
stevew817 0:296308a935f5 1091
stevew817 0:296308a935f5 1092 /***************************************************************************//**
stevew817 0:296308a935f5 1093 * @brief
stevew817 0:296308a935f5 1094 * Sets up and enables the Wake-up On Motion feature
stevew817 0:296308a935f5 1095 *
stevew817 0:296308a935f5 1096 * @param[in] enable
stevew817 0:296308a935f5 1097 * If true enables the WOM feature, disables otherwise
stevew817 0:296308a935f5 1098 *
stevew817 0:296308a935f5 1099 * @param[in] womThreshold
stevew817 0:296308a935f5 1100 * Threshold value for the Wake on Motion Interrupt for ACCEL x/y/z axes.
stevew817 0:296308a935f5 1101 * LSB = 4mg. Range is 0mg to 1020mg
stevew817 0:296308a935f5 1102 *
stevew817 0:296308a935f5 1103 * @param[in] sampleRate
stevew817 0:296308a935f5 1104 * The desired sample rate of the accel sensor in Hz
stevew817 0:296308a935f5 1105 *
stevew817 0:296308a935f5 1106 * @return
stevew817 0:296308a935f5 1107 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 1108 ******************************************************************************/
stevew817 0:296308a935f5 1109 uint32_t ICM20648::enable_wake_on_motion(bool enable, uint8_t womThreshold, float sampleRate)
stevew817 0:296308a935f5 1110 {
stevew817 0:296308a935f5 1111 if ( enable ) {
stevew817 0:296308a935f5 1112 /* Make sure that the chip is not in sleep */
stevew817 0:296308a935f5 1113 enable_sleepmode(false);
stevew817 0:296308a935f5 1114
stevew817 0:296308a935f5 1115 /* And in continuous mode */
stevew817 0:296308a935f5 1116 enable_cyclemode(false);
stevew817 0:296308a935f5 1117
stevew817 0:296308a935f5 1118 /* Enable only the accelerometer */
stevew817 0:296308a935f5 1119 enable_sensor(true, false, false);
stevew817 0:296308a935f5 1120
stevew817 0:296308a935f5 1121 /* Set sample rate */
stevew817 0:296308a935f5 1122 set_sample_rate(sampleRate);
stevew817 0:296308a935f5 1123
stevew817 0:296308a935f5 1124 /* Set the bandwidth to 1210Hz */
stevew817 0:296308a935f5 1125 set_accel_bandwidth(ICM20648_ACCEL_BW_1210HZ);
stevew817 0:296308a935f5 1126
stevew817 0:296308a935f5 1127 /* Accel: 2G full scale */
stevew817 0:296308a935f5 1128 set_accel_fullscale(ICM20648_ACCEL_FULLSCALE_2G);
stevew817 0:296308a935f5 1129
stevew817 0:296308a935f5 1130 /* Enable the Wake On Motion interrupt */
stevew817 0:296308a935f5 1131 enable_irq(false, true);
stevew817 0:296308a935f5 1132 wait_ms(50);
stevew817 0:296308a935f5 1133
stevew817 0:296308a935f5 1134 /* Enable Wake On Motion feature */
stevew817 0:296308a935f5 1135 write_register(ICM20648_REG_ACCEL_INTEL_CTRL, ICM20648_BIT_ACCEL_INTEL_EN | ICM20648_BIT_ACCEL_INTEL_MODE);
stevew817 0:296308a935f5 1136
stevew817 0:296308a935f5 1137 /* Set the wake on motion threshold value */
stevew817 0:296308a935f5 1138 write_register(ICM20648_REG_ACCEL_WOM_THR, womThreshold);
stevew817 0:296308a935f5 1139
stevew817 0:296308a935f5 1140 /* Enable low power mode */
stevew817 0:296308a935f5 1141 enter_lowpowermode(true, false, false);
stevew817 0:296308a935f5 1142 } else {
stevew817 0:296308a935f5 1143 /* Disable Wake On Motion feature */
stevew817 0:296308a935f5 1144 write_register(ICM20648_REG_ACCEL_INTEL_CTRL, 0x00);
stevew817 0:296308a935f5 1145
stevew817 0:296308a935f5 1146 /* Disable the Wake On Motion interrupt */
stevew817 0:296308a935f5 1147 enable_irq(false, false);
stevew817 0:296308a935f5 1148
stevew817 0:296308a935f5 1149 /* Disable cycle mode */
stevew817 0:296308a935f5 1150 enable_cyclemode(false);
stevew817 0:296308a935f5 1151 }
stevew817 0:296308a935f5 1152
stevew817 0:296308a935f5 1153 return ICM20648_OK;
stevew817 0:296308a935f5 1154 }
stevew817 0:296308a935f5 1155
stevew817 0:296308a935f5 1156 /***************************************************************************//**
stevew817 0:296308a935f5 1157 * @brief
stevew817 0:296308a935f5 1158 * Accelerometer and gyroscope calibration function. Reads the gyroscope
stevew817 0:296308a935f5 1159 * and accelerometer values while the device is at rest and in level. The
stevew817 0:296308a935f5 1160 * resulting values are loaded to the accel and gyro bias registers to cancel
stevew817 0:296308a935f5 1161 * the static offset error.
stevew817 0:296308a935f5 1162 *
stevew817 0:296308a935f5 1163 * @param[out] accelBiasScaled
stevew817 0:296308a935f5 1164 * The mesured acceleration sensor bias in mg
stevew817 0:296308a935f5 1165 *
stevew817 0:296308a935f5 1166 * @param[out] gyroBiasScaled
stevew817 0:296308a935f5 1167 * The mesured gyro sensor bias in deg/sec
stevew817 0:296308a935f5 1168 *
stevew817 0:296308a935f5 1169 * @return
stevew817 0:296308a935f5 1170 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 1171 ******************************************************************************/
stevew817 0:296308a935f5 1172 uint32_t ICM20648::calibrate(float *accelBiasScaled, float *gyroBiasScaled)
stevew817 0:296308a935f5 1173 {
stevew817 0:296308a935f5 1174 uint8_t data[12];
stevew817 0:296308a935f5 1175 uint16_t i, packetCount, fifoCount;
stevew817 0:296308a935f5 1176 int32_t gyroBias[3] = { 0, 0, 0 };
stevew817 0:296308a935f5 1177 int32_t accelBias[3] = { 0, 0, 0 };
stevew817 0:296308a935f5 1178 int32_t accelTemp[3];
stevew817 0:296308a935f5 1179 int32_t gyroTemp[3];
stevew817 0:296308a935f5 1180 int32_t accelBiasFactory[3];
stevew817 0:296308a935f5 1181 int32_t gyroBiasStored[3];
stevew817 0:296308a935f5 1182 float gyroRes, accelRes;
stevew817 0:296308a935f5 1183
stevew817 0:296308a935f5 1184 /* Enable the accelerometer and the gyro */
stevew817 0:296308a935f5 1185 enable_sensor(true, true, false);
stevew817 0:296308a935f5 1186
stevew817 0:296308a935f5 1187 /* Set 1kHz sample rate */
stevew817 0:296308a935f5 1188 set_sample_rate(1100.0);
stevew817 0:296308a935f5 1189
stevew817 0:296308a935f5 1190 /* 246Hz BW for the accelerometer and 200Hz for the gyroscope */
stevew817 0:296308a935f5 1191 set_accel_bandwidth(ICM20648_ACCEL_BW_246HZ);
stevew817 0:296308a935f5 1192 set_gyro_bandwidth(ICM20648_GYRO_BW_12HZ);
stevew817 0:296308a935f5 1193
stevew817 0:296308a935f5 1194 /* Set the most sensitive range: 2G full scale and 250dps full scale */
stevew817 0:296308a935f5 1195 set_accel_fullscale(ICM20648_ACCEL_FULLSCALE_2G);
stevew817 0:296308a935f5 1196 set_gyro_fullscale(ICM20648_GYRO_FULLSCALE_250DPS);
stevew817 0:296308a935f5 1197
stevew817 0:296308a935f5 1198 /* Retrieve the resolution per bit */
stevew817 0:296308a935f5 1199 get_accel_resolution(&accelRes);
stevew817 0:296308a935f5 1200 get_gyro_resolution(&gyroRes);
stevew817 0:296308a935f5 1201
stevew817 0:296308a935f5 1202 /* The accel sensor needs max 30ms, the gyro max 35ms to fully start */
stevew817 0:296308a935f5 1203 /* Experiments show that the gyro needs more time to get reliable results */
stevew817 0:296308a935f5 1204 wait_ms(50);
stevew817 0:296308a935f5 1205
stevew817 0:296308a935f5 1206 /* Disable the FIFO */
stevew817 0:296308a935f5 1207 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN);
stevew817 0:296308a935f5 1208 write_register(ICM20648_REG_FIFO_MODE, 0x0F);
stevew817 0:296308a935f5 1209
stevew817 0:296308a935f5 1210 /* Enable accelerometer and gyro to store the data in FIFO */
stevew817 0:296308a935f5 1211 write_register(ICM20648_REG_FIFO_EN_2, ICM20648_BIT_ACCEL_FIFO_EN | ICM20648_BITS_GYRO_FIFO_EN);
stevew817 0:296308a935f5 1212
stevew817 0:296308a935f5 1213 /* Reset the FIFO */
stevew817 0:296308a935f5 1214 write_register(ICM20648_REG_FIFO_RST, 0x0F);
stevew817 0:296308a935f5 1215 write_register(ICM20648_REG_FIFO_RST, 0x00);
stevew817 0:296308a935f5 1216
stevew817 0:296308a935f5 1217 /* Enable the FIFO */
stevew817 0:296308a935f5 1218 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN);
stevew817 0:296308a935f5 1219
stevew817 0:296308a935f5 1220 /* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */
stevew817 0:296308a935f5 1221 /* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */
stevew817 0:296308a935f5 1222 /* Loop until at least 4080 samples gathered */
stevew817 0:296308a935f5 1223 fifoCount = 0;
stevew817 0:296308a935f5 1224 while ( fifoCount < 4080 ) {
stevew817 0:296308a935f5 1225 wait_ms(5);
stevew817 0:296308a935f5 1226 /* Read FIFO sample count */
stevew817 0:296308a935f5 1227 read_register(ICM20648_REG_FIFO_COUNT_H, 2, &data[0]);
stevew817 0:296308a935f5 1228 /* Convert to a 16 bit value */
stevew817 0:296308a935f5 1229 fifoCount = ( (uint16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1230 }
stevew817 0:296308a935f5 1231
stevew817 0:296308a935f5 1232 /* Disable accelerometer and gyro to store the data in FIFO */
stevew817 0:296308a935f5 1233 write_register(ICM20648_REG_FIFO_EN_2, 0x00);
stevew817 0:296308a935f5 1234
stevew817 0:296308a935f5 1235 /* Read FIFO sample count */
stevew817 0:296308a935f5 1236 read_register(ICM20648_REG_FIFO_COUNT_H, 2, &data[0]);
stevew817 0:296308a935f5 1237
stevew817 0:296308a935f5 1238 /* Convert to a 16 bit value */
stevew817 0:296308a935f5 1239 fifoCount = ( (uint16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1240
stevew817 0:296308a935f5 1241 /* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */
stevew817 0:296308a935f5 1242 packetCount = fifoCount / 12;
stevew817 0:296308a935f5 1243
stevew817 0:296308a935f5 1244 /* Retrieve the data from the FIFO */
stevew817 0:296308a935f5 1245 for ( i = 0; i < packetCount; i++ ) {
stevew817 0:296308a935f5 1246 read_register(ICM20648_REG_FIFO_R_W, 12, &data[0]);
stevew817 0:296308a935f5 1247 /* Convert to 16 bit signed accel and gyro x,y and z values */
stevew817 0:296308a935f5 1248 accelTemp[0] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1249 accelTemp[1] = ( (int16_t) (data[2] << 8) | data[3]);
stevew817 0:296308a935f5 1250 accelTemp[2] = ( (int16_t) (data[4] << 8) | data[5]);
stevew817 0:296308a935f5 1251 gyroTemp[0] = ( (int16_t) (data[6] << 8) | data[7]);
stevew817 0:296308a935f5 1252 gyroTemp[1] = ( (int16_t) (data[8] << 8) | data[9]);
stevew817 0:296308a935f5 1253 gyroTemp[2] = ( (int16_t) (data[10] << 8) | data[11]);
stevew817 0:296308a935f5 1254
stevew817 0:296308a935f5 1255 /* Sum the values */
stevew817 0:296308a935f5 1256 accelBias[0] += accelTemp[0];
stevew817 0:296308a935f5 1257 accelBias[1] += accelTemp[1];
stevew817 0:296308a935f5 1258 accelBias[2] += accelTemp[2];
stevew817 0:296308a935f5 1259 gyroBias[0] += gyroTemp[0];
stevew817 0:296308a935f5 1260 gyroBias[1] += gyroTemp[1];
stevew817 0:296308a935f5 1261 gyroBias[2] += gyroTemp[2];
stevew817 0:296308a935f5 1262 }
stevew817 0:296308a935f5 1263
stevew817 0:296308a935f5 1264 /* Divide by packet count to get the average */
stevew817 0:296308a935f5 1265 accelBias[0] /= packetCount;
stevew817 0:296308a935f5 1266 accelBias[1] /= packetCount;
stevew817 0:296308a935f5 1267 accelBias[2] /= packetCount;
stevew817 0:296308a935f5 1268 gyroBias[0] /= packetCount;
stevew817 0:296308a935f5 1269 gyroBias[1] /= packetCount;
stevew817 0:296308a935f5 1270 gyroBias[2] /= packetCount;
stevew817 0:296308a935f5 1271
stevew817 0:296308a935f5 1272 /* Acceleormeter: add or remove (depending on the orientation of the chip) 1G (gravity) from the Z axis value */
stevew817 0:296308a935f5 1273 if ( accelBias[2] > 0L ) {
stevew817 0:296308a935f5 1274 accelBias[2] -= (int32_t) (1.0 / accelRes);
stevew817 0:296308a935f5 1275 } else {
stevew817 0:296308a935f5 1276 accelBias[2] += (int32_t) (1.0 / accelRes);
stevew817 0:296308a935f5 1277 }
stevew817 0:296308a935f5 1278
stevew817 0:296308a935f5 1279 /* Convert the values to degrees per sec for displaying */
stevew817 0:296308a935f5 1280 gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes;
stevew817 0:296308a935f5 1281 gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes;
stevew817 0:296308a935f5 1282 gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes;
stevew817 0:296308a935f5 1283
stevew817 0:296308a935f5 1284 /* Read stored gyro trim values. After reset these values are all 0 */
stevew817 0:296308a935f5 1285 read_register(ICM20648_REG_XG_OFFS_USRH, 2, &data[0]);
stevew817 0:296308a935f5 1286 gyroBiasStored[0] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1287 read_register(ICM20648_REG_YG_OFFS_USRH, 2, &data[0]);
stevew817 0:296308a935f5 1288 gyroBiasStored[1] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1289 read_register(ICM20648_REG_ZG_OFFS_USRH, 2, &data[0]);
stevew817 0:296308a935f5 1290 gyroBiasStored[2] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1291
stevew817 0:296308a935f5 1292 /* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */
stevew817 0:296308a935f5 1293 /* the best sensitivity, so need to divide by 4 */
stevew817 0:296308a935f5 1294 /* Substract from the stored calibration value */
stevew817 0:296308a935f5 1295 gyroBiasStored[0] -= gyroBias[0] / 4;
stevew817 0:296308a935f5 1296 gyroBiasStored[1] -= gyroBias[1] / 4;
stevew817 0:296308a935f5 1297 gyroBiasStored[2] -= gyroBias[2] / 4;
stevew817 0:296308a935f5 1298
stevew817 0:296308a935f5 1299 /* Split the values into two bytes */
stevew817 0:296308a935f5 1300 data[0] = (gyroBiasStored[0] >> 8) & 0xFF;
stevew817 0:296308a935f5 1301 data[1] = (gyroBiasStored[0]) & 0xFF;
stevew817 0:296308a935f5 1302 data[2] = (gyroBiasStored[1] >> 8) & 0xFF;
stevew817 0:296308a935f5 1303 data[3] = (gyroBiasStored[1]) & 0xFF;
stevew817 0:296308a935f5 1304 data[4] = (gyroBiasStored[2] >> 8) & 0xFF;
stevew817 0:296308a935f5 1305 data[5] = (gyroBiasStored[2]) & 0xFF;
stevew817 0:296308a935f5 1306
stevew817 0:296308a935f5 1307 /* Write the gyro bias values to the chip */
stevew817 0:296308a935f5 1308 write_register(ICM20648_REG_XG_OFFS_USRH, data[0]);
stevew817 0:296308a935f5 1309 write_register(ICM20648_REG_XG_OFFS_USRL, data[1]);
stevew817 0:296308a935f5 1310 write_register(ICM20648_REG_YG_OFFS_USRH, data[2]);
stevew817 0:296308a935f5 1311 write_register(ICM20648_REG_YG_OFFS_USRL, data[3]);
stevew817 0:296308a935f5 1312 write_register(ICM20648_REG_ZG_OFFS_USRH, data[4]);
stevew817 0:296308a935f5 1313 write_register(ICM20648_REG_ZG_OFFS_USRL, data[5]);
stevew817 0:296308a935f5 1314
stevew817 0:296308a935f5 1315 /* Calculate the accelerometer bias values to store in the hardware accelerometer bias registers. These registers contain */
stevew817 0:296308a935f5 1316 /* factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold */
stevew817 0:296308a935f5 1317 /* non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature */
stevew817 0:296308a935f5 1318 /* compensation calculations(? the datasheet is not clear). Accelerometer bias registers expect bias input */
stevew817 0:296308a935f5 1319 /* as 2048 LSB per g, so that the accelerometer biases calculated above must be divided by 8. */
stevew817 0:296308a935f5 1320
stevew817 0:296308a935f5 1321 /* Read factory accelerometer trim values */
stevew817 0:296308a935f5 1322 read_register(ICM20648_REG_XA_OFFSET_H, 2, &data[0]);
stevew817 0:296308a935f5 1323 accelBiasFactory[0] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1324 read_register(ICM20648_REG_YA_OFFSET_H, 2, &data[0]);
stevew817 0:296308a935f5 1325 accelBiasFactory[1] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1326 read_register(ICM20648_REG_ZA_OFFSET_H, 2, &data[0]);
stevew817 0:296308a935f5 1327 accelBiasFactory[2] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1328
stevew817 0:296308a935f5 1329 /* Construct total accelerometer bias, including calculated average accelerometer bias from above */
stevew817 0:296308a935f5 1330 /* Scale the 2g full scale (most sensitive range) results to 16g full scale - divide by 8 */
stevew817 0:296308a935f5 1331 /* Clear the last bit (temperature compensation? - the datasheet is not clear) */
stevew817 0:296308a935f5 1332 /* Substract from the factory calibration value */
stevew817 0:296308a935f5 1333
stevew817 0:296308a935f5 1334 accelBiasFactory[0] -= ( (accelBias[0] / 8) & ~1);
stevew817 0:296308a935f5 1335 accelBiasFactory[1] -= ( (accelBias[1] / 8) & ~1);
stevew817 0:296308a935f5 1336 accelBiasFactory[2] -= ( (accelBias[2] / 8) & ~1);
stevew817 0:296308a935f5 1337
stevew817 0:296308a935f5 1338 /* Split the values into two bytes */
stevew817 0:296308a935f5 1339 data[0] = (accelBiasFactory[0] >> 8) & 0xFF;
stevew817 0:296308a935f5 1340 data[1] = (accelBiasFactory[0]) & 0xFF;
stevew817 0:296308a935f5 1341 data[2] = (accelBiasFactory[1] >> 8) & 0xFF;
stevew817 0:296308a935f5 1342 data[3] = (accelBiasFactory[1]) & 0xFF;
stevew817 0:296308a935f5 1343 data[4] = (accelBiasFactory[2] >> 8) & 0xFF;
stevew817 0:296308a935f5 1344 data[5] = (accelBiasFactory[2]) & 0xFF;
stevew817 0:296308a935f5 1345
stevew817 0:296308a935f5 1346 /* Store them in the accelerometer offset registers */
stevew817 0:296308a935f5 1347 write_register(ICM20648_REG_XA_OFFSET_H, data[0]);
stevew817 0:296308a935f5 1348 write_register(ICM20648_REG_XA_OFFSET_L, data[1]);
stevew817 0:296308a935f5 1349 write_register(ICM20648_REG_YA_OFFSET_H, data[2]);
stevew817 0:296308a935f5 1350 write_register(ICM20648_REG_YA_OFFSET_L, data[3]);
stevew817 0:296308a935f5 1351 write_register(ICM20648_REG_ZA_OFFSET_H, data[4]);
stevew817 0:296308a935f5 1352 write_register(ICM20648_REG_ZA_OFFSET_L, data[5]);
stevew817 0:296308a935f5 1353
stevew817 0:296308a935f5 1354 /* Convert the values to G for displaying */
stevew817 0:296308a935f5 1355 accelBiasScaled[0] = (float) accelBias[0] * accelRes;
stevew817 0:296308a935f5 1356 accelBiasScaled[1] = (float) accelBias[1] * accelRes;
stevew817 0:296308a935f5 1357 accelBiasScaled[2] = (float) accelBias[2] * accelRes;
stevew817 0:296308a935f5 1358
stevew817 0:296308a935f5 1359 /* Turn off FIFO */
stevew817 0:296308a935f5 1360 write_register(ICM20648_REG_USER_CTRL, 0x00);
stevew817 0:296308a935f5 1361
stevew817 0:296308a935f5 1362 /* Disable all sensors */
stevew817 0:296308a935f5 1363 enable_sensor(false, false, false);
stevew817 0:296308a935f5 1364
stevew817 0:296308a935f5 1365 return ICM20648_OK;
stevew817 0:296308a935f5 1366 }
stevew817 0:296308a935f5 1367
stevew817 0:296308a935f5 1368 /***************************************************************************//**
stevew817 0:296308a935f5 1369 * @brief
stevew817 0:296308a935f5 1370 * Gyroscope calibration function. Reads the gyroscope
stevew817 0:296308a935f5 1371 * values while the device is at rest and in level. The
stevew817 0:296308a935f5 1372 * resulting values are loaded to the gyro bias registers to cancel
stevew817 0:296308a935f5 1373 * the static offset error.
stevew817 0:296308a935f5 1374 *
stevew817 0:296308a935f5 1375 * @param[out] gyroBiasScaled
stevew817 0:296308a935f5 1376 * The mesured gyro sensor bias in deg/sec
stevew817 0:296308a935f5 1377 *
stevew817 0:296308a935f5 1378 * @return
stevew817 0:296308a935f5 1379 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 1380 ******************************************************************************/
stevew817 0:296308a935f5 1381 uint32_t ICM20648::calibrate_gyro(float *gyroBiasScaled)
stevew817 0:296308a935f5 1382 {
stevew817 0:296308a935f5 1383 uint8_t data[12];
stevew817 0:296308a935f5 1384 uint16_t i, packetCount, fifoCount;
stevew817 0:296308a935f5 1385 int32_t gyroBias[3] = { 0, 0, 0 };
stevew817 0:296308a935f5 1386 int32_t gyroTemp[3];
stevew817 0:296308a935f5 1387 int32_t gyroBiasStored[3];
stevew817 0:296308a935f5 1388 float gyroRes;
stevew817 0:296308a935f5 1389
stevew817 0:296308a935f5 1390 /* Enable the accelerometer and the gyro */
stevew817 0:296308a935f5 1391 enable_sensor(true, true, false);
stevew817 0:296308a935f5 1392
stevew817 0:296308a935f5 1393 /* Set 1kHz sample rate */
stevew817 0:296308a935f5 1394 set_sample_rate(1100.0);
stevew817 0:296308a935f5 1395
stevew817 0:296308a935f5 1396 /* Configure bandwidth for gyroscope to 12Hz */
stevew817 0:296308a935f5 1397 set_gyro_bandwidth(ICM20648_GYRO_BW_12HZ);
stevew817 0:296308a935f5 1398
stevew817 0:296308a935f5 1399 /* Configure sensitivity to 250dps full scale */
stevew817 0:296308a935f5 1400 set_gyro_fullscale(ICM20648_GYRO_FULLSCALE_250DPS);
stevew817 0:296308a935f5 1401
stevew817 0:296308a935f5 1402 /* Retrieve the resolution per bit */
stevew817 0:296308a935f5 1403 get_gyro_resolution(&gyroRes);
stevew817 0:296308a935f5 1404
stevew817 0:296308a935f5 1405 /* The accel sensor needs max 30ms, the gyro max 35ms to fully start */
stevew817 0:296308a935f5 1406 /* Experiments show that the gyro needs more time to get reliable results */
stevew817 0:296308a935f5 1407 wait_ms(50);
stevew817 0:296308a935f5 1408
stevew817 0:296308a935f5 1409 /* Disable the FIFO */
stevew817 0:296308a935f5 1410 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN);
stevew817 0:296308a935f5 1411 write_register(ICM20648_REG_FIFO_MODE, 0x0F);
stevew817 0:296308a935f5 1412
stevew817 0:296308a935f5 1413 /* Enable accelerometer and gyro to store the data in FIFO */
stevew817 0:296308a935f5 1414 write_register(ICM20648_REG_FIFO_EN_2, ICM20648_BITS_GYRO_FIFO_EN);
stevew817 0:296308a935f5 1415
stevew817 0:296308a935f5 1416 /* Reset the FIFO */
stevew817 0:296308a935f5 1417 write_register(ICM20648_REG_FIFO_RST, 0x0F);
stevew817 0:296308a935f5 1418 write_register(ICM20648_REG_FIFO_RST, 0x00);
stevew817 0:296308a935f5 1419
stevew817 0:296308a935f5 1420 /* Enable the FIFO */
stevew817 0:296308a935f5 1421 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN);
stevew817 0:296308a935f5 1422
stevew817 0:296308a935f5 1423 /* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */
stevew817 0:296308a935f5 1424 /* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */
stevew817 0:296308a935f5 1425 /* Loop until at least 4080 samples gathered */
stevew817 0:296308a935f5 1426 fifoCount = 0;
stevew817 0:296308a935f5 1427 while ( fifoCount < 4080 ) {
stevew817 0:296308a935f5 1428 wait_ms(5);
stevew817 0:296308a935f5 1429
stevew817 0:296308a935f5 1430 /* Read FIFO sample count */
stevew817 0:296308a935f5 1431 read_register(ICM20648_REG_FIFO_COUNT_H, 2, &data[0]);
stevew817 0:296308a935f5 1432
stevew817 0:296308a935f5 1433 /* Convert to a 16 bit value */
stevew817 0:296308a935f5 1434 fifoCount = ( (uint16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1435 }
stevew817 0:296308a935f5 1436
stevew817 0:296308a935f5 1437 /* Disable accelerometer and gyro to store the data in FIFO */
stevew817 0:296308a935f5 1438 write_register(ICM20648_REG_FIFO_EN_2, 0x00);
stevew817 0:296308a935f5 1439
stevew817 0:296308a935f5 1440 /* Read FIFO sample count */
stevew817 0:296308a935f5 1441 read_register(ICM20648_REG_FIFO_COUNT_H, 2, &data[0]);
stevew817 0:296308a935f5 1442
stevew817 0:296308a935f5 1443 /* Convert to a 16 bit value */
stevew817 0:296308a935f5 1444 fifoCount = ( (uint16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1445
stevew817 0:296308a935f5 1446 /* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */
stevew817 0:296308a935f5 1447 packetCount = fifoCount / 12;
stevew817 0:296308a935f5 1448
stevew817 0:296308a935f5 1449 /* Retrieve the data from the FIFO */
stevew817 0:296308a935f5 1450 for ( i = 0; i < packetCount; i++ ) {
stevew817 0:296308a935f5 1451 read_register(ICM20648_REG_FIFO_R_W, 12, &data[0]);
stevew817 0:296308a935f5 1452 /* Convert to 16 bit signed accel and gyro x,y and z values */
stevew817 0:296308a935f5 1453 gyroTemp[0] = ( (int16_t) (data[6] << 8) | data[7]);
stevew817 0:296308a935f5 1454 gyroTemp[1] = ( (int16_t) (data[8] << 8) | data[9]);
stevew817 0:296308a935f5 1455 gyroTemp[2] = ( (int16_t) (data[10] << 8) | data[11]);
stevew817 0:296308a935f5 1456
stevew817 0:296308a935f5 1457 /* Sum the values */
stevew817 0:296308a935f5 1458 gyroBias[0] += gyroTemp[0];
stevew817 0:296308a935f5 1459 gyroBias[1] += gyroTemp[1];
stevew817 0:296308a935f5 1460 gyroBias[2] += gyroTemp[2];
stevew817 0:296308a935f5 1461 }
stevew817 0:296308a935f5 1462
stevew817 0:296308a935f5 1463 /* Divide by packet count to get the average */
stevew817 0:296308a935f5 1464 gyroBias[0] /= packetCount;
stevew817 0:296308a935f5 1465 gyroBias[1] /= packetCount;
stevew817 0:296308a935f5 1466 gyroBias[2] /= packetCount;
stevew817 0:296308a935f5 1467
stevew817 0:296308a935f5 1468 /* Convert the values to degrees per sec for displaying */
stevew817 0:296308a935f5 1469 gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes;
stevew817 0:296308a935f5 1470 gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes;
stevew817 0:296308a935f5 1471 gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes;
stevew817 0:296308a935f5 1472
stevew817 0:296308a935f5 1473 /* Read stored gyro trim values. After reset these values are all 0 */
stevew817 0:296308a935f5 1474 read_register(ICM20648_REG_XG_OFFS_USRH, 2, &data[0]);
stevew817 0:296308a935f5 1475 gyroBiasStored[0] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1476
stevew817 0:296308a935f5 1477 read_register(ICM20648_REG_YG_OFFS_USRH, 2, &data[0]);
stevew817 0:296308a935f5 1478 gyroBiasStored[1] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1479
stevew817 0:296308a935f5 1480 read_register(ICM20648_REG_ZG_OFFS_USRH, 2, &data[0]);
stevew817 0:296308a935f5 1481 gyroBiasStored[2] = ( (int16_t) (data[0] << 8) | data[1]);
stevew817 0:296308a935f5 1482
stevew817 0:296308a935f5 1483 /* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */
stevew817 0:296308a935f5 1484 /* the best sensitivity, so need to divide by 4 */
stevew817 0:296308a935f5 1485 /* Substract from the stored calibration value */
stevew817 0:296308a935f5 1486 gyroBiasStored[0] -= gyroBias[0] / 4;
stevew817 0:296308a935f5 1487 gyroBiasStored[1] -= gyroBias[1] / 4;
stevew817 0:296308a935f5 1488 gyroBiasStored[2] -= gyroBias[2] / 4;
stevew817 0:296308a935f5 1489
stevew817 0:296308a935f5 1490 /* Split the values into two bytes */
stevew817 0:296308a935f5 1491 data[0] = (gyroBiasStored[0] >> 8) & 0xFF;
stevew817 0:296308a935f5 1492 data[1] = (gyroBiasStored[0]) & 0xFF;
stevew817 0:296308a935f5 1493 data[2] = (gyroBiasStored[1] >> 8) & 0xFF;
stevew817 0:296308a935f5 1494 data[3] = (gyroBiasStored[1]) & 0xFF;
stevew817 0:296308a935f5 1495 data[4] = (gyroBiasStored[2] >> 8) & 0xFF;
stevew817 0:296308a935f5 1496 data[5] = (gyroBiasStored[2]) & 0xFF;
stevew817 0:296308a935f5 1497
stevew817 0:296308a935f5 1498 /* Write the gyro bias values to the chip */
stevew817 0:296308a935f5 1499 write_register(ICM20648_REG_XG_OFFS_USRH, data[0]);
stevew817 0:296308a935f5 1500 write_register(ICM20648_REG_XG_OFFS_USRL, data[1]);
stevew817 0:296308a935f5 1501 write_register(ICM20648_REG_YG_OFFS_USRH, data[2]);
stevew817 0:296308a935f5 1502 write_register(ICM20648_REG_YG_OFFS_USRL, data[3]);
stevew817 0:296308a935f5 1503 write_register(ICM20648_REG_ZG_OFFS_USRH, data[4]);
stevew817 0:296308a935f5 1504 write_register(ICM20648_REG_ZG_OFFS_USRL, data[5]);
stevew817 0:296308a935f5 1505
stevew817 0:296308a935f5 1506 /* Turn off FIFO */
stevew817 0:296308a935f5 1507 write_register(ICM20648_REG_USER_CTRL, 0x00);
stevew817 0:296308a935f5 1508
stevew817 0:296308a935f5 1509 /* Disable all sensors */
stevew817 0:296308a935f5 1510 enable_sensor(false, false, false);
stevew817 0:296308a935f5 1511
stevew817 0:296308a935f5 1512 return ICM20648_OK;
stevew817 0:296308a935f5 1513 }
stevew817 0:296308a935f5 1514
stevew817 0:296308a935f5 1515 /***************************************************************************//**
stevew817 0:296308a935f5 1516 * @brief
stevew817 0:296308a935f5 1517 * Reads the temperature sensor raw value and converts to Celsius.
stevew817 0:296308a935f5 1518 *
stevew817 0:296308a935f5 1519 * @param[out] temperature
stevew817 0:296308a935f5 1520 * The mesured temperature in Celsius
stevew817 0:296308a935f5 1521 *
stevew817 0:296308a935f5 1522 * @return
stevew817 0:296308a935f5 1523 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 1524 ******************************************************************************/
stevew817 0:296308a935f5 1525 uint32_t ICM20648::read_temperature(float *temperature)
stevew817 0:296308a935f5 1526 {
stevew817 0:296308a935f5 1527 uint8_t data[2];
stevew817 0:296308a935f5 1528 int16_t raw_temp;
stevew817 0:296308a935f5 1529
stevew817 0:296308a935f5 1530 /* Read temperature registers */
stevew817 0:296308a935f5 1531 read_register(ICM20648_REG_TEMPERATURE_H, 2, data);
stevew817 0:296308a935f5 1532
stevew817 0:296308a935f5 1533 /* Convert to int16 */
stevew817 0:296308a935f5 1534 raw_temp = (int16_t) ( (data[0] << 8) + data[1]);
stevew817 0:296308a935f5 1535
stevew817 0:296308a935f5 1536 /* Calculate the Celsius value from the raw reading */
stevew817 0:296308a935f5 1537 *temperature = ( (float) raw_temp / 333.87) + 21.0;
stevew817 0:296308a935f5 1538
stevew817 0:296308a935f5 1539 return ICM20648_OK;
stevew817 0:296308a935f5 1540 }
stevew817 0:296308a935f5 1541
stevew817 0:296308a935f5 1542 /***************************************************************************//**
stevew817 0:296308a935f5 1543 * @brief
stevew817 0:296308a935f5 1544 * Reads the device ID of the ICM20648
stevew817 0:296308a935f5 1545 *
stevew817 0:296308a935f5 1546 * @param[out] devID
stevew817 0:296308a935f5 1547 * The ID of the device read from teh WHO_AM_I register. Expected value? 0xE0
stevew817 0:296308a935f5 1548 *
stevew817 0:296308a935f5 1549 * @return
stevew817 0:296308a935f5 1550 * Returns zero on OK, non-zero otherwise
stevew817 0:296308a935f5 1551 ******************************************************************************/
stevew817 0:296308a935f5 1552 uint32_t ICM20648::get_device_id(uint8_t *device_id)
stevew817 0:296308a935f5 1553 {
stevew817 0:296308a935f5 1554 read_register(ICM20648_REG_WHO_AM_I, 1, device_id);
stevew817 0:296308a935f5 1555
stevew817 0:296308a935f5 1556 return ICM20648_OK;
stevew817 0:296308a935f5 1557 }
stevew817 0:296308a935f5 1558
stevew817 0:296308a935f5 1559 void ICM20648::irq_handler(void)
stevew817 0:296308a935f5 1560 {
stevew817 0:296308a935f5 1561
stevew817 0:296308a935f5 1562 }