Hello. I am using MPU6500. I work on wearable technology. I get normal data from the sensor at first. Then, as you start jumping and running, the values are reversed or very different values appear. What is the reason for this issue? Would you help me with this topic?
Hello,
May I ask for more information about your setup, and firmware you are using with it?
Hello. We wrote the software by revising the addresses and some commands from the ICM software. Don't get hung up on the naming. Our .c and .h files are attached. Do you have a suggestion about this?
Files
Unfortunately I can't upload documents here.
/***************************************************************************/
#include "icm20648.h"
#include "nrf_delay.h"
/**************************************************************************/
/**************************************************************************/
static void ICM20648_chipSelectSet( bool select );
void UTIL_delay(int ms)
{
unsigned long i;
for(i=0;i<(ms);i++) nrf_delay_us(1000);
}
uint8_t data;
/***************************************************************************/
uint32_t ICM20648_init( void )
{
I2CSoftInit();
/* Issue reset */
ICM20648_reset();
/* Read Who am I register, should get 0x71 */
ICM20648_registerRead( ICM20648_REG_WHO_AM_I, 1, &data );
/*printf( "Who am I: 0x%x\r\n", data );*/
/* If not - return */
if( data != ICM20648_DEVICE_ID ) {
return ICM20648_ERROR_INVALID_DEVICE_ID;
}
/* Auto selects the best available clock source ? PLL if ready, else use the Internal oscillator */
ICM20648_registerWrite( ICM20648_REG_PWR_MGMT_1, ICM20648_BIT_CLK_PLL );
ICM20648_registerRead( ICM20648_REG_PWR_MGMT_1, 1, &data );
/* PLL startup time - maybe it is too long but better be on the safe side, no spec in the datasheet */
UTIL_delay( 30 );
/* 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 */
ICM20648_registerWrite( ICM20648_REG_INT_PIN_CFG, ICM20648_BIT_INT_ACTL | ICM20648_BIT_INT_OPEN );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_deInit( void )
{
/* Disable the chip */
//**BOARD_imuEnable( false );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_reset( void )
{
/* Set H_RESET bit to initiate soft reset */
ICM20648_registerWrite( ICM20648_REG_PWR_MGMT_1, ICM20648_BIT_H_RESET );
/* Wait 100ms to complete the reset sequence */
UTIL_delay( 100 );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_accelDataRead( float *accel )
{
uint8_t rawData[6];
float accelRes;
int16_t temp;
/* Retrieve the current resolution */
ICM20648_accelResolutionGet( &accelRes );
/* Read the six raw data registers into data array */
ICM20648_registerRead( ICM20648_REG_ACCEL_XOUT_H_SH, 6, &rawData[0] );
/* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the G value */
temp = ( (int16_t) rawData[0] << 8 ) | rawData[1];
accel[0] = (float) temp * accelRes;
temp = ( (int16_t) rawData[2] << 8 ) | rawData[3];
accel[1] = (float) temp * accelRes;
temp = ( (int16_t) rawData[4] << 8 ) | rawData[5];
accel[2] = (float) temp * accelRes;
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_accelRawDataRead( uint8_t *accel )
{
uint8_t rawData[6];
float accelRes;
int16_t temp;
/* Retrieve the current resolution */
ICM20648_accelResolutionGet( &accelRes );
/* Read the six raw data registers into data array */
ICM20648_registerRead( ICM20648_REG_ACCEL_XOUT_H_SH, 6, &rawData[0] );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_gyroDataRead( float *gyro )
{
uint8_t rawData[6];
float gyroRes;
int16_t temp;
/* Retrieve the current resolution */
ICM20648_gyroResolutionGet( &gyroRes );
/* Read the six raw data registers into data array */
ICM20648_registerRead( ICM20648_REG_GYRO_XOUT_H_SH, 6, &rawData[0] );
/* Convert the MSB and LSB into a signed 16-bit value and multiply by the resolution to get the dps value */
temp = ( (int16_t) rawData[0] << 8 ) | rawData[1];
gyro[0] = (float) temp * gyroRes;
temp = ( (int16_t) rawData[2] << 8 ) | rawData[3];
gyro[1] = (float) temp * gyroRes;
temp = ( (int16_t) rawData[4] << 8 ) | rawData[5];
gyro[2] = (float) temp * gyroRes;
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_gyroRawDataRead( uint8_t *gyro )
{
uint8_t rawData[6];
float gyroRes;
int16_t temp;
/* Retrieve the current resolution */
ICM20648_gyroResolutionGet( &gyroRes );
/* Read the six raw data registers into data array */
ICM20648_registerRead( ICM20648_REG_GYRO_XOUT_H_SH, 6, &rawData[0] );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_accelResolutionGet( float *accelRes )
{
uint8_t reg;
/* Read the actual acceleration full scale setting */
ICM20648_registerRead( ICM20648_REG_ACCEL_CONFIG, 1, ® );
reg &= ICM20648_MASK_ACCEL_FULLSCALE;
/* Calculate the resolution */
switch( reg ) {
case ICM20648_ACCEL_FULLSCALE_2G:
*accelRes = 2.0 / 32768.0;
break;
case ICM20648_ACCEL_FULLSCALE_4G:
*accelRes = 4.0 / 32768.0;
break;
case ICM20648_ACCEL_FULLSCALE_8G:
*accelRes = 8.0 / 32768.0;
break;
case ICM20648_ACCEL_FULLSCALE_16G:
*accelRes = 16.0 / 32768.0;
break;
}
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_gyroResolutionGet( float *gyroRes )
{
uint8_t reg;
/* Read the actual gyroscope full scale setting */
ICM20648_registerRead( ICM20648_REG_GYRO_CONFIG_1, 1, ® );
reg &= ICM20648_MASK_GYRO_FULLSCALE;
/* Calculate the resolution */
switch( reg ) {
case ICM20648_GYRO_FULLSCALE_250DPS:
*gyroRes = 250.0 / 32768.0;
break;
case ICM20648_GYRO_FULLSCALE_500DPS:
*gyroRes = 500.0 / 32768.0;
break;
case ICM20648_GYRO_FULLSCALE_1000DPS:
*gyroRes = 1000.0 / 32768.0;
break;
case ICM20648_GYRO_FULLSCALE_2000DPS:
*gyroRes = 2000.0 / 32768.0;
break;
}
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_accelFullscaleSet( uint8_t accelFs )
{
uint8_t reg;
accelFs &= ICM20648_MASK_ACCEL_FULLSCALE;
ICM20648_registerRead( ICM20648_REG_ACCEL_CONFIG, 1, ® );
reg &= ~( ICM20648_MASK_ACCEL_FULLSCALE );
reg |= accelFs;
ICM20648_registerWrite( ICM20648_REG_ACCEL_CONFIG, reg );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_gyroFullscaleSet( uint8_t gyroFs )
{
uint8_t reg;
gyroFs &= ICM20648_MASK_GYRO_FULLSCALE;
ICM20648_registerRead( ICM20648_REG_GYRO_CONFIG_1, 1, ® );
reg &= ~( ICM20648_MASK_GYRO_FULLSCALE );
reg |= gyroFs;
ICM20648_registerWrite( ICM20648_REG_GYRO_CONFIG_1, reg );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_sampleRateSet( float sampleRate )
{
ICM20648_gyroSampleRateSet( sampleRate );
ICM20648_accelSampleRateSet( sampleRate );
return ICM20648_OK;
}
/***************************************************************************/
float ICM20648_gyroSampleRateSet( float sampleRate )
{
uint8_t gyroDiv;
float gyroSampleRate;
/* Calculate the sample rate divider */
gyroSampleRate = ( 1125.0 / sampleRate ) - 1.0;
/* Check if it fits in the divider register */
if( gyroSampleRate > 255.0 ) {
gyroSampleRate = 255.0;
}
if( gyroSampleRate < 0 ) {
gyroSampleRate = 0.0;
}
/* Write the value to the register */
gyroDiv = (uint8_t) gyroSampleRate;
ICM20648_registerWrite( ICM20648_REG_GYRO_SMPLRT_DIV, gyroDiv );
/* Calculate the actual sample rate from the divider value */
gyroSampleRate = 1125.0 / ( gyroDiv + 1 );
return gyroSampleRate;
}
/***************************************************************************/
float ICM20648_accelSampleRateSet( float sampleRate )
{
uint16_t accelDiv;
float accelSampleRate;
/* Calculate the sample rate divider */
accelSampleRate = ( 1125.0 / sampleRate ) - 1.0;
/* Check if it fits in the divider registers */
if( accelSampleRate > 4095.0 ) {
accelSampleRate = 4095.0;
}
if( accelSampleRate < 0 ) {
accelSampleRate = 0.0;
}
/* Write the value to the registers */
accelDiv = (uint16_t) accelSampleRate;
ICM20648_registerWrite( ICM20648_REG_ACCEL_SMPLRT_DIV_1, (uint8_t) ( accelDiv >> 8 ) );
ICM20648_registerWrite( ICM20648_REG_ACCEL_SMPLRT_DIV_2, (uint8_t) ( accelDiv & 0xFF ) );
/* Calculate the actual sample rate from the divider value */
accelSampleRate = 1125.0 / ( accelDiv + 1 );
return accelSampleRate;
}
/***************************************************************************/
uint32_t ICM20648_gyroBandwidthSet( uint8_t gyroBw )
{
uint8_t reg;
/* Read the GYRO_CONFIG_1 register */
ICM20648_registerRead( ICM20648_REG_GYRO_CONFIG_1, 1, ® );
reg &= ~( ICM20648_MASK_GYRO_BW );
/* Write the new bandwidth value to the gyro config register */
reg |= ( gyroBw & ICM20648_MASK_GYRO_BW );
ICM20648_registerWrite( ICM20648_REG_GYRO_CONFIG_1, reg );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_accelBandwidthSet( uint8_t accelBw )
{
uint8_t reg;
/* Read the GYRO_CONFIG_1 register */
ICM20648_registerRead( ICM20648_REG_ACCEL_CONFIG, 1, ® );
reg &= ~( ICM20648_MASK_ACCEL_BW );
/* Write the new bandwidth value to the gyro config register */
reg |= ( accelBw & ICM20648_MASK_ACCEL_BW );
ICM20648_registerWrite( ICM20648_REG_ACCEL_CONFIG, reg );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_sleepModeEnable( bool enable )
{
uint8_t reg;
ICM20648_registerRead( ICM20648_REG_PWR_MGMT_1, 1, ® );
if( enable ) {
/* Sleep: set the SLEEP bit */
reg |= ICM20648_BIT_SLEEP;
}
else {
/* Wake up: clear the SLEEP bit */
reg &= ~( ICM20648_BIT_SLEEP );
}
ICM20648_registerWrite( ICM20648_REG_PWR_MGMT_1, reg );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_cycleModeEnable( bool enable )
{
uint8_t reg;
reg = 0x00;
if( enable ) {
reg = ICM20648_BIT_ACCEL_CYCLE | ICM20648_BIT_GYRO_CYCLE;
}
ICM20648_registerWrite( ICM20648_REG_LP_CONFIG, reg );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_sensorEnable( bool accel, bool gyro, bool temp )
{
uint8_t pwrManagement1;
uint8_t pwrManagement2;
ICM20648_registerRead( ICM20648_REG_PWR_MGMT_1, 1, &pwrManagement1 );
pwrManagement2 = 0;
/* To enable the accelerometer clear the DISABLE_ACCEL bits in PWR_MGMT_2 */
if( accel ) {
pwrManagement2 &= ~( ICM20648_BIT_PWR_ACCEL_STBY );
}
else {
pwrManagement2 |= ICM20648_BIT_PWR_ACCEL_STBY;
}
/* To enable gyro clear the DISABLE_GYRO bits in PWR_MGMT_2 */
if( gyro ) {
pwrManagement2 &= ~( ICM20648_BIT_PWR_GYRO_STBY );
}
else {
pwrManagement2 |= ICM20648_BIT_PWR_GYRO_STBY;
}
/* To enable the temperature sensor clear the TEMP_DIS bit in PWR_MGMT_1 */
if( temp ) {
pwrManagement1 &= ~( ICM20648_BIT_TEMP_DIS );
}
else {
pwrManagement1 |= ICM20648_BIT_TEMP_DIS;
}
/* Write back the modified values */
ICM20648_registerWrite( ICM20648_REG_PWR_MGMT_1, pwrManagement1 );
ICM20648_registerWrite( ICM20648_REG_PWR_MGMT_2, pwrManagement2 );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_lowPowerModeEnter( bool enAccel, bool enGyro, bool enTemp )
{
uint8_t data;
ICM20648_registerRead( ICM20648_REG_PWR_MGMT_1, 1, &data );
if( enAccel || enGyro || enTemp ) {
/* Make sure that the chip is not in sleep */
ICM20648_sleepModeEnable( false );
/* And in continuous mode */
ICM20648_cycleModeEnable( false );
/* Enable the accelerometer and the gyroscope*/
ICM20648_sensorEnable( enAccel, enGyro, enTemp );
UTIL_delay( 50 );
/* Enable cycle mode */
ICM20648_cycleModeEnable( true );
/* Set the LP_EN bit to enable low power mode */
data |= ICM20648_BIT_LP_EN;
}
else {
/* Enable continuous mode */
ICM20648_cycleModeEnable( false );
/* Clear the LP_EN bit to disable low power mode */
data &= ~ICM20648_BIT_LP_EN;
}
/* Write the updated value to the PWR_MGNT_1 register */
ICM20648_registerWrite( ICM20648_REG_PWR_MGMT_1, data );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_interruptEnable( bool dataReadyEnable, bool womEnable )
{
uint8_t intEnable;
/* All interrupts disabled by default */
intEnable = 0;
/* Enable one or both of the interrupt sources if required */
if( womEnable ) {
intEnable = ICM20648_BIT_WOM_INT_EN;
}
/* Write value to register */
ICM20648_registerWrite( ICM20648_REG_INT_ENABLE, intEnable );
/* All interrupts disabled by default */
intEnable = 0;
if( dataReadyEnable ) {
intEnable = ICM20648_BIT_RAW_DATA_0_RDY_EN;
}
/* Write value to register */
ICM20648_registerWrite( ICM20648_REG_INT_ENABLE_1, intEnable );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_interruptStatusRead( uint32_t *intStatus )
{
uint8_t reg[4];
ICM20648_registerRead( ICM20648_REG_INT_STATUS, 4, reg );
*intStatus = (uint32_t) reg[0];
*intStatus |= ( ( (uint32_t) reg[1] ) << 8 );
*intStatus |= ( ( (uint32_t) reg[2] ) << 16 );
*intStatus |= ( ( (uint32_t) reg[3] ) << 24 );
return ICM20648_OK;
}
/***************************************************************************/
bool ICM20648_isDataReady( void )
{
uint8_t status;
bool ret;
ret = false;
ICM20648_registerRead( ICM20648_REG_INT_STATUS_1, 1, &status );
if( status & ICM20648_BIT_RAW_DATA_0_RDY_INT ) {
ret = true;
}
return ret;
}
/***************************************************************************/
uint32_t ICM20648_wakeOnMotionITEnable( bool enable, uint8_t womThreshold, float sampleRate )
{
if( enable ) {
/* Make sure that the chip is not in sleep */
ICM20648_sleepModeEnable( false );
/* And in continuous mode */
ICM20648_cycleModeEnable( false );
/* Enable only the accelerometer */
ICM20648_sensorEnable( true, false, false );
/* Set sample rate */
ICM20648_sampleRateSet( sampleRate );
/* Set the bandwidth to 1210Hz */
ICM20648_accelBandwidthSet( ICM20648_ACCEL_BW_1210HZ );
/* Accel: 2G full scale */
ICM20648_accelFullscaleSet( ICM20648_ACCEL_FULLSCALE_2G );
/* Enable the Wake On Motion interrupt */
ICM20648_interruptEnable( false, true );
UTIL_delay( 50 );
/* Enable Wake On Motion feature */
ICM20648_registerWrite( ICM20648_REG_ACCEL_INTEL_CTRL, ICM20648_BIT_ACCEL_INTEL_EN | ICM20648_BIT_ACCEL_INTEL_MODE );
/* Set the wake on motion threshold value */
ICM20648_registerWrite( ICM20648_REG_ACCEL_WOM_THR, womThreshold );
/* Enable low power mode */
ICM20648_lowPowerModeEnter( true, false, false );
}
else {
/* Disable Wake On Motion feature */
ICM20648_registerWrite( ICM20648_REG_ACCEL_INTEL_CTRL, 0x00 );
/* Disable the Wake On Motion interrupt */
ICM20648_interruptEnable( false, false );
/* Disable cycle mode */
ICM20648_cycleModeEnable( false );
}
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_accelGyroCalibrate( float *accelBiasScaled, float *gyroBiasScaled )
{
uint8_t data[12];
uint16_t i, packetCount, fifoCount;
int32_t gyroBias[3] = { 0, 0, 0 };
int32_t accelBias[3] = { 0, 0, 0 };
int32_t accelTemp[3];
int32_t gyroTemp[3];
int32_t accelBiasFactory[3];
int32_t gyroBiasStored[3];
float gyroRes, accelRes;
/* Enable the accelerometer and the gyro */
ICM20648_sensorEnable( true, true, false );
/* Set 1kHz sample rate */
ICM20648_sampleRateSet( 1100.0 );
/* 246Hz BW for the accelerometer and 200Hz for the gyroscope */
ICM20648_accelBandwidthSet( ICM20648_ACCEL_BW_246HZ );
ICM20648_gyroBandwidthSet( ICM20648_GYRO_BW_12HZ );
/* Set the most sensitive range: 2G full scale and 250dps full scale */
ICM20648_accelFullscaleSet( ICM20648_ACCEL_FULLSCALE_2G );
ICM20648_gyroFullscaleSet( ICM20648_GYRO_FULLSCALE_250DPS );
/* Retrieve the resolution per bit */
ICM20648_accelResolutionGet( &accelRes );
ICM20648_gyroResolutionGet( &gyroRes );
/* The accel sensor needs max 30ms, the gyro max 35ms to fully start */
/* Experiments show that the gyro needs more time to get reliable results */
UTIL_delay( 50 );
/* Disable the FIFO */
ICM20648_registerWrite( ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN );
ICM20648_registerWrite( ICM20648_REG_FIFO_MODE, 0x0F );
/* Enable accelerometer and gyro to store the data in FIFO */
ICM20648_registerWrite( ICM20648_REG_FIFO_EN_2, ICM20648_BIT_ACCEL_FIFO_EN | ICM20648_BITS_GYRO_FIFO_EN );
/* Reset the FIFO */
ICM20648_registerWrite( ICM20648_REG_FIFO_RST, 0x0F );
ICM20648_registerWrite( ICM20648_REG_FIFO_RST, 0x00 );
/* Enable the FIFO */
ICM20648_registerWrite( ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN );
/* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */
/* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */
/* Loop until at least 4080 samples gathered */
fifoCount = 0;
while( fifoCount < 4080 ) {
UTIL_delay( 5 );
/* Read FIFO sample count */
ICM20648_registerRead( ICM20648_REG_FIFO_COUNT_H, 2, &data[0] );
/* Convert to a 16 bit value */
fifoCount = ( (uint16_t) ( data[0] << 8 ) | data[1] );
}
/* Disable accelerometer and gyro to store the data in FIFO */
ICM20648_registerWrite( ICM20648_REG_FIFO_EN_2, 0x00 );
/* Read FIFO sample count */
ICM20648_registerRead( ICM20648_REG_FIFO_COUNT_H, 2, &data[0] );
/* Convert to a 16 bit value */
fifoCount = ( (uint16_t) ( data[0] << 8 ) | data[1] );
/* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */
packetCount = fifoCount / 12;
/* Retrieve the data from the FIFO */
for( i = 0; i < packetCount; i++ ) {
ICM20648_registerRead( ICM20648_REG_FIFO_R_W, 12, &data[0] );
/* Convert to 16 bit signed accel and gyro x,y and z values */
accelTemp[0] = ( (int16_t) ( data[0] << 8 ) | data[1] );
accelTemp[1] = ( (int16_t) ( data[2] << 8 ) | data[3] );
accelTemp[2] = ( (int16_t) ( data[4] << 8 ) | data[5] );
gyroTemp[0] = ( (int16_t) ( data[6] << 8 ) | data[7] );
gyroTemp[1] = ( (int16_t) ( data[8] << 8 ) | data[9] );
gyroTemp[2] = ( (int16_t) ( data[10] << 8 ) | data[11] );
/* Sum the values */
accelBias[0] += accelTemp[0];
accelBias[1] += accelTemp[1];
accelBias[2] += accelTemp[2];
gyroBias[0] += gyroTemp[0];
gyroBias[1] += gyroTemp[1];
gyroBias[2] += gyroTemp[2];
}
/* Divide by packet count to get the average */
accelBias[0] /= packetCount;
accelBias[1] /= packetCount;
accelBias[2] /= packetCount;
gyroBias[0] /= packetCount;
gyroBias[1] /= packetCount;
gyroBias[2] /= packetCount;
/* Acceleormeter: add or remove (depending on the orientation of the chip) 1G (gravity) from the Z axis value */
if( accelBias[2] > 0L ) {
accelBias[2] -= (int32_t) ( 1.0 / accelRes );
}
else {
accelBias[2] += (int32_t) ( 1.0 / accelRes );
}
/* Convert the values to degrees per sec for displaying */
gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes;
gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes;
gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes;
/* Read stored gyro trim values. After reset these values are all 0 */
ICM20648_registerRead( ICM20648_REG_XG_OFFS_USRH, 2, &data[0] );
gyroBiasStored[0] = ( (int16_t) ( data[0] << 8 ) | data[1] );
ICM20648_registerRead( ICM20648_REG_YG_OFFS_USRH, 2, &data[0] );
gyroBiasStored[1] = ( (int16_t) ( data[0] << 8 ) | data[1] );
ICM20648_registerRead( ICM20648_REG_ZG_OFFS_USRH, 2, &data[0] );
gyroBiasStored[2] = ( (int16_t) ( data[0] << 8 ) | data[1] );
/* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */
/* the best sensitivity, so need to divide by 4 */
/* Substract from the stored calibration value */
gyroBiasStored[0] -= gyroBias[0] / 4;
gyroBiasStored[1] -= gyroBias[1] / 4;
gyroBiasStored[2] -= gyroBias[2] / 4;
/* Split the values into two bytes */
data[0] = ( gyroBiasStored[0] >> 8 ) & 0xFF;
data[1] = ( gyroBiasStored[0] ) & 0xFF;
data[2] = ( gyroBiasStored[1] >> 8 ) & 0xFF;
data[3] = ( gyroBiasStored[1] ) & 0xFF;
data[4] = ( gyroBiasStored[2] >> 8 ) & 0xFF;
data[5] = ( gyroBiasStored[2] ) & 0xFF;
/* Write the gyro bias values to the chip */
ICM20648_registerWrite( ICM20648_REG_XG_OFFS_USRH, data[0] );
ICM20648_registerWrite( ICM20648_REG_XG_OFFS_USRL, data[1] );
ICM20648_registerWrite( ICM20648_REG_YG_OFFS_USRH, data[2] );
ICM20648_registerWrite( ICM20648_REG_YG_OFFS_USRL, data[3] );
ICM20648_registerWrite( ICM20648_REG_ZG_OFFS_USRH, data[4] );
ICM20648_registerWrite( ICM20648_REG_ZG_OFFS_USRL, data[5] );
/* Calculate the accelerometer bias values to store in the hardware accelerometer bias registers. These registers contain */
/* factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold */
/* non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature */
/* compensation calculations(? the datasheet is not clear). Accelerometer bias registers expect bias input */
/* as 2048 LSB per g, so that the accelerometer biases calculated above must be divided by 8. */
/* Read factory accelerometer trim values */
ICM20648_registerRead( ICM20648_REG_XA_OFFSET_H, 2, &data[0] );
accelBiasFactory[0] = ( (int16_t) ( data[0] << 8 ) | data[1] );
ICM20648_registerRead( ICM20648_REG_YA_OFFSET_H, 2, &data[0] );
accelBiasFactory[1] = ( (int16_t) ( data[0] << 8 ) | data[1] );
ICM20648_registerRead( ICM20648_REG_ZA_OFFSET_H, 2, &data[0] );
accelBiasFactory[2] = ( (int16_t) ( data[0] << 8 ) | data[1] );
/* Construct total accelerometer bias, including calculated average accelerometer bias from above */
/* Scale the 2g full scale (most sensitive range) results to 16g full scale - divide by 8 */
/* Clear the last bit (temperature compensation? - the datasheet is not clear) */
/* Substract from the factory calibration value */
accelBiasFactory[0] -= ( ( accelBias[0] / 8 ) & ~1 );
accelBiasFactory[1] -= ( ( accelBias[1] / 8 ) & ~1 );
accelBiasFactory[2] -= ( ( accelBias[2] / 8 ) & ~1 );
/* Split the values into two bytes */
data[0] = ( accelBiasFactory[0] >> 8 ) & 0xFF;
data[1] = ( accelBiasFactory[0] ) & 0xFF;
data[2] = ( accelBiasFactory[1] >> 8 ) & 0xFF;
data[3] = ( accelBiasFactory[1] ) & 0xFF;
data[4] = ( accelBiasFactory[2] >> 8 ) & 0xFF;
data[5] = ( accelBiasFactory[2] ) & 0xFF;
/* Store them in the accelerometer offset registers */
ICM20648_registerWrite( ICM20648_REG_XA_OFFSET_H, data[0] );
ICM20648_registerWrite( ICM20648_REG_XA_OFFSET_L, data[1] );
ICM20648_registerWrite( ICM20648_REG_YA_OFFSET_H, data[2] );
ICM20648_registerWrite( ICM20648_REG_YA_OFFSET_L, data[3] );
ICM20648_registerWrite( ICM20648_REG_ZA_OFFSET_H, data[4] );
ICM20648_registerWrite( ICM20648_REG_ZA_OFFSET_L, data[5] );
/* Convert the values to G for displaying */
accelBiasScaled[0] = (float) accelBias[0] * accelRes;
accelBiasScaled[1] = (float) accelBias[1] * accelRes;
accelBiasScaled[2] = (float) accelBias[2] * accelRes;
/* Turn off FIFO */
ICM20648_registerWrite( ICM20648_REG_USER_CTRL, 0x00 );
/* Disable all sensors */
ICM20648_sensorEnable( false, false, false );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_gyroCalibrate( float *gyroBiasScaled )
{
uint8_t data[12];
uint16_t i, packetCount, fifoCount;
int32_t gyroBias[3] = { 0, 0, 0 };
int32_t gyroTemp[3];
int32_t gyroBiasStored[3];
float gyroRes;
/* Enable the accelerometer and the gyro */
ICM20648_sensorEnable( true, true, false );
/* Set 1kHz sample rate */
ICM20648_sampleRateSet( 1100.0 );
/* Configure bandwidth for gyroscope to 12Hz */
ICM20648_gyroBandwidthSet( ICM20648_GYRO_BW_12HZ );
/* Configure sensitivity to 250dps full scale */
ICM20648_gyroFullscaleSet( ICM20648_GYRO_FULLSCALE_250DPS );
/* Retrieve the resolution per bit */
ICM20648_gyroResolutionGet( &gyroRes );
/* The accel sensor needs max 30ms, the gyro max 35ms to fully start */
/* Experiments show that the gyro needs more time to get reliable results */
UTIL_delay( 50 );
/* Disable the FIFO */
ICM20648_registerWrite( ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN );
ICM20648_registerWrite( ICM20648_REG_FIFO_MODE, 0x0F );
/* Enable accelerometer and gyro to store the data in FIFO */
ICM20648_registerWrite( ICM20648_REG_FIFO_EN_2, ICM20648_BITS_GYRO_FIFO_EN );
/* Reset the FIFO */
ICM20648_registerWrite( ICM20648_REG_FIFO_RST, 0x0F );
ICM20648_registerWrite( ICM20648_REG_FIFO_RST, 0x00 );
/* Enable the FIFO */
ICM20648_registerWrite( ICM20648_REG_USER_CTRL, ICM20648_BIT_FIFO_EN );
/* The max FIFO size is 4096 bytes, one set of measurements takes 12 bytes */
/* (3 axes, 2 sensors, 2 bytes each value ) 340 samples use 4080 bytes of FIFO */
/* Loop until at least 4080 samples gathered */
fifoCount = 0;
while( fifoCount < 4080 ) {
UTIL_delay( 5 );
/* Read FIFO sample count */
ICM20648_registerRead( ICM20648_REG_FIFO_COUNT_H, 2, &data[0] );
/* Convert to a 16 bit value */
fifoCount = ( (uint16_t) ( data[0] << 8 ) | data[1] );
}
/* Disable accelerometer and gyro to store the data in FIFO */
ICM20648_registerWrite( ICM20648_REG_FIFO_EN_2, 0x00 );
/* Read FIFO sample count */
ICM20648_registerRead( ICM20648_REG_FIFO_COUNT_H, 2, &data[0] );
/* Convert to a 16 bit value */
fifoCount = ( (uint16_t) ( data[0] << 8 ) | data[1] );
/* Calculate the number of data sets (3 axis of accel an gyro, two bytes each = 12 bytes) */
packetCount = fifoCount / 12;
/* Retrieve the data from the FIFO */
for( i = 0; i < packetCount; i++ ) {
ICM20648_registerRead( ICM20648_REG_FIFO_R_W, 12, &data[0] );
/* Convert to 16 bit signed accel and gyro x,y and z values */
gyroTemp[0] = ( (int16_t) ( data[6] << 8 ) | data[7] );
gyroTemp[1] = ( (int16_t) ( data[8] << 8 ) | data[9] );
gyroTemp[2] = ( (int16_t) ( data[10] << 8 ) | data[11] );
/* Sum the values */
gyroBias[0] += gyroTemp[0];
gyroBias[1] += gyroTemp[1];
gyroBias[2] += gyroTemp[2];
}
/* Divide by packet count to get the average */
gyroBias[0] /= packetCount;
gyroBias[1] /= packetCount;
gyroBias[2] /= packetCount;
/* Convert the values to degrees per sec for displaying */
gyroBiasScaled[0] = (float) gyroBias[0] * gyroRes;
gyroBiasScaled[1] = (float) gyroBias[1] * gyroRes;
gyroBiasScaled[2] = (float) gyroBias[2] * gyroRes;
/* Read stored gyro trim values. After reset these values are all 0 */
ICM20648_registerRead( ICM20648_REG_XG_OFFS_USRH, 2, &data[0] );
gyroBiasStored[0] = ( (int16_t) ( data[0] << 8 ) | data[1] );
ICM20648_registerRead( ICM20648_REG_YG_OFFS_USRH, 2, &data[0] );
gyroBiasStored[1] = ( (int16_t) ( data[0] << 8 ) | data[1] );
ICM20648_registerRead( ICM20648_REG_ZG_OFFS_USRH, 2, &data[0] );
gyroBiasStored[2] = ( (int16_t) ( data[0] << 8 ) | data[1] );
/* The gyro bias should be stored in 1000dps full scaled format. We measured in 250dps to get */
/* the best sensitivity, so need to divide by 4 */
/* Substract from the stored calibration value */
gyroBiasStored[0] -= gyroBias[0] / 4;
gyroBiasStored[1] -= gyroBias[1] / 4;
gyroBiasStored[2] -= gyroBias[2] / 4;
/* Split the values into two bytes */
data[0] = ( gyroBiasStored[0] >> 8 ) & 0xFF;
data[1] = ( gyroBiasStored[0] ) & 0xFF;
data[2] = ( gyroBiasStored[1] >> 8 ) & 0xFF;
data[3] = ( gyroBiasStored[1] ) & 0xFF;
data[4] = ( gyroBiasStored[2] >> 8 ) & 0xFF;
data[5] = ( gyroBiasStored[2] ) & 0xFF;
/* Write the gyro bias values to the chip */
ICM20648_registerWrite( ICM20648_REG_XG_OFFS_USRH, data[0] );
ICM20648_registerWrite( ICM20648_REG_XG_OFFS_USRL, data[1] );
ICM20648_registerWrite( ICM20648_REG_YG_OFFS_USRH, data[2] );
ICM20648_registerWrite( ICM20648_REG_YG_OFFS_USRL, data[3] );
ICM20648_registerWrite( ICM20648_REG_ZG_OFFS_USRH, data[4] );
ICM20648_registerWrite( ICM20648_REG_ZG_OFFS_USRL, data[5] );
/* Turn off FIFO */
ICM20648_registerWrite( ICM20648_REG_USER_CTRL, 0x00 );
/* Disable all sensors */
ICM20648_sensorEnable( false, false, false );
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_temperatureRead( float *temperature )
{
uint8_t data[2];
int16_t raw_temp;
/* Read temperature registers */
ICM20648_registerRead( ICM20648_REG_TEMPERATURE_H, 2, data );
/* Convert to int16 */
raw_temp = (int16_t) ( ( data[0] << 8 ) + data[1] );
/* Calculate the Celsius value from the raw reading */
*temperature = ( (float) raw_temp / 333.87 ) + 21.0;
return ICM20648_OK;
}
/***************************************************************************/
uint32_t ICM20648_getDeviceID( uint8_t *devID )
{
ICM20648_registerRead( ICM20648_REG_WHO_AM_I, 1, devID );
return ICM20648_OK;
}
/***************************************************************************/
void ICM20648_registerRead( uint16_t addr, int numBytes, uint8_t *data )
{
unsigned long AckTmOut;
uint8_t i;
uint8_t regAddr = (uint8_t) ( addr & 0x7F );
I2CStart(Motion);
if(I2CSendData(ICM20648_WriteCmd)==Ack)
{
AckTmOut = 50000;
while(I2CSendData(regAddr)){if(AckTmOut-- <= 1)break;}
I2CStart(Motion);
if(I2CSendData(ICM20648_ReadCmd)==Ack)
{
for (i = 0; i < numBytes-1;i++)
{
*data++ = I2CReadData(Ack);
}
*data = I2CReadData(Nack);
}
I2CStop();
}
//EepromBusy = 0;
return;
}
/***************************************************************************/
void ICM20648_registerWrite( uint16_t addr, uint8_t data )
{
unsigned long i,ulRemain = 0;
unsigned long AckTmOut;
uint8_t regAddr = (uint8_t) ( addr & 0x7F );
//EepromBusy = 1;
I2CStart(Motion);
if(I2CSendData(ICM20648_WriteCmd)==Ack)
{
AckTmOut = 50000;
while(I2CSendData(regAddr)){if(AckTmOut-- <= 1)break;}
I2CSendData(data);
I2CStop();
}
//EepromBusy = 0;
return;
}
unsigned char
ICM20648_WriteData(unsigned char ucAdr, unsigned char ucData)
{
unsigned long AckTmOut;
unsigned long i;
unsigned char rv;
//EepromBusy = 1;
I2CStart(Motion);
if(I2CSendData(ICM20648_WriteCmd)==Ack)
{
AckTmOut = 50000;
while(I2CSendData(ucAdr)){if(AckTmOut-- <= 1)break;}
rv = I2CSendData(ucData)?false:true;
}
I2CStop();
//EepromBusy = 0;
return rv;
}
unsigned char
ICM20648_ReadData(unsigned char ucAdr)
{
unsigned long AckTmOut;
unsigned char ucData;
//EepromBusy = 1;
I2CStart(Motion);
if(I2CSendData(ICM20648_DvcAddress)==Ack)
{
AckTmOut = 50000;
while(I2CSendData(ucAdr)){if(AckTmOut-- <= 1)break;}
I2CStart(Motion);
if(I2CSendData(ICM20648_DvcAddress+1)==Ack)
{
ucData = I2CReadData(Nack);
}
else ucData = 0;
}
I2CStop();
//EepromBusy = 0;
return ucData;
}
uint8_t motData[5];
void MotionTest()
{
unsigned char i;
I2CSoftInit();
//ICM20648_WriteData(0,1);
while(1)
{
//for(i=0;i<4;i++) motData[i] = 5;
//for(i=0;i<3;i++) motData[i] = ICM20648_ReadData2(i);
motData[0] = 0x25;
motData[0] = ICM20648_ReadData(0);
if(motData[0] != 0xE0)
motData[0] = 0x35;
//motData[2] = ICM20648_ReadData(2);
//motData[3] = ICM20648_ReadData(3);
}
}
/***************************************************************************/
#ifndef __ICM20648_H_
#define __ICM20648_H_
#include "i2c_soft.h"
#define ICM20648_DvcAddress 0xD0
#define ICM20648_WriteCmd 0xD0
#define ICM20648_ReadCmd 0xD1
/***************************************************************************/
/***************************************************************************/
#define ICM20648_OK 0x0000
#define ICM20648_ERROR_INVALID_DEVICE_ID 0x0001
/***************************************************************************/
/* ICM20648 register banks */
#define ICM20648_BANK_0 ( 0 << 7 )
#define ICM20648_BANK_1 ( 1 << 7 )
#define ICM20648_BANK_2 ( 2 << 7 )
#define ICM20648_BANK_3 ( 3 << 7 )
/* Register and associated bit definitions */
/***********************/
/* Bank 0 register map */
/***********************/
#define ICM20648_REG_WHO_AM_I 0x75//( ICM20648_BANK_0 | 0x00 )
#define ICM20648_REG_USER_CTRL ( ICM20648_BANK_0 | 0x03 )
#define ICM20648_BIT_DMP_EN 0x80
#define ICM20648_BIT_FIFO_EN 0x40
#define ICM20648_BIT_I2C_MST_EN 0x20
#define ICM20648_BIT_I2C_IF_DIS 0x10
#define ICM20648_BIT_DMP_RST 0x08
#define ICM20648_BIT_DIAMOND_DMP_RST 0x04
#define ICM20648_REG_LP_CONFIG ( ICM20648_BANK_0 | 0x05 )
#define ICM20648_BIT_I2C_MST_CYCLE 0x40
#define ICM20648_BIT_ACCEL_CYCLE 0x20
#define ICM20648_BIT_GYRO_CYCLE 0x10
#define ICM20648_REG_PWR_MGMT_1 0x6B//( ICM20648_BANK_0 | 0x06 )
#define ICM20648_BIT_H_RESET 0x80
#define ICM20648_BIT_SLEEP 0x40
#define ICM20648_BIT_LP_EN 0x20
#define ICM20648_BIT_TEMP_DIS 0x08
#define ICM20648_BIT_CLK_PLL 0x01
#define ICM20648_REG_PWR_MGMT_2 0x6C//( ICM20648_BANK_0 | 0x07 )
#define ICM20648_BIT_PWR_ACCEL_STBY 0x38
#define ICM20648_BIT_PWR_GYRO_STBY 0x07
#define ICM20648_BIT_PWR_ALL_OFF 0x7F
#define ICM20648_REG_INT_PIN_CFG 0x37//( ICM20648_BANK_0 | 0x0F )
#define ICM20648_BIT_INT_ACTL 0x80
#define ICM20648_BIT_INT_OPEN 0x40
#define ICM20648_BIT_INT_LATCH_EN 0x20
#define ICM20648_REG_INT_ENABLE 0x38//( ICM20648_BANK_0 | 0x10 )
#define ICM20648_BIT_WOM_INT_EN 0x08
#define ICM20648_REG_INT_ENABLE_1 ( ICM20648_BANK_0 | 0x11 )
#define ICM20648_BIT_RAW_DATA_0_RDY_EN 0x01
#define ICM20648_REG_INT_ENABLE_2 ( ICM20648_BANK_0 | 0x12 )
#define ICM20648_BIT_FIFO_OVERFLOW_EN_0 0x01
#define ICM20648_REG_INT_ENABLE_3 ( ICM20648_BANK_0 | 0x13 )
#define ICM20648_REG_INT_STATUS ( ICM20648_BANK_0 | 0x19 )
#define ICM20648_BIT_WOM_INT 0x08
#define ICM20648_BIT_PLL_RDY 0x04
#define ICM20648_REG_INT_STATUS_1 ( ICM20648_BANK_0 | 0x1A )
#define ICM20648_BIT_RAW_DATA_0_RDY_INT 0x01
#define ICM20648_REG_INT_STATUS_2 ( ICM20648_BANK_0 | 0x1B )
#define ICM20648_REG_ACCEL_XOUT_H_SH 0x3B//( ICM20648_BANK_0 | 0x2D )
#define ICM20648_REG_ACCEL_XOUT_L_SH 0x3C//( ICM20648_BANK_0 | 0x2E )
#define ICM20648_REG_ACCEL_YOUT_H_SH 0x3D//( ICM20648_BANK_0 | 0x2F )
#define ICM20648_REG_ACCEL_YOUT_L_SH 0x3E//( ICM20648_BANK_0 | 0x30 )
#define ICM20648_REG_ACCEL_ZOUT_H_SH 0x3F//( ICM20648_BANK_0 | 0x31 )
#define ICM20648_REG_ACCEL_ZOUT_L_SH 0x40//( ICM20648_BANK_0 | 0x32 )
#define ICM20648_REG_GYRO_XOUT_H_SH 0x43//( ICM20648_BANK_0 | 0x33 )
#define ICM20648_REG_GYRO_XOUT_L_SH 0x44//( ICM20648_BANK_0 | 0x34 )
#define ICM20648_REG_GYRO_YOUT_H_SH 0x45//( ICM20648_BANK_0 | 0x35 )
#define ICM20648_REG_GYRO_YOUT_L_SH 0x46//( ICM20648_BANK_0 | 0x36 )
#define ICM20648_REG_GYRO_ZOUT_H_SH 0x47//( ICM20648_BANK_0 | 0x37 )
#define ICM20648_REG_GYRO_ZOUT_L_SH 0x48//( ICM20648_BANK_0 | 0x38 )
#define ICM20648_REG_TEMPERATURE_H ( ICM20648_BANK_0 | 0x39 )
#define ICM20648_REG_TEMPERATURE_L ( ICM20648_BANK_0 | 0x3A )
#define ICM20648_REG_TEMP_CONFIG ( ICM20648_BANK_0 | 0x53 )
#define ICM20648_REG_FIFO_EN_1 ( ICM20648_BANK_0 | 0x66 )
#define ICM20648_REG_FIFO_EN_2 ( ICM20648_BANK_0 | 0x67 )
#define ICM20648_BIT_ACCEL_FIFO_EN 0x10
#define ICM20648_BITS_GYRO_FIFO_EN 0x0E
#define ICM20648_REG_FIFO_RST ( ICM20648_BANK_0 | 0x68 )
#define ICM20648_REG_FIFO_MODE ( ICM20648_BANK_0 | 0x69 )
#define ICM20648_REG_FIFO_COUNT_H ( ICM20648_BANK_0 | 0x70 )
#define ICM20648_REG_FIFO_COUNT_L ( ICM20648_BANK_0 | 0x71 )
#define ICM20648_REG_FIFO_R_W ( ICM20648_BANK_0 | 0x72 )
#define ICM20648_REG_DATA_RDY_STATUS ( ICM20648_BANK_0 | 0x74 )
#define ICM20648_BIT_RAW_DATA_0_RDY 0x01
#define ICM20648_REG_FIFO_CFG ( ICM20648_BANK_0 | 0x76 )
#define ICM20648_BIT_MULTI_FIFO_CFG 0x01
#define ICM20648_BIT_SINGLE_FIFO_CFG 0x00
/***********************/
/* Bank 1 register map */
/***********************/
#define ICM20648_REG_XA_OFFSET_H ( ICM20648_BANK_1 | 0x14 )
#define ICM20648_REG_XA_OFFSET_L ( ICM20648_BANK_1 | 0x15 )
#define ICM20648_REG_YA_OFFSET_H ( ICM20648_BANK_1 | 0x17 )
#define ICM20648_REG_YA_OFFSET_L ( ICM20648_BANK_1 | 0x18 )
#define ICM20648_REG_ZA_OFFSET_H ( ICM20648_BANK_1 | 0x1A )
#define ICM20648_REG_ZA_OFFSET_L ( ICM20648_BANK_1 | 0x1B )
#define ICM20648_REG_TIMEBASE_CORR_PLL ( ICM20648_BANK_1 | 0x28 )
/***********************/
/* Bank 2 register map */
/***********************/
#define ICM20648_REG_GYRO_SMPLRT_DIV ( ICM20648_BANK_2 | 0x00 )
#define ICM20648_REG_GYRO_CONFIG_1 ( ICM20648_BANK_2 | 0x01 )
#define ICM20648_BIT_GYRO_FCHOICE 0x01
#define ICM20648_SHIFT_GYRO_FS_SEL 1
#define ICM20648_SHIFT_GYRO_DLPCFG 3
#define ICM20648_MASK_GYRO_FULLSCALE 0x06
#define ICM20648_MASK_GYRO_BW 0x39
#define ICM20648_GYRO_FULLSCALE_250DPS ( 0x00 << ICM20648_SHIFT_GYRO_FS_SEL )
#define ICM20648_GYRO_FULLSCALE_500DPS ( 0x01 << ICM20648_SHIFT_GYRO_FS_SEL )
#define ICM20648_GYRO_FULLSCALE_1000DPS ( 0x02 << ICM20648_SHIFT_GYRO_FS_SEL )
#define ICM20648_GYRO_FULLSCALE_2000DPS ( 0x03 << ICM20648_SHIFT_GYRO_FS_SEL )
#define ICM20648_GYRO_BW_12100HZ ( 0x00 << ICM20648_SHIFT_GYRO_DLPCFG )
#define ICM20648_GYRO_BW_360HZ ( ( 0x07 << ICM20648_SHIFT_GYRO_DLPCFG ) | ICM20648_BIT_GYRO_FCHOICE )
#define ICM20648_GYRO_BW_200HZ ( ( 0x00 << ICM20648_SHIFT_GYRO_DLPCFG ) | ICM20648_BIT_GYRO_FCHOICE )
#define ICM20648_GYRO_BW_150HZ ( ( 0x01 << ICM20648_SHIFT_GYRO_DLPCFG ) | ICM20648_BIT_GYRO_FCHOICE )
#define ICM20648_GYRO_BW_120HZ ( ( 0x02 << ICM20648_SHIFT_GYRO_DLPCFG ) | ICM20648_BIT_GYRO_FCHOICE )
#define ICM20648_GYRO_BW_51HZ ( ( 0x03 << ICM20648_SHIFT_GYRO_DLPCFG ) | ICM20648_BIT_GYRO_FCHOICE )
#define ICM20648_GYRO_BW_24HZ ( ( 0x04 << ICM20648_SHIFT_GYRO_DLPCFG ) | ICM20648_BIT_GYRO_FCHOICE )
#define ICM20648_GYRO_BW_12HZ ( ( 0x05 << ICM20648_SHIFT_GYRO_DLPCFG ) | ICM20648_BIT_GYRO_FCHOICE )
#define ICM20648_GYRO_BW_6HZ ( ( 0x06 << ICM20648_SHIFT_GYRO_DLPCFG ) | ICM20648_BIT_GYRO_FCHOICE )
#define ICM20648_REG_GYRO_CONFIG_2 ( ICM20648_BANK_2 | 0x02 )
#define ICM20648_BIT_GYRO_CTEN 0x38
#define ICM20648_REG_XG_OFFS_USRH ( ICM20648_BANK_2 | 0x03 )
#define ICM20648_REG_XG_OFFS_USRL ( ICM20648_BANK_2 | 0x04 )
#define ICM20648_REG_YG_OFFS_USRH ( ICM20648_BANK_2 | 0x05 )
#define ICM20648_REG_YG_OFFS_USRL ( ICM20648_BANK_2 | 0x06 )
#define ICM20648_REG_ZG_OFFS_USRH ( ICM20648_BANK_2 | 0x07 )
#define ICM20648_REG_ZG_OFFS_USRL ( ICM20648_BANK_2 | 0x08 )
#define ICM20648_REG_ODR_ALIGN_EN ( ICM20648_BANK_2 | 0x09 )
#define ICM20648_REG_ACCEL_SMPLRT_DIV_1 ( ICM20648_BANK_2 | 0x10 )
#define ICM20648_REG_ACCEL_SMPLRT_DIV_2 ( ICM20648_BANK_2 | 0x11 )
#define ICM20648_REG_ACCEL_INTEL_CTRL ( ICM20648_BANK_2 | 0x12 )
#define ICM20648_BIT_ACCEL_INTEL_EN 0x02
#define ICM20648_BIT_ACCEL_INTEL_MODE 0x01
#define ICM20648_REG_ACCEL_WOM_THR ( ICM20648_BANK_2 | 0x13 )
#define ICM20648_REG_ACCEL_CONFIG ( ICM20648_BANK_2 | 0x14 )
#define ICM20648_BIT_ACCEL_FCHOICE 0x01
#define ICM20648_SHIFT_ACCEL_FS 1
#define ICM20648_SHIFT_ACCEL_DLPCFG 3
#define ICM20648_MASK_ACCEL_FULLSCALE 0x06
#define ICM20648_MASK_ACCEL_BW 0x39
#define ICM20648_ACCEL_FULLSCALE_2G ( 0x00 << ICM20648_SHIFT_ACCEL_FS )
#define ICM20648_ACCEL_FULLSCALE_4G ( 0x01 << ICM20648_SHIFT_ACCEL_FS )
#define ICM20648_ACCEL_FULLSCALE_8G ( 0x02 << ICM20648_SHIFT_ACCEL_FS )
#define ICM20648_ACCEL_FULLSCALE_16G ( 0x03 << ICM20648_SHIFT_ACCEL_FS )
#define ICM20648_ACCEL_BW_1210HZ ( 0x00 << ICM20648_SHIFT_ACCEL_DLPCFG )
#define ICM20648_ACCEL_BW_470HZ ( ( 0x07 << ICM20648_SHIFT_ACCEL_DLPCFG ) | ICM20648_BIT_ACCEL_FCHOICE )
#define ICM20648_ACCEL_BW_246HZ ( ( 0x00 << ICM20648_SHIFT_ACCEL_DLPCFG ) | ICM20648_BIT_ACCEL_FCHOICE )
#define ICM20648_ACCEL_BW_111HZ ( ( 0x02 << ICM20648_SHIFT_ACCEL_DLPCFG ) | ICM20648_BIT_ACCEL_FCHOICE )
#define ICM20648_ACCEL_BW_50HZ ( ( 0x03 << ICM20648_SHIFT_ACCEL_DLPCFG ) | ICM20648_BIT_ACCEL_FCHOICE )
#define ICM20648_ACCEL_BW_24HZ ( ( 0x04 << ICM20648_SHIFT_ACCEL_DLPCFG ) | ICM20648_BIT_ACCEL_FCHOICE )
#define ICM20648_ACCEL_BW_12HZ ( ( 0x05 << ICM20648_SHIFT_ACCEL_DLPCFG ) | ICM20648_BIT_ACCEL_FCHOICE )
#define ICM20648_ACCEL_BW_6HZ ( ( 0x06 << ICM20648_SHIFT_ACCEL_DLPCFG ) | ICM20648_BIT_ACCEL_FCHOICE )
#define ICM20648_REG_ACCEL_CONFIG_2 ( ICM20648_BANK_2 | 0x15 )
#define ICM20648_BIT_ACCEL_CTEN 0x1C
/***********************/
/* Bank 3 register map */
/***********************/
#define ICM20648_REG_I2C_MST_ODR_CONFIG ( ICM20648_BANK_3 | 0x00 )
#define ICM20648_REG_I2C_MST_CTRL ( ICM20648_BANK_3 | 0x01 )
#define ICM20648_BIT_I2C_MST_P_NSR 0x10
#define ICM20648_REG_I2C_MST_DELAY_CTRL ( ICM20648_BANK_3 | 0x02 )
#define ICM20648_BIT_SLV0_DLY_EN 0x01
#define ICM20648_BIT_SLV1_DLY_EN 0x02
#define ICM20648_BIT_SLV2_DLY_EN 0x04
#define ICM20648_BIT_SLV3_DLY_EN 0x08
#define ICM20648_REG_I2C_SLV0_ADDR ( ICM20648_BANK_3 | 0x03 )
#define ICM20648_REG_I2C_SLV0_REG ( ICM20648_BANK_3 | 0x04 )
#define ICM20648_REG_I2C_SLV0_CTRL ( ICM20648_BANK_3 | 0x05 )
#define ICM20648_REG_I2C_SLV0_DO ( ICM20648_BANK_3 | 0x06 )
#define ICM20648_REG_I2C_SLV1_ADDR ( ICM20648_BANK_3 | 0x07 )
#define ICM20648_REG_I2C_SLV1_REG ( ICM20648_BANK_3 | 0x08 )
#define ICM20648_REG_I2C_SLV1_CTRL ( ICM20648_BANK_3 | 0x09 )
#define ICM20648_REG_I2C_SLV1_DO ( ICM20648_BANK_3 | 0x0A )
#define ICM20648_REG_I2C_SLV2_ADDR ( ICM20648_BANK_3 | 0x0B )
#define ICM20648_REG_I2C_SLV2_REG ( ICM20648_BANK_3 | 0x0C )
#define ICM20648_REG_I2C_SLV2_CTRL ( ICM20648_BANK_3 | 0x0D )
#define ICM20648_REG_I2C_SLV2_DO ( ICM20648_BANK_3 | 0x0E )
#define ICM20648_REG_I2C_SLV3_ADDR ( ICM20648_BANK_3 | 0x0F )
#define ICM20648_REG_I2C_SLV3_REG ( ICM20648_BANK_3 | 0x10 )
#define ICM20648_REG_I2C_SLV3_CTRL ( ICM20648_BANK_3 | 0x11 )
#define ICM20648_REG_I2C_SLV3_DO ( ICM20648_BANK_3 | 0x12 )
#define ICM20648_REG_I2C_SLV4_ADDR ( ICM20648_BANK_3 | 0x13 )
#define ICM20648_REG_I2C_SLV4_REG ( ICM20648_BANK_3 | 0x14 )
#define ICM20648_REG_I2C_SLV4_CTRL ( ICM20648_BANK_3 | 0x15 )
#define ICM20648_REG_I2C_SLV4_DO ( ICM20648_BANK_3 | 0x16 )
#define ICM20648_REG_I2C_SLV4_DI ( ICM20648_BANK_3 | 0x17 )
#define ICM20648_BIT_I2C_SLV_EN 0x80
#define ICM20648_BIT_I2C_BYTE_SW 0x40
#define ICM20648_BIT_I2C_REG_DIS 0x20
#define ICM20648_BIT_I2C_GRP 0x10
#define ICM20648_BIT_I2C_READ 0x80
/* Register common for all banks */
#define ICM20648_REG_BANK_SEL 0x7F
#define ICM20648_DEVICE_ID 0x70
/***************************************************************************/
uint32_t ICM20648_spiInit( void );
void ICM20648_registerRead(uint16_t addr, int numBytes, uint8_t *data);
void ICM20648_registerWrite(uint16_t addr, uint8_t data);
void ICM20648_bankSelect( uint8_t bank );
uint32_t ICM20648_reset( void );
uint32_t ICM20648_sampleRateSet( float sampleRate );
float ICM20648_gyroSampleRateSet( float sampleRate );
float ICM20648_accelSampleRateSet( float sampleRate );
uint32_t ICM20648_gyroBandwidthSet( uint8_t gyroBw );
uint32_t ICM20648_accelBandwidthSet( uint8_t accelBw );
uint32_t ICM20648_accelDataRead( float *accel );
uint32_t ICM20648_gyroDataRead( float *gyro );
uint32_t ICM20648_accelResolutionGet( float *accelRes );
uint32_t ICM20648_gyroResolutionGet( float *gyroRes );
uint32_t ICM20648_accelFullscaleSet( uint8_t accelFs );
uint32_t ICM20648_gyroFullscaleSet( uint8_t gyroFs );
uint32_t ICM20648_sleepModeEnable( bool enable );
uint32_t ICM20648_cycleModeEnable( bool enable );
uint32_t ICM20648_sensorEnable( bool accel, bool gyro, bool temp );
uint32_t ICM20648_lowPowerModeEnter( bool enAccel, bool enGyro, bool enTemp );
uint32_t ICM20648_interruptEnable( bool dataReadyEnable, bool womEnable );
uint32_t ICM20648_interruptStatusRead( uint32_t *int_status );
bool ICM20648_isDataReady( void );
uint32_t ICM20648_wakeOnMotionITEnable( bool enable, uint8_t womThreshold, float sampleRate );
uint32_t ICM20648_init( void );
uint32_t ICM20648_deInit( void );
uint32_t ICM20648_accelGyroCalibrate( float *accelBiasScaled, float *gyroBiasScaled );
uint32_t ICM20648_gyroCalibrate( float *gyroBiasScaled );
uint32_t ICM20648_temperatureRead( float *temperature );
uint32_t ICM20648_getDeviceID( uint8_t *devID );
uint32_t ICM20648_accelRawDataRead( uint8_t *accel );
uint32_t ICM20648_gyroRawDataRead( uint8_t *gyro );
unsigned char ICM20648_WriteData(unsigned char ucAdr, unsigned char ucData);
unsigned char ICM20648_ReadData(unsigned char ucAdr);
void MotionTest();
#endif /* __ICM20648_H_ */
Yeah, I see that you modified ICM-20648 firmware. I would suggest to use the drivers designed for MPU65000. You can find them here: https://invensense.tdk.com/developers/download/emd-6-12/?wpdmdl=45
https://invensense.tdk.com/developers/software-downloads/#smartmotion