Silicon Labs
Simple driver for the TDK InvenSense ICM20648 6-axis IMU
Dependents: TBSense2_Sensor_Demo
Embed:
(wiki syntax)
Show/hide line numbers
ICM20648.cpp
Go to the documentation of this file.
00001 /***************************************************************************//** 00002 * @file ICM20648.cpp 00003 ******************************************************************************* 00004 * @section License 00005 * <b>(C) Copyright 2017 Silicon Labs, http://www.silabs.com</b> 00006 ******************************************************************************* 00007 * 00008 * SPDX-License-Identifier: Apache-2.0 00009 * 00010 * Licensed under the Apache License, Version 2.0 (the "License"); you may 00011 * not use this file except in compliance with the License. 00012 * You may obtain a copy of the License at 00013 * 00014 * http://www.apache.org/licenses/LICENSE-2.0 00015 * 00016 * Unless required by applicable law or agreed to in writing, software 00017 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 00018 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00019 * See the License for the specific language governing permissions and 00020 * limitations under the License. 00021 * 00022 ******************************************************************************/ 00023 00024 #include "ICM20648.h " 00025 00026 /** @cond DO_NOT_INCLUDE_WITH_DOXYGEN */ 00027 00028 /**************************************************************************//** 00029 * @name Error Codes 00030 * @{ 00031 ******************************************************************************/ 00032 #define ICM20648_OK 0x0000 /**< No errors */ 00033 #define ICM20648_ERROR_INVALID_DEVICE_ID 0x0001 /**< Invalid device ID */ 00034 /**@}*/ 00035 00036 /**************************************************************************//** 00037 * @name ICM20648 register banks 00038 * @{ 00039 ******************************************************************************/ 00040 #define ICM20648_BANK_0 (0 << 7) /**< Register bank 0 */ 00041 #define ICM20648_BANK_1 (1 << 7) /**< Register bank 1 */ 00042 #define ICM20648_BANK_2 (2 << 7) /**< Register bank 2 */ 00043 #define ICM20648_BANK_3 (3 << 7) /**< Register bank 3 */ 00044 /**@}*/ 00045 00046 /**************************************************************************//** 00047 * @name Register and associated bit definitions 00048 * @{ 00049 ******************************************************************************/ 00050 /***********************/ 00051 /* Bank 0 register map */ 00052 /***********************/ 00053 #define ICM20648_REG_WHO_AM_I (ICM20648_BANK_0 | 0x00) /**< Device ID register */ 00054 00055 #define ICM20648_REG_USER_CTRL (ICM20648_BANK_0 | 0x03) /**< User control register */ 00056 #define ICM20648_BIT_DMP_EN 0x80 /**< DMP enable bit */ 00057 #define ICM20648_BIT_FIFO_EN 0x40 /**< FIFO enable bit */ 00058 #define ICM20648_BIT_I2C_MST_EN 0x20 /**< I2C master I/F enable bit */ 00059 #define ICM20648_BIT_I2C_IF_DIS 0x10 /**< Disable I2C, enable SPI bit */ 00060 #define ICM20648_BIT_DMP_RST 0x08 /**< DMP module reset bit */ 00061 #define ICM20648_BIT_DIAMOND_DMP_RST 0x04 /**< SRAM module reset bit */ 00062 00063 #define ICM20648_REG_LP_CONFIG (ICM20648_BANK_0 | 0x05) /**< Low Power mode config register */ 00064 #define ICM20648_BIT_I2C_MST_CYCLE 0x40 /**< I2C master cycle mode enable */ 00065 #define ICM20648_BIT_ACCEL_CYCLE 0x20 /**< Accelerometer cycle mode enable */ 00066 #define ICM20648_BIT_GYRO_CYCLE 0x10 /**< Gyroscope cycle mode enable */ 00067 00068 #define ICM20648_REG_PWR_MGMT_1 (ICM20648_BANK_0 | 0x06) /**< Power Management 1 register */ 00069 #define ICM20648_BIT_H_RESET 0x80 /**< Device reset bit */ 00070 #define ICM20648_BIT_SLEEP 0x40 /**< Sleep mode enable bit */ 00071 #define ICM20648_BIT_LP_EN 0x20 /**< Low Power feature enable bit */ 00072 #define ICM20648_BIT_TEMP_DIS 0x08 /**< Temperature sensor disable bit */ 00073 #define ICM20648_BIT_CLK_PLL 0x01 /**< Auto clock source selection setting */ 00074 00075 #define ICM20648_REG_PWR_MGMT_2 (ICM20648_BANK_0 | 0x07) /**< Power Management 2 register */ 00076 #define ICM20648_BIT_PWR_ACCEL_STBY 0x38 /**< Disable accelerometer */ 00077 #define ICM20648_BIT_PWR_GYRO_STBY 0x07 /**< Disable gyroscope */ 00078 #define ICM20648_BIT_PWR_ALL_OFF 0x7F /**< Disable both accel and gyro */ 00079 00080 #define ICM20648_REG_INT_PIN_CFG (ICM20648_BANK_0 | 0x0F) /**< Interrupt Pin Configuration register */ 00081 #define ICM20648_BIT_INT_ACTL 0x80 /**< Active low setting bit */ 00082 #define ICM20648_BIT_INT_OPEN 0x40 /**< Open collector onfiguration bit */ 00083 #define ICM20648_BIT_INT_LATCH_EN 0x20 /**< Latch enable bit */ 00084 00085 #define ICM20648_REG_INT_ENABLE (ICM20648_BANK_0 | 0x10) /**< Interrupt Enable register */ 00086 #define ICM20648_BIT_WOM_INT_EN 0x08 /**< Wake-up On Motion enable bit */ 00087 00088 #define ICM20648_REG_INT_ENABLE_1 (ICM20648_BANK_0 | 0x11) /**< Interrupt Enable 1 register */ 00089 #define ICM20648_BIT_RAW_DATA_0_RDY_EN 0x01 /**< Raw data ready interrupt enable bit */ 00090 00091 #define ICM20648_REG_INT_ENABLE_2 (ICM20648_BANK_0 | 0x12) /**< Interrupt Enable 2 register */ 00092 #define ICM20648_BIT_FIFO_OVERFLOW_EN_0 0x01 /**< FIFO overflow interrupt enable bit */ 00093 00094 #define ICM20648_REG_INT_ENABLE_3 (ICM20648_BANK_0 | 0x13) /**< Interrupt Enable 2 register */ 00095 00096 #define ICM20648_REG_INT_STATUS (ICM20648_BANK_0 | 0x19) /**< Interrupt Status register */ 00097 #define ICM20648_BIT_WOM_INT 0x08 /**< Wake-up on motion interrupt occured bit */ 00098 #define ICM20648_BIT_PLL_RDY 0x04 /**< PLL ready interrupt occured bit */ 00099 00100 #define ICM20648_REG_INT_STATUS_1 (ICM20648_BANK_0 | 0x1A) /**< Interrupt Status 1 register */ 00101 #define ICM20648_BIT_RAW_DATA_0_RDY_INT 0x01 /**< Raw data ready interrupt occured bit */ 00102 00103 #define ICM20648_REG_INT_STATUS_2 (ICM20648_BANK_0 | 0x1B) /**< Interrupt Status 2 register */ 00104 00105 #define ICM20648_REG_ACCEL_XOUT_H_SH (ICM20648_BANK_0 | 0x2D) /**< Accelerometer X-axis data high byte */ 00106 #define ICM20648_REG_ACCEL_XOUT_L_SH (ICM20648_BANK_0 | 0x2E) /**< Accelerometer X-axis data low byte */ 00107 #define ICM20648_REG_ACCEL_YOUT_H_SH (ICM20648_BANK_0 | 0x2F) /**< Accelerometer Y-axis data high byte */ 00108 #define ICM20648_REG_ACCEL_YOUT_L_SH (ICM20648_BANK_0 | 0x30) /**< Accelerometer Y-axis data low byte */ 00109 #define ICM20648_REG_ACCEL_ZOUT_H_SH (ICM20648_BANK_0 | 0x31) /**< Accelerometer Z-axis data high byte */ 00110 #define ICM20648_REG_ACCEL_ZOUT_L_SH (ICM20648_BANK_0 | 0x32) /**< Accelerometer Z-axis data low byte */ 00111 00112 #define ICM20648_REG_GYRO_XOUT_H_SH (ICM20648_BANK_0 | 0x33) /**< Gyroscope X-axis data high byte */ 00113 #define ICM20648_REG_GYRO_XOUT_L_SH (ICM20648_BANK_0 | 0x34) /**< Gyroscope X-axis data low byte */ 00114 #define ICM20648_REG_GYRO_YOUT_H_SH (ICM20648_BANK_0 | 0x35) /**< Gyroscope Y-axis data high byte */ 00115 #define ICM20648_REG_GYRO_YOUT_L_SH (ICM20648_BANK_0 | 0x36) /**< Gyroscope Y-axis data low byte */ 00116 #define ICM20648_REG_GYRO_ZOUT_H_SH (ICM20648_BANK_0 | 0x37) /**< Gyroscope Z-axis data high byte */ 00117 #define ICM20648_REG_GYRO_ZOUT_L_SH (ICM20648_BANK_0 | 0x38) /**< Gyroscope Z-axis data low byte */ 00118 00119 #define ICM20648_REG_TEMPERATURE_H (ICM20648_BANK_0 | 0x39) /**< Temperature data high byte */ 00120 #define ICM20648_REG_TEMPERATURE_L (ICM20648_BANK_0 | 0x3A) /**< Temperature data low byte */ 00121 #define ICM20648_REG_TEMP_CONFIG (ICM20648_BANK_0 | 0x53) /**< Temperature Configuration register */ 00122 00123 #define ICM20648_REG_FIFO_EN_1 (ICM20648_BANK_0 | 0x66) /**< FIFO Enable 1 register */ 00124 00125 #define ICM20648_REG_FIFO_EN_2 (ICM20648_BANK_0 | 0x67) /**< FIFO Enable 2 register */ 00126 #define ICM20648_BIT_ACCEL_FIFO_EN 0x10 /**< Enable writing acceleration data to FIFO bit */ 00127 #define ICM20648_BITS_GYRO_FIFO_EN 0x0E /**< Enable writing gyroscope data to FIFO bit */ 00128 00129 #define ICM20648_REG_FIFO_RST (ICM20648_BANK_0 | 0x68) /**< FIFO Reset register */ 00130 #define ICM20648_REG_FIFO_MODE (ICM20648_BANK_0 | 0x69) /**< FIFO Mode register */ 00131 00132 #define ICM20648_REG_FIFO_COUNT_H (ICM20648_BANK_0 | 0x70) /**< FIFO data count high byte */ 00133 #define ICM20648_REG_FIFO_COUNT_L (ICM20648_BANK_0 | 0x71) /**< FIFO data count low byte */ 00134 #define ICM20648_REG_FIFO_R_W (ICM20648_BANK_0 | 0x72) /**< FIFO Read/Write register */ 00135 00136 #define ICM20648_REG_DATA_RDY_STATUS (ICM20648_BANK_0 | 0x74) /**< Data Ready Status register */ 00137 #define ICM20648_BIT_RAW_DATA_0_RDY 0x01 /**< Raw Data Ready bit */ 00138 00139 #define ICM20648_REG_FIFO_CFG (ICM20648_BANK_0 | 0x76) /**< FIFO Configuration register */ 00140 #define ICM20648_BIT_MULTI_FIFO_CFG 0x01 /**< Interrupt status for each sensor is required */ 00141 #define ICM20648_BIT_SINGLE_FIFO_CFG 0x00 /**< Interrupt status for only a single sensor is required */ 00142 00143 /***********************/ 00144 /* Bank 1 register map */ 00145 /***********************/ 00146 #define ICM20648_REG_XA_OFFSET_H (ICM20648_BANK_1 | 0x14) /**< Acceleration sensor X-axis offset cancellation high byte */ 00147 #define ICM20648_REG_XA_OFFSET_L (ICM20648_BANK_1 | 0x15) /**< Acceleration sensor X-axis offset cancellation low byte */ 00148 #define ICM20648_REG_YA_OFFSET_H (ICM20648_BANK_1 | 0x17) /**< Acceleration sensor Y-axis offset cancellation high byte */ 00149 #define ICM20648_REG_YA_OFFSET_L (ICM20648_BANK_1 | 0x18) /**< Acceleration sensor Y-axis offset cancellation low byte */ 00150 #define ICM20648_REG_ZA_OFFSET_H (ICM20648_BANK_1 | 0x1A) /**< Acceleration sensor Z-axis offset cancellation high byte */ 00151 #define ICM20648_REG_ZA_OFFSET_L (ICM20648_BANK_1 | 0x1B) /**< Acceleration sensor Z-axis offset cancellation low byte */ 00152 00153 #define ICM20648_REG_TIMEBASE_CORR_PLL (ICM20648_BANK_1 | 0x28) /**< PLL Timebase Correction register */ 00154 00155 /***********************/ 00156 /* Bank 2 register map */ 00157 /***********************/ 00158 #define ICM20648_REG_GYRO_SMPLRT_DIV (ICM20648_BANK_2 | 0x00) /**< Gyroscope Sample Rate Divider regiser */ 00159 00160 #define ICM20648_REG_GYRO_CONFIG_1 (ICM20648_BANK_2 | 0x01) /**< Gyroscope Configuration 1 register */ 00161 #define ICM20648_BIT_GYRO_FCHOICE 0x01 /**< Gyro Digital Low-Pass Filter enable bit */ 00162 #define ICM20648_SHIFT_GYRO_FS_SEL 1 /**< Gyro Full Scale Select bit shift */ 00163 #define ICM20648_SHIFT_GYRO_DLPCFG 3 /**< Gyro DLPF Config bit shift */ 00164 #define ICM20648_MASK_GYRO_FULLSCALE 0x06 /**< Gyro Full Scale Select bitmask */ 00165 #define ICM20648_MASK_GYRO_BW 0x39 /**< Gyro Bandwidth Select bitmask */ 00166 #define ICM20648_GYRO_FULLSCALE_250DPS (0x00 << ICM20648_SHIFT_GYRO_FS_SEL) /**< Gyro Full Scale = 250 deg/sec */ 00167 #define ICM20648_GYRO_FULLSCALE_500DPS (0x01 << ICM20648_SHIFT_GYRO_FS_SEL) /**< Gyro Full Scale = 500 deg/sec */ 00168 #define ICM20648_GYRO_FULLSCALE_1000DPS (0x02 << ICM20648_SHIFT_GYRO_FS_SEL) /**< Gyro Full Scale = 1000 deg/sec */ 00169 #define ICM20648_GYRO_FULLSCALE_2000DPS (0x03 << ICM20648_SHIFT_GYRO_FS_SEL) /**< Gyro Full Scale = 2000 deg/sec */ 00170 #define ICM20648_GYRO_BW_12100HZ (0x00 << ICM20648_SHIFT_GYRO_DLPCFG) /**< Gyro Bandwidth = 12100 Hz */ 00171 #define ICM20648_GYRO_BW_360HZ ( (0x07 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 360 Hz */ 00172 #define ICM20648_GYRO_BW_200HZ ( (0x00 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 200 Hz */ 00173 #define ICM20648_GYRO_BW_150HZ ( (0x01 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 150 Hz */ 00174 #define ICM20648_GYRO_BW_120HZ ( (0x02 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 120 Hz */ 00175 #define ICM20648_GYRO_BW_51HZ ( (0x03 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 51 Hz */ 00176 #define ICM20648_GYRO_BW_24HZ ( (0x04 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 24 Hz */ 00177 #define ICM20648_GYRO_BW_12HZ ( (0x05 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 12 Hz */ 00178 #define ICM20648_GYRO_BW_6HZ ( (0x06 << ICM20648_SHIFT_GYRO_DLPCFG) | ICM20648_BIT_GYRO_FCHOICE) /**< Gyro Bandwidth = 6 Hz */ 00179 00180 #define ICM20648_REG_GYRO_CONFIG_2 (ICM20648_BANK_2 | 0x02) /**< Gyroscope Configuration 2 register */ 00181 #define ICM20648_BIT_GYRO_CTEN 0x38 /**< Gyroscope Self-Test Enable bits */ 00182 00183 #define ICM20648_REG_XG_OFFS_USRH (ICM20648_BANK_2 | 0x03) /**< Gyroscope sensor X-axis offset cancellation high byte */ 00184 #define ICM20648_REG_XG_OFFS_USRL (ICM20648_BANK_2 | 0x04) /**< Gyroscope sensor X-axis offset cancellation low byte */ 00185 #define ICM20648_REG_YG_OFFS_USRH (ICM20648_BANK_2 | 0x05) /**< Gyroscope sensor Y-axis offset cancellation high byte */ 00186 #define ICM20648_REG_YG_OFFS_USRL (ICM20648_BANK_2 | 0x06) /**< Gyroscope sensor Y-axis offset cancellation low byte */ 00187 #define ICM20648_REG_ZG_OFFS_USRH (ICM20648_BANK_2 | 0x07) /**< Gyroscope sensor Z-axis offset cancellation high byte */ 00188 #define ICM20648_REG_ZG_OFFS_USRL (ICM20648_BANK_2 | 0x08) /**< Gyroscope sensor Z-axis offset cancellation low byte */ 00189 00190 #define ICM20648_REG_ODR_ALIGN_EN (ICM20648_BANK_2 | 0x09) /**< Output Data Rate start time alignment */ 00191 00192 #define ICM20648_REG_ACCEL_SMPLRT_DIV_1 (ICM20648_BANK_2 | 0x10) /**< Acceleration Sensor Sample Rate Divider 1 register */ 00193 #define ICM20648_REG_ACCEL_SMPLRT_DIV_2 (ICM20648_BANK_2 | 0x11) /**< Acceleration Sensor Sample Rate Divider 2 register */ 00194 00195 #define ICM20648_REG_ACCEL_INTEL_CTRL (ICM20648_BANK_2 | 0x12) /**< Accelerometer Hardware Intelligence Control register */ 00196 #define ICM20648_BIT_ACCEL_INTEL_EN 0x02 /**< Wake-up On Motion enable bit */ 00197 #define ICM20648_BIT_ACCEL_INTEL_MODE 0x01 /**< WOM algorithm selection bit */ 00198 00199 #define ICM20648_REG_ACCEL_WOM_THR (ICM20648_BANK_2 | 0x13) /**< Wake-up On Motion Threshold register */ 00200 00201 #define ICM20648_REG_ACCEL_CONFIG (ICM20648_BANK_2 | 0x14) /**< Accelerometer Configuration register */ 00202 #define ICM20648_BIT_ACCEL_FCHOICE 0x01 /**< Accel Digital Low-Pass Filter enable bit */ 00203 #define ICM20648_SHIFT_ACCEL_FS 1 /**< Accel Full Scale Select bit shift */ 00204 #define ICM20648_SHIFT_ACCEL_DLPCFG 3 /**< Accel DLPF Config bit shift */ 00205 #define ICM20648_MASK_ACCEL_FULLSCALE 0x06 /**< Accel Full Scale Select bitmask */ 00206 #define ICM20648_MASK_ACCEL_BW 0x39 /**< Accel Bandwidth Select bitmask */ 00207 #define ICM20648_ACCEL_FULLSCALE_2G (0x00 << ICM20648_SHIFT_ACCEL_FS) /**< Accel Full Scale = 2 g */ 00208 #define ICM20648_ACCEL_FULLSCALE_4G (0x01 << ICM20648_SHIFT_ACCEL_FS) /**< Accel Full Scale = 4 g */ 00209 #define ICM20648_ACCEL_FULLSCALE_8G (0x02 << ICM20648_SHIFT_ACCEL_FS) /**< Accel Full Scale = 8 g */ 00210 #define ICM20648_ACCEL_FULLSCALE_16G (0x03 << ICM20648_SHIFT_ACCEL_FS) /**< Accel Full Scale = 16 g */ 00211 #define ICM20648_ACCEL_BW_1210HZ (0x00 << ICM20648_SHIFT_ACCEL_DLPCFG) /**< Accel Bandwidth = 1210 Hz */ 00212 #define ICM20648_ACCEL_BW_470HZ ( (0x07 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 470 Hz */ 00213 #define ICM20648_ACCEL_BW_246HZ ( (0x00 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 246 Hz */ 00214 #define ICM20648_ACCEL_BW_111HZ ( (0x02 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 111 Hz */ 00215 #define ICM20648_ACCEL_BW_50HZ ( (0x03 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 50 Hz */ 00216 #define ICM20648_ACCEL_BW_24HZ ( (0x04 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 24 Hz */ 00217 #define ICM20648_ACCEL_BW_12HZ ( (0x05 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 12 Hz */ 00218 #define ICM20648_ACCEL_BW_6HZ ( (0x06 << ICM20648_SHIFT_ACCEL_DLPCFG) | ICM20648_BIT_ACCEL_FCHOICE) /**< Accel Bandwidth = 6 Hz */ 00219 00220 #define ICM20648_REG_ACCEL_CONFIG_2 (ICM20648_BANK_2 | 0x15) /**< Accelerometer Configuration 2 register */ 00221 #define ICM20648_BIT_ACCEL_CTEN 0x1C /**< Accelerometer Self-Test Enable bits */ 00222 00223 /***********************/ 00224 /* Bank 3 register map */ 00225 /***********************/ 00226 #define ICM20648_REG_I2C_MST_ODR_CONFIG (ICM20648_BANK_3 | 0x00) /**< I2C Master Output Data Rate Configuration register */ 00227 00228 #define ICM20648_REG_I2C_MST_CTRL (ICM20648_BANK_3 | 0x01) /**< I2C Master Control register */ 00229 #define ICM20648_BIT_I2C_MST_P_NSR 0x10 /**< Stop between reads enabling bit */ 00230 00231 #define ICM20648_REG_I2C_MST_DELAY_CTRL (ICM20648_BANK_3 | 0x02) /**< I2C Master Delay Control register */ 00232 #define ICM20648_BIT_SLV0_DLY_EN 0x01 /**< I2C Slave0 Delay Enable bit */ 00233 #define ICM20648_BIT_SLV1_DLY_EN 0x02 /**< I2C Slave1 Delay Enable bit */ 00234 #define ICM20648_BIT_SLV2_DLY_EN 0x04 /**< I2C Slave2 Delay Enable bit */ 00235 #define ICM20648_BIT_SLV3_DLY_EN 0x08 /**< I2C Slave3 Delay Enable bit */ 00236 00237 #define ICM20648_REG_I2C_SLV0_ADDR (ICM20648_BANK_3 | 0x03) /**< I2C Slave0 Physical Address register */ 00238 #define ICM20648_REG_I2C_SLV0_REG (ICM20648_BANK_3 | 0x04) /**< I2C Slave0 Register Address register */ 00239 #define ICM20648_REG_I2C_SLV0_CTRL (ICM20648_BANK_3 | 0x05) /**< I2C Slave0 Control register */ 00240 #define ICM20648_REG_I2C_SLV0_DO (ICM20648_BANK_3 | 0x06) /**< I2C Slave0 Data Out register */ 00241 00242 #define ICM20648_REG_I2C_SLV1_ADDR (ICM20648_BANK_3 | 0x07) /**< I2C Slave1 Physical Address register */ 00243 #define ICM20648_REG_I2C_SLV1_REG (ICM20648_BANK_3 | 0x08) /**< I2C Slave1 Register Address register */ 00244 #define ICM20648_REG_I2C_SLV1_CTRL (ICM20648_BANK_3 | 0x09) /**< I2C Slave1 Control register */ 00245 #define ICM20648_REG_I2C_SLV1_DO (ICM20648_BANK_3 | 0x0A) /**< I2C Slave1 Data Out register */ 00246 00247 #define ICM20648_REG_I2C_SLV2_ADDR (ICM20648_BANK_3 | 0x0B) /**< I2C Slave2 Physical Address register */ 00248 #define ICM20648_REG_I2C_SLV2_REG (ICM20648_BANK_3 | 0x0C) /**< I2C Slave2 Register Address register */ 00249 #define ICM20648_REG_I2C_SLV2_CTRL (ICM20648_BANK_3 | 0x0D) /**< I2C Slave2 Control register */ 00250 #define ICM20648_REG_I2C_SLV2_DO (ICM20648_BANK_3 | 0x0E) /**< I2C Slave2 Data Out register */ 00251 00252 #define ICM20648_REG_I2C_SLV3_ADDR (ICM20648_BANK_3 | 0x0F) /**< I2C Slave3 Physical Address register */ 00253 #define ICM20648_REG_I2C_SLV3_REG (ICM20648_BANK_3 | 0x10) /**< I2C Slave3 Register Address register */ 00254 #define ICM20648_REG_I2C_SLV3_CTRL (ICM20648_BANK_3 | 0x11) /**< I2C Slave3 Control register */ 00255 #define ICM20648_REG_I2C_SLV3_DO (ICM20648_BANK_3 | 0x12) /**< I2C Slave3 Data Out register */ 00256 00257 #define ICM20648_REG_I2C_SLV4_ADDR (ICM20648_BANK_3 | 0x13) /**< I2C Slave4 Physical Address register */ 00258 #define ICM20648_REG_I2C_SLV4_REG (ICM20648_BANK_3 | 0x14) /**< I2C Slave4 Register Address register */ 00259 #define ICM20648_REG_I2C_SLV4_CTRL (ICM20648_BANK_3 | 0x15) /**< I2C Slave4 Control register */ 00260 #define ICM20648_REG_I2C_SLV4_DO (ICM20648_BANK_3 | 0x16) /**< I2C Slave4 Data Out register */ 00261 #define ICM20648_REG_I2C_SLV4_DI (ICM20648_BANK_3 | 0x17) /**< I2C Slave4 Data In register */ 00262 00263 #define ICM20648_BIT_I2C_SLV_EN 0x80 /**< I2C Slave Enable bit */ 00264 #define ICM20648_BIT_I2C_BYTE_SW 0x40 /**< I2C Slave Byte Swap enable bit */ 00265 #define ICM20648_BIT_I2C_REG_DIS 0x20 /**< I2C Slave Do Not Write Register Value bit */ 00266 #define ICM20648_BIT_I2C_GRP 0x10 /**< I2C Slave Group bit */ 00267 #define ICM20648_BIT_I2C_READ 0x80 /**< I2C Slave R/W bit */ 00268 00269 /* Register common for all banks */ 00270 #define ICM20648_REG_BANK_SEL 0x7F /**< Bank Select register */ 00271 00272 #define ICM20648_DEVICE_ID 0xE0 /**< ICM20648 Device ID value */ 00273 #define ICM20948_DEVICE_ID 0xEA /**< ICM20948 Device ID value */ 00274 /**@}*/ 00275 00276 /** @endcond */ 00277 00278 00279 ICM20648::ICM20648(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName irq) : m_SPI(mosi, miso, sclk), m_CS(cs, 1), m_IRQ(irq) 00280 { 00281 m_IRQ.disable_irq(); 00282 m_IRQ.fall(Callback<void(void)>(this, &ICM20648::irq_handler)); 00283 } 00284 00285 ICM20648::~ICM20648(void) 00286 { 00287 } 00288 00289 bool ICM20648::open() 00290 { 00291 uint8_t data; 00292 00293 reset(); 00294 00295 /* Disable I2C interface, use SPI */ 00296 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_I2C_IF_DIS); 00297 00298 /* Read Who am I register, should get 0x71 */ 00299 read_register(ICM20648_REG_WHO_AM_I, 1, &data); 00300 00301 /* If not - return */ 00302 if ( (data != ICM20648_DEVICE_ID) && (data != ICM20948_DEVICE_ID) ) { 00303 return false; 00304 } 00305 00306 /* Auto selects the best available clock source PLL if ready, else use the Internal oscillator */ 00307 write_register(ICM20648_REG_PWR_MGMT_1, ICM20648_BIT_CLK_PLL); 00308 00309 /* PLL startup time - maybe it is too long but better be on the safe side, no spec in the datasheet */ 00310 wait_ms(30); 00311 00312 /* 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 */ 00313 write_register(ICM20648_REG_INT_PIN_CFG, ICM20648_BIT_INT_ACTL | ICM20648_BIT_INT_OPEN); 00314 00315 return true; 00316 } 00317 00318 /** Perform a measurement 00319 * 00320 * @returns true if measurement was successful 00321 */ 00322 bool ICM20648::measure() 00323 { 00324 00325 } 00326 00327 /** Do a measurement on the gyroscope 00328 * 00329 * @param[out] gyr_x Gyroscope measurement on X axis 00330 * @param[out] gyr_y Gyroscope measurement on Y axis 00331 * @param[out] gyr_z Gyroscope measurement on Z axis 00332 * 00333 * @returns true if measurement was successful 00334 */ 00335 bool ICM20648::get_gyroscope(float *gyr_x, float *gyr_y, float *gyr_z) 00336 { 00337 float buf[3]; 00338 if(read_gyro_data(buf)) { 00339 return false; 00340 } 00341 00342 *gyr_x = buf[0]; 00343 *gyr_y = buf[1]; 00344 *gyr_z = buf[2]; 00345 } 00346 00347 /** Do a measurement on the accelerometer 00348 * 00349 * @param[out] acc_x Accelerometer measurement on X axis 00350 * @param[out] acc_y Accelerometer measurement on Y axis 00351 * @param[out] acc_z Accelerometer measurement on Z axis 00352 * 00353 * @returns true if measurement was successful 00354 */ 00355 bool ICM20648::get_accelerometer(float *acc_x, float *acc_y, float *acc_z) 00356 { 00357 float buf[3]; 00358 if(read_accel_data(buf)) { 00359 return false; 00360 } 00361 00362 *acc_x = buf[0]; 00363 *acc_y = buf[1]; 00364 *acc_z = buf[2]; 00365 } 00366 00367 bool ICM20648::get_temperature(float *temperature) 00368 { 00369 read_temperature(temperature); 00370 return true; 00371 } 00372 00373 /***************************************************************************//** 00374 * @brief 00375 * Reads register from the ICM20648 device 00376 * 00377 * @param[in] addr 00378 * The register address to read from in the sensor 00379 * Bit[8:7] - bank address 00380 * Bit[6:0] - register address 00381 * 00382 * @param[in] numBytes 00383 * The number of bytes to read 00384 * 00385 * @param[out] data 00386 * The data read from the register 00387 * 00388 * @return 00389 * None 00390 ******************************************************************************/ 00391 void ICM20648::read_register(uint16_t addr, int numBytes, uint8_t *data) 00392 { 00393 uint8_t regAddr; 00394 uint8_t bank; 00395 00396 regAddr = (uint8_t) (addr & 0x7F); 00397 bank = (uint8_t) (addr >> 7); 00398 00399 select_bank(bank); 00400 00401 /* Enable chip select */ 00402 m_CS = 0; 00403 00404 /* Set R/W bit to 1 - read */ 00405 m_SPI.write(regAddr | 0x80); 00406 /* Transmit 0's to provide clock and read the data */ 00407 m_SPI.write(NULL, 0, (char*)data, numBytes); 00408 00409 /* Disable chip select */ 00410 m_CS = 1; 00411 00412 return; 00413 } 00414 00415 /***************************************************************************//** 00416 * @brief 00417 * Writes a register in the ICM20648 device 00418 * 00419 * @param[in] addr 00420 * The register address to write 00421 * Bit[8:7] - bank address 00422 * Bit[6:0] - register address 00423 * 00424 * @param[in] data 00425 * The data to write to the register 00426 * 00427 * @return 00428 * None 00429 ******************************************************************************/ 00430 void ICM20648::write_register(uint16_t addr, uint8_t data) 00431 { 00432 uint8_t regAddr; 00433 uint8_t bank; 00434 00435 regAddr = (uint8_t) (addr & 0x7F); 00436 bank = (uint8_t) (addr >> 7); 00437 00438 select_bank(bank); 00439 00440 /* Enable chip select */ 00441 m_CS = 0; 00442 00443 /* clear R/W bit - write, send the address */ 00444 m_SPI.write(regAddr & 0x7F); 00445 m_SPI.write(data); 00446 00447 /* Disable chip select */ 00448 m_CS = 1; 00449 00450 return; 00451 } 00452 00453 /***************************************************************************//** 00454 * @brief 00455 * Select the desired register bank 00456 * 00457 * @param[in] bank 00458 * The address of the register bank (0..3) 00459 * 00460 * @return 00461 * None 00462 ******************************************************************************/ 00463 void ICM20648::select_bank(uint8_t bank) 00464 { 00465 /* Enable chip select */ 00466 m_CS = 0; 00467 00468 /* clear R/W bit - write, send the address */ 00469 m_SPI.write(ICM20648_REG_BANK_SEL); 00470 m_SPI.write((uint8_t)(bank << 4)); 00471 00472 /* Disable chip select */ 00473 m_CS = 1; 00474 00475 return; 00476 } 00477 00478 /***************************************************************************//** 00479 * @brief 00480 * Performs soft reset on the ICM20648 chip 00481 * 00482 * @return 00483 * Returns zero on OK, non-zero otherwise 00484 ******************************************************************************/ 00485 uint32_t ICM20648::reset(void) 00486 { 00487 /* Set H_RESET bit to initiate soft reset */ 00488 write_register(ICM20648_REG_PWR_MGMT_1, ICM20648_BIT_H_RESET); 00489 00490 /* Wait 100ms to complete the reset sequence */ 00491 wait_ms(100); 00492 00493 return ICM20648_OK; 00494 } 00495 00496 /***************************************************************************//** 00497 * @brief 00498 * Sets the sample rate both of the accelerometer and the gyroscope. 00499 * 00500 * @param[in] sampleRate 00501 * The desired sample rate in Hz. Since the resolution of the sample rate 00502 * divider is different in the accel and gyro stages it is possible that 00503 * the two sensor will have different sample rate set. 00504 * 00505 * @return 00506 * Returns zero on OK, non-zero otherwise 00507 ******************************************************************************/ 00508 uint32_t ICM20648::set_sample_rate(float sampleRate) 00509 { 00510 set_gyro_sample_rate(sampleRate); 00511 set_accel_sample_rate(sampleRate); 00512 00513 return ICM20648_OK; 00514 } 00515 00516 /***************************************************************************//** 00517 * @brief 00518 * Sets the sample rate of the accelerometer 00519 * 00520 * @param[in] sampleRate 00521 * The desired sample rate in Hz 00522 * 00523 * @return 00524 * The actual sample rate. May be different from the desired value because 00525 * of the finite and discrete number of divider settings 00526 ******************************************************************************/ 00527 float ICM20648::set_gyro_sample_rate(float sampleRate) 00528 { 00529 uint8_t gyroDiv; 00530 float gyroSampleRate; 00531 00532 /* Calculate the sample rate divider */ 00533 gyroSampleRate = (1125.0 / sampleRate) - 1.0; 00534 00535 /* Check if it fits in the divider register */ 00536 if ( gyroSampleRate > 255.0 ) { 00537 gyroSampleRate = 255.0; 00538 } 00539 00540 if ( gyroSampleRate < 0 ) { 00541 gyroSampleRate = 0.0; 00542 } 00543 00544 /* Write the value to the register */ 00545 gyroDiv = (uint8_t) gyroSampleRate; 00546 write_register(ICM20648_REG_GYRO_SMPLRT_DIV, gyroDiv); 00547 00548 /* Calculate the actual sample rate from the divider value */ 00549 gyroSampleRate = 1125.0 / (gyroDiv + 1); 00550 00551 return gyroSampleRate; 00552 } 00553 00554 /***************************************************************************//** 00555 * @brief 00556 * Sets the sample rate of the gyroscope 00557 * 00558 * @param[in] sampleRate 00559 * The desired sample rate in Hz 00560 * 00561 * @return 00562 * The actual sample rate. May be different from the desired value because 00563 * of the finite and discrete number of divider settings 00564 ******************************************************************************/ 00565 float ICM20648::set_accel_sample_rate(float sampleRate) 00566 { 00567 uint16_t accelDiv; 00568 float accelSampleRate; 00569 00570 /* Calculate the sample rate divider */ 00571 accelSampleRate = (1125.0 / sampleRate) - 1.0; 00572 00573 /* Check if it fits in the divider registers */ 00574 if ( accelSampleRate > 4095.0 ) { 00575 accelSampleRate = 4095.0; 00576 } 00577 00578 if ( accelSampleRate < 0 ) { 00579 accelSampleRate = 0.0; 00580 } 00581 00582 /* Write the value to the registers */ 00583 accelDiv = (uint16_t) accelSampleRate; 00584 write_register(ICM20648_REG_ACCEL_SMPLRT_DIV_1, (uint8_t) (accelDiv >> 8) ); 00585 write_register(ICM20648_REG_ACCEL_SMPLRT_DIV_2, (uint8_t) (accelDiv & 0xFF) ); 00586 00587 /* Calculate the actual sample rate from the divider value */ 00588 accelSampleRate = 1125.0 / (accelDiv + 1); 00589 00590 return accelSampleRate; 00591 } 00592 00593 /***************************************************************************//** 00594 * @brief 00595 * Sets the bandwidth of the gyroscope 00596 * 00597 * @param[in] gyroBw 00598 * The desired bandwidth value. Use the ICM20648_GYRO_BW_xHZ macros, which 00599 * are defined in the icm20648.h file. The value of x can be 00600 * 6, 12, 24, 51, 120, 150, 200, 360 or 12100. 00601 * 00602 * @return 00603 * Returns zero on OK, non-zero otherwise 00604 ******************************************************************************/ 00605 uint32_t ICM20648::set_gyro_bandwidth(uint8_t gyroBw) 00606 { 00607 uint8_t reg; 00608 00609 /* Read the GYRO_CONFIG_1 register */ 00610 read_register(ICM20648_REG_GYRO_CONFIG_1, 1, ®); 00611 reg &= ~(ICM20648_MASK_GYRO_BW); 00612 00613 /* Write the new bandwidth value to the gyro config register */ 00614 reg |= (gyroBw & ICM20648_MASK_GYRO_BW); 00615 write_register(ICM20648_REG_GYRO_CONFIG_1, reg); 00616 00617 return ICM20648_OK; 00618 } 00619 00620 /***************************************************************************//** 00621 * @brief 00622 * Sets the bandwidth of the accelerometer 00623 * 00624 * @param[in] accelBw 00625 * The desired bandwidth value. Use the ICM20648_ACCEL_BW_yHZ macros, which 00626 * are defined in the icm20648.h file. The value of y can be 00627 * 6, 12, 24, 50, 111, 246, 470 or 1210. 00628 * 00629 * @return 00630 * Returns zero on OK, non-zero otherwise 00631 ******************************************************************************/ 00632 uint32_t ICM20648::set_accel_bandwidth(uint8_t accelBw) 00633 { 00634 uint8_t reg; 00635 00636 /* Read the GYRO_CONFIG_1 register */ 00637 read_register(ICM20648_REG_ACCEL_CONFIG, 1, ®); 00638 reg &= ~(ICM20648_MASK_ACCEL_BW); 00639 00640 /* Write the new bandwidth value to the gyro config register */ 00641 reg |= (accelBw & ICM20648_MASK_ACCEL_BW); 00642 write_register(ICM20648_REG_ACCEL_CONFIG, reg); 00643 00644 return ICM20648_OK; 00645 } 00646 00647 /***************************************************************************//** 00648 * @brief 00649 * Reads the raw acceleration value and converts to g value based on 00650 * the actual resolution 00651 * 00652 * @param[out] accel 00653 * A 3-element array of float numbers containing the acceleration values 00654 * for the x, y and z axes in g units. 00655 * 00656 * @return 00657 * Returns zero on OK, non-zero otherwise 00658 ******************************************************************************/ 00659 uint32_t ICM20648::read_accel_data(float *accel) 00660 { 00661 uint8_t rawData[6]; 00662 float accelRes; 00663 int16_t temp; 00664 00665 /* Retrieve the current resolution */ 00666 get_accel_resolution(&accelRes); 00667 00668 /* Read the six raw data registers into data array */ 00669 read_register(ICM20648_REG_ACCEL_XOUT_H_SH, 6, &rawData[0]); 00670 00671 /* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the G value */ 00672 temp = ( (int16_t) rawData[0] << 8) | rawData[1]; 00673 accel[0] = (float) temp * accelRes; 00674 temp = ( (int16_t) rawData[2] << 8) | rawData[3]; 00675 accel[1] = (float) temp * accelRes; 00676 temp = ( (int16_t) rawData[4] << 8) | rawData[5]; 00677 accel[2] = (float) temp * accelRes; 00678 00679 return ICM20648_OK; 00680 } 00681 00682 /***************************************************************************//** 00683 * @brief 00684 * Reads the raw gyroscope value and converts to deg/sec value based on 00685 * the actual resolution 00686 * 00687 * @param[out] gyro 00688 * A 3-element array of float numbers containing the gyroscope values 00689 * for the x, y and z axes in deg/sec units. 00690 * 00691 * @return 00692 * Returns zero on OK, non-zero otherwise 00693 ******************************************************************************/ 00694 uint32_t ICM20648::read_gyro_data(float *gyro) 00695 { 00696 uint8_t rawData[6]; 00697 float gyroRes; 00698 int16_t temp; 00699 00700 /* Retrieve the current resolution */ 00701 get_gyro_resolution(&gyroRes); 00702 00703 /* Read the six raw data registers into data array */ 00704 read_register(ICM20648_REG_GYRO_XOUT_H_SH, 6, &rawData[0]); 00705 00706 /* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the dps value */ 00707 temp = ( (int16_t) rawData[0] << 8) | rawData[1]; 00708 gyro[0] = (float) temp * gyroRes; 00709 temp = ( (int16_t) rawData[2] << 8) | rawData[3]; 00710 gyro[1] = (float) temp * gyroRes; 00711 temp = ( (int16_t) rawData[4] << 8) | rawData[5]; 00712 gyro[2] = (float) temp * gyroRes; 00713 00714 return ICM20648_OK; 00715 } 00716 00717 /***************************************************************************//** 00718 * @brief 00719 * Gets the actual resolution of the accelerometer 00720 * 00721 * @param[out] accelRes 00722 * The resolution in g/bit units 00723 * 00724 * @return 00725 * Returns zero on OK, non-zero otherwise 00726 ******************************************************************************/ 00727 uint32_t ICM20648::get_accel_resolution(float *accelRes) 00728 { 00729 uint8_t reg; 00730 00731 /* Read the actual acceleration full scale setting */ 00732 read_register(ICM20648_REG_ACCEL_CONFIG, 1, ®); 00733 reg &= ICM20648_MASK_ACCEL_FULLSCALE; 00734 00735 /* Calculate the resolution */ 00736 switch ( reg ) { 00737 case ICM20648_ACCEL_FULLSCALE_2G: 00738 *accelRes = 2.0 / 32768.0; 00739 break; 00740 00741 case ICM20648_ACCEL_FULLSCALE_4G: 00742 *accelRes = 4.0 / 32768.0; 00743 break; 00744 00745 case ICM20648_ACCEL_FULLSCALE_8G: 00746 *accelRes = 8.0 / 32768.0; 00747 break; 00748 00749 case ICM20648_ACCEL_FULLSCALE_16G: 00750 *accelRes = 16.0 / 32768.0; 00751 break; 00752 } 00753 00754 return ICM20648_OK; 00755 } 00756 00757 /***************************************************************************//** 00758 * @brief 00759 * Gets the actual resolution of the gyroscope 00760 * 00761 * @param[out] gyroRes 00762 * The actual resolution in (deg/sec)/bit units 00763 * 00764 * @return 00765 * Returns zero on OK, non-zero otherwise 00766 ******************************************************************************/ 00767 uint32_t ICM20648::get_gyro_resolution(float *gyroRes) 00768 { 00769 uint8_t reg; 00770 00771 /* Read the actual gyroscope full scale setting */ 00772 read_register(ICM20648_REG_GYRO_CONFIG_1, 1, ®); 00773 reg &= ICM20648_MASK_GYRO_FULLSCALE; 00774 00775 /* Calculate the resolution */ 00776 switch ( reg ) { 00777 case ICM20648_GYRO_FULLSCALE_250DPS: 00778 *gyroRes = 250.0 / 32768.0; 00779 break; 00780 00781 case ICM20648_GYRO_FULLSCALE_500DPS: 00782 *gyroRes = 500.0 / 32768.0; 00783 break; 00784 00785 case ICM20648_GYRO_FULLSCALE_1000DPS: 00786 *gyroRes = 1000.0 / 32768.0; 00787 break; 00788 00789 case ICM20648_GYRO_FULLSCALE_2000DPS: 00790 *gyroRes = 2000.0 / 32768.0; 00791 break; 00792 } 00793 00794 return ICM20648_OK; 00795 } 00796 00797 /***************************************************************************//** 00798 * @brief 00799 * Sets the full scale value of the accelerometer 00800 * 00801 * @param[in] accelFs 00802 * The desired full scale value. Use the ICM20648_ACCEL_FULLSCALE_xG 00803 * macros, which are defined in the icm20648.h file. The value of x can be 00804 * 2, 4, 8 or 16. 00805 * 00806 * @return 00807 * Returns zero on OK, non-zero otherwise 00808 ******************************************************************************/ 00809 uint32_t ICM20648::set_accel_fullscale(uint8_t accelFs) 00810 { 00811 uint8_t reg; 00812 00813 accelFs &= ICM20648_MASK_ACCEL_FULLSCALE; 00814 read_register(ICM20648_REG_ACCEL_CONFIG, 1, ®); 00815 reg &= ~(ICM20648_MASK_ACCEL_FULLSCALE); 00816 reg |= accelFs; 00817 write_register(ICM20648_REG_ACCEL_CONFIG, reg); 00818 00819 return ICM20648_OK; 00820 } 00821 00822 /***************************************************************************//** 00823 * @brief 00824 * Sets the full scale value of the gyroscope 00825 * 00826 * @param[in] gyroFs 00827 * The desired full scale value. Use the ICM20648_GYRO_FULLSCALE_yDPS 00828 * macros, which are defined in the icm20648.h file. The value of y can be 00829 * 250, 500, 1000 or 2000. 00830 * 00831 * @return 00832 * Returns zero on OK, non-zero otherwise 00833 ******************************************************************************/ 00834 uint32_t ICM20648::set_gyro_fullscale(uint8_t gyroFs) 00835 { 00836 uint8_t reg; 00837 00838 gyroFs &= ICM20648_MASK_GYRO_FULLSCALE; 00839 read_register(ICM20648_REG_GYRO_CONFIG_1, 1, ®); 00840 reg &= ~(ICM20648_MASK_GYRO_FULLSCALE); 00841 reg |= gyroFs; 00842 write_register(ICM20648_REG_GYRO_CONFIG_1, reg); 00843 00844 return ICM20648_OK; 00845 } 00846 00847 /***************************************************************************//** 00848 * @brief 00849 * Enables or disables the sleep mode of the device 00850 * 00851 * @param[in] enable 00852 * If true, sleep mode is enabled. Set to false to disable sleep mode. 00853 * 00854 * @return 00855 * Returns zero on OK, non-zero otherwise 00856 ******************************************************************************/ 00857 uint32_t ICM20648::enable_sleepmode(bool enable) 00858 { 00859 uint8_t reg; 00860 00861 read_register(ICM20648_REG_PWR_MGMT_1, 1, ®); 00862 00863 if ( enable ) { 00864 /* Sleep: set the SLEEP bit */ 00865 reg |= ICM20648_BIT_SLEEP; 00866 } else { 00867 /* Wake up: clear the SLEEP bit */ 00868 reg &= ~(ICM20648_BIT_SLEEP); 00869 } 00870 00871 write_register(ICM20648_REG_PWR_MGMT_1, reg); 00872 00873 return ICM20648_OK; 00874 } 00875 00876 /***************************************************************************//** 00877 * @brief 00878 * Enables or disables the cycle mode operation of the accel and gyro 00879 * 00880 * @param[in] enable 00881 * If true both the accel and gyro sensors will operate in cycle mode. If 00882 * false the senors working in continuous mode. 00883 * 00884 * @return 00885 * Returns zero on OK, non-zero otherwise 00886 ******************************************************************************/ 00887 uint32_t ICM20648::enable_cyclemode(bool enable) 00888 { 00889 uint8_t reg; 00890 00891 reg = 0x00; 00892 00893 if ( enable ) { 00894 reg = ICM20648_BIT_ACCEL_CYCLE | ICM20648_BIT_GYRO_CYCLE; 00895 } 00896 00897 write_register(ICM20648_REG_LP_CONFIG, reg); 00898 00899 return ICM20648_OK; 00900 } 00901 00902 /***************************************************************************//** 00903 * @brief 00904 * Enables or disables the sensors in the ICM20648 chip 00905 * 00906 * @param[in] accel 00907 * If true enables the acceleration sensor 00908 * 00909 * @param[in] gyro 00910 * If true enables the gyroscope sensor 00911 * 00912 * @param[in] temp 00913 * If true enables the temperature sensor 00914 * 00915 * @return 00916 * Returns zero on OK, non-zero otherwise 00917 ******************************************************************************/ 00918 uint32_t ICM20648::enable_sensor(bool accel, bool gyro, bool temp) 00919 { 00920 uint8_t pwrManagement1; 00921 uint8_t pwrManagement2; 00922 00923 read_register(ICM20648_REG_PWR_MGMT_1, 1, &pwrManagement1); 00924 pwrManagement2 = 0; 00925 00926 /* To enable the accelerometer clear the DISABLE_ACCEL bits in PWR_MGMT_2 */ 00927 if ( accel ) { 00928 pwrManagement2 &= ~(ICM20648_BIT_PWR_ACCEL_STBY); 00929 } else { 00930 pwrManagement2 |= ICM20648_BIT_PWR_ACCEL_STBY; 00931 } 00932 00933 /* To enable gyro clear the DISABLE_GYRO bits in PWR_MGMT_2 */ 00934 if ( gyro ) { 00935 pwrManagement2 &= ~(ICM20648_BIT_PWR_GYRO_STBY); 00936 } else { 00937 pwrManagement2 |= ICM20648_BIT_PWR_GYRO_STBY; 00938 } 00939 00940 /* To enable the temperature sensor clear the TEMP_DIS bit in PWR_MGMT_1 */ 00941 if ( temp ) { 00942 pwrManagement1 &= ~(ICM20648_BIT_TEMP_DIS); 00943 } else { 00944 pwrManagement1 |= ICM20648_BIT_TEMP_DIS; 00945 } 00946 00947 /* Write back the modified values */ 00948 write_register(ICM20648_REG_PWR_MGMT_1, pwrManagement1); 00949 write_register(ICM20648_REG_PWR_MGMT_2, pwrManagement2); 00950 00951 return ICM20648_OK; 00952 } 00953 00954 /***************************************************************************//** 00955 * @brief 00956 * Enables or disables the sensors in low power mode in the ICM20648 chip 00957 * 00958 * @param[in] enAccel 00959 * If true enables the acceleration sensor in low power mode 00960 * 00961 * @param[in] enGyro 00962 * If true enables the gyroscope sensor in low power mode 00963 * 00964 * @param[in] enTemp 00965 * If true enables the temperature sensor in low power mode 00966 * 00967 * @return 00968 * Returns zero on OK, non-zero otherwise 00969 ******************************************************************************/ 00970 uint32_t ICM20648::enter_lowpowermode(bool enAccel, bool enGyro, bool enTemp) 00971 { 00972 uint8_t data; 00973 00974 read_register(ICM20648_REG_PWR_MGMT_1, 1, &data); 00975 00976 if ( enAccel || enGyro || enTemp ) { 00977 /* Make sure that the chip is not in sleep */ 00978 enable_sleepmode(false); 00979 00980 /* And in continuous mode */ 00981 enable_cyclemode(false); 00982 00983 /* Enable the accelerometer and the gyroscope*/ 00984 enable_sensor(enAccel, enGyro, enTemp); 00985 wait_ms(50); 00986 00987 /* Enable cycle mode */ 00988 enable_cyclemode(true); 00989 00990 /* Set the LP_EN bit to enable low power mode */ 00991 data |= ICM20648_BIT_LP_EN; 00992 } else { 00993 /* Enable continuous mode */ 00994 enable_cyclemode(false); 00995 00996 /* Clear the LP_EN bit to disable low power mode */ 00997 data &= ~ICM20648_BIT_LP_EN; 00998 } 00999 01000 /* Write the updated value to the PWR_MGNT_1 register */ 01001 write_register(ICM20648_REG_PWR_MGMT_1, data); 01002 01003 return ICM20648_OK; 01004 } 01005 01006 /***************************************************************************//** 01007 * @brief 01008 * Enables or disables the interrupts in the ICM20648 chip 01009 * 01010 * @param[in] dataReadyEnable 01011 * If true enables the Raw Data Ready interrupt, otherwise disables. 01012 * 01013 * @param[in] womEnable 01014 * If true enables the Wake-up On Motion interrupt, otherwise disables. 01015 * 01016 * @return 01017 * Returns zero on OK, non-zero otherwise 01018 ******************************************************************************/ 01019 uint32_t ICM20648::enable_irq(bool dataReadyEnable, bool womEnable) 01020 { 01021 uint8_t intEnable; 01022 01023 /* All interrupts disabled by default */ 01024 intEnable = 0; 01025 01026 /* Enable one or both of the interrupt sources if required */ 01027 if ( womEnable ) { 01028 intEnable = ICM20648_BIT_WOM_INT_EN; 01029 } 01030 /* Write value to register */ 01031 write_register(ICM20648_REG_INT_ENABLE, intEnable); 01032 01033 /* All interrupts disabled by default */ 01034 intEnable = 0; 01035 01036 if ( dataReadyEnable ) { 01037 intEnable = ICM20648_BIT_RAW_DATA_0_RDY_EN; 01038 } 01039 01040 /* Write value to register */ 01041 write_register(ICM20648_REG_INT_ENABLE_1, intEnable); 01042 01043 return ICM20648_OK; 01044 } 01045 01046 /***************************************************************************//** 01047 * @brief 01048 * Reads the interrupt status registers of the ICM20648 chip 01049 * 01050 * @param[out] intStatus 01051 * The content the four interrupt registers. LSByte is INT_STATUS, MSByte is 01052 * INT_STATUS_3 01053 * 01054 * @return 01055 * Returns zero on OK, non-zero otherwise 01056 ******************************************************************************/ 01057 uint32_t ICM20648::read_irqstatus(uint32_t *int_status) 01058 { 01059 uint8_t reg[4]; 01060 01061 read_register(ICM20648_REG_INT_STATUS, 4, reg); 01062 *int_status = (uint32_t) reg[0]; 01063 *int_status |= ( ( (uint32_t) reg[1]) << 8); 01064 *int_status |= ( ( (uint32_t) reg[2]) << 16); 01065 *int_status |= ( ( (uint32_t) reg[3]) << 24); 01066 01067 return ICM20648_OK; 01068 } 01069 01070 /***************************************************************************//** 01071 * @brief 01072 * Checks if new data is available for read 01073 * 01074 * @return 01075 * Returns true if the Raw Data Ready interrupt bit set, false otherwise 01076 ******************************************************************************/ 01077 bool ICM20648::is_data_ready(void) 01078 { 01079 uint8_t status; 01080 bool ret; 01081 01082 ret = false; 01083 read_register(ICM20648_REG_INT_STATUS_1, 1, &status); 01084 01085 if ( status & ICM20648_BIT_RAW_DATA_0_RDY_INT ) { 01086 ret = true; 01087 } 01088 01089 return ret; 01090 } 01091 01092 /***************************************************************************//** 01093 * @brief 01094 * Sets up and enables the Wake-up On Motion feature 01095 * 01096 * @param[in] enable 01097 * If true enables the WOM feature, disables otherwise 01098 * 01099 * @param[in] womThreshold 01100 * Threshold value for the Wake on Motion Interrupt for ACCEL x/y/z axes. 01101 * LSB = 4mg. Range is 0mg to 1020mg 01102 * 01103 * @param[in] sampleRate 01104 * The desired sample rate of the accel sensor in Hz 01105 * 01106 * @return 01107 * Returns zero on OK, non-zero otherwise 01108 ******************************************************************************/ 01109 uint32_t ICM20648::enable_wake_on_motion(bool enable, uint8_t womThreshold, float sampleRate) 01110 { 01111 if ( enable ) { 01112 /* Make sure that the chip is not in sleep */ 01113 enable_sleepmode(false); 01114 01115 /* And in continuous mode */ 01116 enable_cyclemode(false); 01117 01118 /* Enable only the accelerometer */ 01119 enable_sensor(true, false, false); 01120 01121 /* Set sample rate */ 01122 set_sample_rate(sampleRate); 01123 01124 /* Set the bandwidth to 1210Hz */ 01125 set_accel_bandwidth(ICM20648_ACCEL_BW_1210HZ); 01126 01127 /* Accel: 2G full scale */ 01128 set_accel_fullscale(ICM20648_ACCEL_FULLSCALE_2G); 01129 01130 /* Enable the Wake On Motion interrupt */ 01131 enable_irq(false, true); 01132 wait_ms(50); 01133 01134 /* Enable Wake On Motion feature */ 01135 write_register(ICM20648_REG_ACCEL_INTEL_CTRL, ICM20648_BIT_ACCEL_INTEL_EN | ICM20648_BIT_ACCEL_INTEL_MODE); 01136 01137 /* Set the wake on motion threshold value */ 01138 write_register(ICM20648_REG_ACCEL_WOM_THR, womThreshold); 01139 01140 /* Enable low power mode */ 01141 enter_lowpowermode(true, false, false); 01142 } else { 01143 /* Disable Wake On Motion feature */ 01144 write_register(ICM20648_REG_ACCEL_INTEL_CTRL, 0x00); 01145 01146 /* Disable the Wake On Motion interrupt */ 01147 enable_irq(false, false); 01148 01149 /* Disable cycle mode */ 01150 enable_cyclemode(false); 01151 } 01152 01153 return ICM20648_OK; 01154 } 01155 01156 /***************************************************************************//** 01157 * @brief 01158 * Accelerometer and gyroscope calibration function. Reads the gyroscope 01159 * and accelerometer values while the device is at rest and in level. The 01160 * resulting values are loaded to the accel and gyro bias registers to cancel 01161 * the static offset error. 01162 * 01163 * @param[out] accelBiasScaled 01164 * The mesured acceleration sensor bias in mg 01165 * 01166 * @param[out] gyroBiasScaled 01167 * The mesured gyro sensor bias in deg/sec 01168 * 01169 * @return 01170 * Returns zero on OK, non-zero otherwise 01171 ******************************************************************************/ 01172 uint32_t ICM20648::calibrate(float *accelBiasScaled, float *gyroBiasScaled) 01173 { 01174 uint8_t data[12]; 01175 uint16_t i, packetCount, fifoCount; 01176 int32_t gyroBias[3] = { 0, 0, 0 }; 01177 int32_t accelBias[3] = { 0, 0, 0 }; 01178 int32_t accelTemp[3]; 01179 int32_t gyroTemp[3]; 01180 int32_t accelBiasFactory[3]; 01181 int32_t gyroBiasStored[3]; 01182 float gyroRes, accelRes; 01183 01184 /* Enable the accelerometer and the gyro */ 01185 enable_sensor(true, true, false); 01186 01187 /* Set 1kHz sample rate */ 01188 set_sample_rate(1100.0); 01189 01190 /* 246Hz BW for the accelerometer and 200Hz for the gyroscope */ 01191 set_accel_bandwidth(ICM20648_ACCEL_BW_246HZ); 01192 set_gyro_bandwidth(ICM20648_GYRO_BW_12HZ); 01193 01194 /* Set the most sensitive range: 2G full scale and 250dps full scale */ 01195 set_accel_fullscale(ICM20648_ACCEL_FULLSCALE_2G); 01196 set_gyro_fullscale(ICM20648_GYRO_FULLSCALE_250DPS); 01197 01198 /* Retrieve the resolution per bit */ 01199 get_accel_resolution(&accelRes); 01200 get_gyro_resolution(&gyroRes); 01201 01202 /* The accel sensor needs max 30ms, the gyro max 35ms to fully start */ 01203 /* Experiments show that the gyro needs more time to get reliable results */ 01204 wait_ms(50); 01205 01206 /* Disable the FIFO */ 01207 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN); 01208 write_register(ICM20648_REG_FIFO_MODE, 0x0F); 01209 01210 /* Enable accelerometer and gyro to store the data in FIFO */ 01211 write_register(ICM20648_REG_FIFO_EN_2, ICM20648_BIT_ACCEL_FIFO_EN | ICM20648_BITS_GYRO_FIFO_EN); 01212 01213 /* Reset the FIFO */ 01214 write_register(ICM20648_REG_FIFO_RST, 0x0F); 01215 write_register(ICM20648_REG_FIFO_RST, 0x00); 01216 01217 /* Enable the FIFO */ 01218 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN); 01219 01220 /* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */ 01221 /* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */ 01222 /* Loop until at least 4080 samples gathered */ 01223 fifoCount = 0; 01224 while ( fifoCount < 4080 ) { 01225 wait_ms(5); 01226 /* Read FIFO sample count */ 01227 read_register(ICM20648_REG_FIFO_COUNT_H, 2, &data[0]); 01228 /* Convert to a 16 bit value */ 01229 fifoCount = ( (uint16_t) (data[0] << 8) | data[1]); 01230 } 01231 01232 /* Disable accelerometer and gyro to store the data in FIFO */ 01233 write_register(ICM20648_REG_FIFO_EN_2, 0x00); 01234 01235 /* Read FIFO sample count */ 01236 read_register(ICM20648_REG_FIFO_COUNT_H, 2, &data[0]); 01237 01238 /* Convert to a 16 bit value */ 01239 fifoCount = ( (uint16_t) (data[0] << 8) | data[1]); 01240 01241 /* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */ 01242 packetCount = fifoCount / 12; 01243 01244 /* Retrieve the data from the FIFO */ 01245 for ( i = 0; i < packetCount; i++ ) { 01246 read_register(ICM20648_REG_FIFO_R_W, 12, &data[0]); 01247 /* Convert to 16 bit signed accel and gyro x,y and z values */ 01248 accelTemp[0] = ( (int16_t) (data[0] << 8) | data[1]); 01249 accelTemp[1] = ( (int16_t) (data[2] << 8) | data[3]); 01250 accelTemp[2] = ( (int16_t) (data[4] << 8) | data[5]); 01251 gyroTemp[0] = ( (int16_t) (data[6] << 8) | data[7]); 01252 gyroTemp[1] = ( (int16_t) (data[8] << 8) | data[9]); 01253 gyroTemp[2] = ( (int16_t) (data[10] << 8) | data[11]); 01254 01255 /* Sum the values */ 01256 accelBias[0] += accelTemp[0]; 01257 accelBias[1] += accelTemp[1]; 01258 accelBias[2] += accelTemp[2]; 01259 gyroBias[0] += gyroTemp[0]; 01260 gyroBias[1] += gyroTemp[1]; 01261 gyroBias[2] += gyroTemp[2]; 01262 } 01263 01264 /* Divide by packet count to get the average */ 01265 accelBias[0] /= packetCount; 01266 accelBias[1] /= packetCount; 01267 accelBias[2] /= packetCount; 01268 gyroBias[0] /= packetCount; 01269 gyroBias[1] /= packetCount; 01270 gyroBias[2] /= packetCount; 01271 01272 /* Acceleormeter: add or remove (depending on the orientation of the chip) 1G (gravity) from the Z axis value */ 01273 if ( accelBias[2] > 0L ) { 01274 accelBias[2] -= (int32_t) (1.0 / accelRes); 01275 } else { 01276 accelBias[2] += (int32_t) (1.0 / accelRes); 01277 } 01278 01279 /* Convert the values to degrees per sec for displaying */ 01280 gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes; 01281 gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes; 01282 gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes; 01283 01284 /* Read stored gyro trim values. After reset these values are all 0 */ 01285 read_register(ICM20648_REG_XG_OFFS_USRH, 2, &data[0]); 01286 gyroBiasStored[0] = ( (int16_t) (data[0] << 8) | data[1]); 01287 read_register(ICM20648_REG_YG_OFFS_USRH, 2, &data[0]); 01288 gyroBiasStored[1] = ( (int16_t) (data[0] << 8) | data[1]); 01289 read_register(ICM20648_REG_ZG_OFFS_USRH, 2, &data[0]); 01290 gyroBiasStored[2] = ( (int16_t) (data[0] << 8) | data[1]); 01291 01292 /* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */ 01293 /* the best sensitivity, so need to divide by 4 */ 01294 /* Substract from the stored calibration value */ 01295 gyroBiasStored[0] -= gyroBias[0] / 4; 01296 gyroBiasStored[1] -= gyroBias[1] / 4; 01297 gyroBiasStored[2] -= gyroBias[2] / 4; 01298 01299 /* Split the values into two bytes */ 01300 data[0] = (gyroBiasStored[0] >> 8) & 0xFF; 01301 data[1] = (gyroBiasStored[0]) & 0xFF; 01302 data[2] = (gyroBiasStored[1] >> 8) & 0xFF; 01303 data[3] = (gyroBiasStored[1]) & 0xFF; 01304 data[4] = (gyroBiasStored[2] >> 8) & 0xFF; 01305 data[5] = (gyroBiasStored[2]) & 0xFF; 01306 01307 /* Write the gyro bias values to the chip */ 01308 write_register(ICM20648_REG_XG_OFFS_USRH, data[0]); 01309 write_register(ICM20648_REG_XG_OFFS_USRL, data[1]); 01310 write_register(ICM20648_REG_YG_OFFS_USRH, data[2]); 01311 write_register(ICM20648_REG_YG_OFFS_USRL, data[3]); 01312 write_register(ICM20648_REG_ZG_OFFS_USRH, data[4]); 01313 write_register(ICM20648_REG_ZG_OFFS_USRL, data[5]); 01314 01315 /* Calculate the accelerometer bias values to store in the hardware accelerometer bias registers. These registers contain */ 01316 /* factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold */ 01317 /* non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature */ 01318 /* compensation calculations(? the datasheet is not clear). Accelerometer bias registers expect bias input */ 01319 /* as 2048 LSB per g, so that the accelerometer biases calculated above must be divided by 8. */ 01320 01321 /* Read factory accelerometer trim values */ 01322 read_register(ICM20648_REG_XA_OFFSET_H, 2, &data[0]); 01323 accelBiasFactory[0] = ( (int16_t) (data[0] << 8) | data[1]); 01324 read_register(ICM20648_REG_YA_OFFSET_H, 2, &data[0]); 01325 accelBiasFactory[1] = ( (int16_t) (data[0] << 8) | data[1]); 01326 read_register(ICM20648_REG_ZA_OFFSET_H, 2, &data[0]); 01327 accelBiasFactory[2] = ( (int16_t) (data[0] << 8) | data[1]); 01328 01329 /* Construct total accelerometer bias, including calculated average accelerometer bias from above */ 01330 /* Scale the 2g full scale (most sensitive range) results to 16g full scale - divide by 8 */ 01331 /* Clear the last bit (temperature compensation? - the datasheet is not clear) */ 01332 /* Substract from the factory calibration value */ 01333 01334 accelBiasFactory[0] -= ( (accelBias[0] / 8) & ~1); 01335 accelBiasFactory[1] -= ( (accelBias[1] / 8) & ~1); 01336 accelBiasFactory[2] -= ( (accelBias[2] / 8) & ~1); 01337 01338 /* Split the values into two bytes */ 01339 data[0] = (accelBiasFactory[0] >> 8) & 0xFF; 01340 data[1] = (accelBiasFactory[0]) & 0xFF; 01341 data[2] = (accelBiasFactory[1] >> 8) & 0xFF; 01342 data[3] = (accelBiasFactory[1]) & 0xFF; 01343 data[4] = (accelBiasFactory[2] >> 8) & 0xFF; 01344 data[5] = (accelBiasFactory[2]) & 0xFF; 01345 01346 /* Store them in the accelerometer offset registers */ 01347 write_register(ICM20648_REG_XA_OFFSET_H, data[0]); 01348 write_register(ICM20648_REG_XA_OFFSET_L, data[1]); 01349 write_register(ICM20648_REG_YA_OFFSET_H, data[2]); 01350 write_register(ICM20648_REG_YA_OFFSET_L, data[3]); 01351 write_register(ICM20648_REG_ZA_OFFSET_H, data[4]); 01352 write_register(ICM20648_REG_ZA_OFFSET_L, data[5]); 01353 01354 /* Convert the values to G for displaying */ 01355 accelBiasScaled[0] = (float) accelBias[0] * accelRes; 01356 accelBiasScaled[1] = (float) accelBias[1] * accelRes; 01357 accelBiasScaled[2] = (float) accelBias[2] * accelRes; 01358 01359 /* Turn off FIFO */ 01360 write_register(ICM20648_REG_USER_CTRL, 0x00); 01361 01362 /* Disable all sensors */ 01363 enable_sensor(false, false, false); 01364 01365 return ICM20648_OK; 01366 } 01367 01368 /***************************************************************************//** 01369 * @brief 01370 * Gyroscope calibration function. Reads the gyroscope 01371 * values while the device is at rest and in level. The 01372 * resulting values are loaded to the gyro bias registers to cancel 01373 * the static offset error. 01374 * 01375 * @param[out] gyroBiasScaled 01376 * The mesured gyro sensor bias in deg/sec 01377 * 01378 * @return 01379 * Returns zero on OK, non-zero otherwise 01380 ******************************************************************************/ 01381 uint32_t ICM20648::calibrate_gyro(float *gyroBiasScaled) 01382 { 01383 uint8_t data[12]; 01384 uint16_t i, packetCount, fifoCount; 01385 int32_t gyroBias[3] = { 0, 0, 0 }; 01386 int32_t gyroTemp[3]; 01387 int32_t gyroBiasStored[3]; 01388 float gyroRes; 01389 01390 /* Enable the accelerometer and the gyro */ 01391 enable_sensor(true, true, false); 01392 01393 /* Set 1kHz sample rate */ 01394 set_sample_rate(1100.0); 01395 01396 /* Configure bandwidth for gyroscope to 12Hz */ 01397 set_gyro_bandwidth(ICM20648_GYRO_BW_12HZ); 01398 01399 /* Configure sensitivity to 250dps full scale */ 01400 set_gyro_fullscale(ICM20648_GYRO_FULLSCALE_250DPS); 01401 01402 /* Retrieve the resolution per bit */ 01403 get_gyro_resolution(&gyroRes); 01404 01405 /* The accel sensor needs max 30ms, the gyro max 35ms to fully start */ 01406 /* Experiments show that the gyro needs more time to get reliable results */ 01407 wait_ms(50); 01408 01409 /* Disable the FIFO */ 01410 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN); 01411 write_register(ICM20648_REG_FIFO_MODE, 0x0F); 01412 01413 /* Enable accelerometer and gyro to store the data in FIFO */ 01414 write_register(ICM20648_REG_FIFO_EN_2, ICM20648_BITS_GYRO_FIFO_EN); 01415 01416 /* Reset the FIFO */ 01417 write_register(ICM20648_REG_FIFO_RST, 0x0F); 01418 write_register(ICM20648_REG_FIFO_RST, 0x00); 01419 01420 /* Enable the FIFO */ 01421 write_register(ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN); 01422 01423 /* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */ 01424 /* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */ 01425 /* Loop until at least 4080 samples gathered */ 01426 fifoCount = 0; 01427 while ( fifoCount < 4080 ) { 01428 wait_ms(5); 01429 01430 /* Read FIFO sample count */ 01431 read_register(ICM20648_REG_FIFO_COUNT_H, 2, &data[0]); 01432 01433 /* Convert to a 16 bit value */ 01434 fifoCount = ( (uint16_t) (data[0] << 8) | data[1]); 01435 } 01436 01437 /* Disable accelerometer and gyro to store the data in FIFO */ 01438 write_register(ICM20648_REG_FIFO_EN_2, 0x00); 01439 01440 /* Read FIFO sample count */ 01441 read_register(ICM20648_REG_FIFO_COUNT_H, 2, &data[0]); 01442 01443 /* Convert to a 16 bit value */ 01444 fifoCount = ( (uint16_t) (data[0] << 8) | data[1]); 01445 01446 /* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */ 01447 packetCount = fifoCount / 12; 01448 01449 /* Retrieve the data from the FIFO */ 01450 for ( i = 0; i < packetCount; i++ ) { 01451 read_register(ICM20648_REG_FIFO_R_W, 12, &data[0]); 01452 /* Convert to 16 bit signed accel and gyro x,y and z values */ 01453 gyroTemp[0] = ( (int16_t) (data[6] << 8) | data[7]); 01454 gyroTemp[1] = ( (int16_t) (data[8] << 8) | data[9]); 01455 gyroTemp[2] = ( (int16_t) (data[10] << 8) | data[11]); 01456 01457 /* Sum the values */ 01458 gyroBias[0] += gyroTemp[0]; 01459 gyroBias[1] += gyroTemp[1]; 01460 gyroBias[2] += gyroTemp[2]; 01461 } 01462 01463 /* Divide by packet count to get the average */ 01464 gyroBias[0] /= packetCount; 01465 gyroBias[1] /= packetCount; 01466 gyroBias[2] /= packetCount; 01467 01468 /* Convert the values to degrees per sec for displaying */ 01469 gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes; 01470 gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes; 01471 gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes; 01472 01473 /* Read stored gyro trim values. After reset these values are all 0 */ 01474 read_register(ICM20648_REG_XG_OFFS_USRH, 2, &data[0]); 01475 gyroBiasStored[0] = ( (int16_t) (data[0] << 8) | data[1]); 01476 01477 read_register(ICM20648_REG_YG_OFFS_USRH, 2, &data[0]); 01478 gyroBiasStored[1] = ( (int16_t) (data[0] << 8) | data[1]); 01479 01480 read_register(ICM20648_REG_ZG_OFFS_USRH, 2, &data[0]); 01481 gyroBiasStored[2] = ( (int16_t) (data[0] << 8) | data[1]); 01482 01483 /* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */ 01484 /* the best sensitivity, so need to divide by 4 */ 01485 /* Substract from the stored calibration value */ 01486 gyroBiasStored[0] -= gyroBias[0] / 4; 01487 gyroBiasStored[1] -= gyroBias[1] / 4; 01488 gyroBiasStored[2] -= gyroBias[2] / 4; 01489 01490 /* Split the values into two bytes */ 01491 data[0] = (gyroBiasStored[0] >> 8) & 0xFF; 01492 data[1] = (gyroBiasStored[0]) & 0xFF; 01493 data[2] = (gyroBiasStored[1] >> 8) & 0xFF; 01494 data[3] = (gyroBiasStored[1]) & 0xFF; 01495 data[4] = (gyroBiasStored[2] >> 8) & 0xFF; 01496 data[5] = (gyroBiasStored[2]) & 0xFF; 01497 01498 /* Write the gyro bias values to the chip */ 01499 write_register(ICM20648_REG_XG_OFFS_USRH, data[0]); 01500 write_register(ICM20648_REG_XG_OFFS_USRL, data[1]); 01501 write_register(ICM20648_REG_YG_OFFS_USRH, data[2]); 01502 write_register(ICM20648_REG_YG_OFFS_USRL, data[3]); 01503 write_register(ICM20648_REG_ZG_OFFS_USRH, data[4]); 01504 write_register(ICM20648_REG_ZG_OFFS_USRL, data[5]); 01505 01506 /* Turn off FIFO */ 01507 write_register(ICM20648_REG_USER_CTRL, 0x00); 01508 01509 /* Disable all sensors */ 01510 enable_sensor(false, false, false); 01511 01512 return ICM20648_OK; 01513 } 01514 01515 /***************************************************************************//** 01516 * @brief 01517 * Reads the temperature sensor raw value and converts to Celsius. 01518 * 01519 * @param[out] temperature 01520 * The mesured temperature in Celsius 01521 * 01522 * @return 01523 * Returns zero on OK, non-zero otherwise 01524 ******************************************************************************/ 01525 uint32_t ICM20648::read_temperature(float *temperature) 01526 { 01527 uint8_t data[2]; 01528 int16_t raw_temp; 01529 01530 /* Read temperature registers */ 01531 read_register(ICM20648_REG_TEMPERATURE_H, 2, data); 01532 01533 /* Convert to int16 */ 01534 raw_temp = (int16_t) ( (data[0] << 8) + data[1]); 01535 01536 /* Calculate the Celsius value from the raw reading */ 01537 *temperature = ( (float) raw_temp / 333.87) + 21.0; 01538 01539 return ICM20648_OK; 01540 } 01541 01542 /***************************************************************************//** 01543 * @brief 01544 * Reads the device ID of the ICM20648 01545 * 01546 * @param[out] devID 01547 * The ID of the device read from teh WHO_AM_I register. Expected value? 0xE0 01548 * 01549 * @return 01550 * Returns zero on OK, non-zero otherwise 01551 ******************************************************************************/ 01552 uint32_t ICM20648::get_device_id(uint8_t *device_id) 01553 { 01554 read_register(ICM20648_REG_WHO_AM_I, 1, device_id); 01555 01556 return ICM20648_OK; 01557 } 01558 01559 void ICM20648::irq_handler(void) 01560 { 01561 01562 }
Generated on Sat Jul 16 2022 11:18:59 by
1.7.2