icm42688.c
#include "icm42688.h"
#if defined(ICM_USE_HARD_SPI)
//#include "spi.h"
#include "IO_SPI.h"
#elif defined(ICM_USE_HARD_I2C)
#include "bsp_cpu_i2c2.h"
#endif
#include "DWT.h"
#include "stdio.h"
static float accSensitivity = 0.244f; //加速度的最小分辨率 mg/LSB
static float gyroSensitivity = 32.8f; //陀螺仪的最小分辨率
/*ICM42688使用的ms级延时函数，须由用户提供。*/
#define ICM42688DelayMs(_nms) bsp_DelayMS(_nms)
#if defined(ICM_USE_HARD_SPI)
#define ICM_RCC_SPIX_CS() __HAL_RCC_GPIOB_CLK_ENABLE()
#define ICM_PORT_SPIX_CS GPIOB
#define ICM_PIN_SPIX_CS GPIO_PIN_12
#define ICM_SPI_CS_LOW() HAL_GPIO_WritePin(ICM_PORT_SPIX_CS, ICM_PIN_SPIX_CS, GPIO_PIN_RESET)
#define ICM_SPI_CS_HIGH() HAL_GPIO_WritePin(ICM_PORT_SPIX_CS, ICM_PIN_SPIX_CS, GPIO_PIN_SET)
/*******************************************************************************
* 名 称： bsp_IcmSpixCsInit
* 功 能： Icm SPI的CS控制引脚初始化
* 入口参数： 无
* 出口参数： 无
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注：
*******************************************************************************/
//void bsp_IcmSpixCsInit(void)
//{
// GPIO_InitTypeDef GPIO_InitStruct = {0};
// /* GPIO Ports Clock Enable */
// ICM_RCC_SPIX_CS();
// /*Configure GPIO pins */
// GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
// GPIO_InitStruct.Pull = GPIO_NOPULL;
// GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
// GPIO_InitStruct.Pin = ICM_PIN_SPIX_CS;
// HAL_GPIO_Init(ICM_PORT_SPIX_CS, &GPIO_InitStruct);
// HAL_GPIO_WritePin(ICM_PORT_SPIX_CS, ICM_PIN_SPIX_CS, GPIO_PIN_SET);
//}
/*******************************************************************************
* 名 称： Icm_Spi_ReadWriteNbytes
* 功 能： 使用SPI读写n个字节
* 入口参数： pBuffer: 写入的数组 len:写入数组的长度
* 出口参数： 无
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注：
*******************************************************************************/
static void Icm_Spi_ReadWriteNbytes(uint8_t* pBuffer, uint8_t len)
{
uint8_t i = 0;
#if defined(ICM_USE_HARD_SPI)
for(i = 0; i < len; i ++)
{
// *pBuffer = hal_Spi2_ReadWriteByte(*pBuffer);
*pBuffer = SPI_WriteReadByte(*pBuffer);
pBuffer++;
}
#endif
}
#endif
/*******************************************************************************
* 名 称： icm42688_read_reg
* 功 能： 读取单个寄存器的值
* 入口参数： reg: 寄存器地址
* 出口参数： 当前寄存器地址的值
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注： 使用SPI读取寄存器时要注意:最高位为读写位，详见datasheet page51.
*******************************************************************************/
static uint8_t icm42688_read_reg(uint8_t reg)
{
uint8_t regval = 0;
#if defined(ICM_USE_HARD_SPI)
ICM_SPI_CS_LOW();
// printf("ICM_SPI_CS_LOW\n");
reg |= 0x80;
/* 写入要读的寄存器地址 */
Icm_Spi_ReadWriteNbytes(®, 1);
/* 读取寄存器数据 */
Icm_Spi_ReadWriteNbytes(®val, 1);
ICM_SPI_CS_HIGH();
// printf("ICM_SPI_CS_HIGH\n");
#elif defined(ICM_USE_HARD_I2C)
#endif
return regval;
}
/*******************************************************************************
* 名 称： icm42688_read_regs
* 功 能： 连续读取多个寄存器的值
* 入口参数： reg: 起始寄存器地址 *buf数据指针,uint16_t len长度
* 出口参数： 无
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注： 使用SPI读取寄存器时要注意:最高位为读写位，详见datasheet page50.
*******************************************************************************/
static void icm42688_read_regs(uint8_t reg, uint8_t* buf, uint16_t len)
{
#if defined(ICM_USE_HARD_SPI)
reg |= 0x80;
ICM_SPI_CS_LOW();
/* 写入要读的寄存器地址 */
Icm_Spi_ReadWriteNbytes(®, 1);
/* 读取寄存器数据 */
Icm_Spi_ReadWriteNbytes(buf, len);
ICM_SPI_CS_HIGH();
#elif defined(ICM_USE_HARD_I2C)
#endif
}
/*******************************************************************************
* 名 称： icm42688_write_reg
* 功 能： 向单个寄存器写数据
* 入口参数： reg: 寄存器地址 value:数据
* 出口参数： 0
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注： 使用SPI读取寄存器时要注意:最高位为读写位，详见datasheet page50.
*******************************************************************************/
static uint8_t icm42688_write_reg(uint8_t reg, uint8_t value)
{
#if defined(ICM_USE_HARD_SPI)
ICM_SPI_CS_LOW();
/* 写入要读的寄存器地址 */
Icm_Spi_ReadWriteNbytes(®, 1);
/* 读取寄存器数据 */
Icm_Spi_ReadWriteNbytes(&value, 1);
ICM_SPI_CS_HIGH();
#elif defined(ICM_USE_HARD_I2C)
#endif
return 0;
}
float bsp_Icm42688GetAres(uint8_t Ascale)
{
switch(Ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (11), 4 Gs (10), 8 Gs (01), and 16 Gs (00).
case AFS_2G:
accSensitivity = 2000 / 32768.0f;
break;
case AFS_4G:
accSensitivity = 4000 / 32768.0f;
break;
case AFS_8G:
accSensitivity = 8000 / 32768.0f;
break;
case AFS_16G:
accSensitivity = 16000 / 32768.0f;
break;
}
return accSensitivity;
}
float bsp_Icm42688GetGres(uint8_t Gscale)
{
switch(Gscale)
{
case GFS_15_125DPS:
gyroSensitivity = 15.125f / 32768.0f;
break;
case GFS_31_25DPS:
gyroSensitivity = 31.25f / 32768.0f;
break;
case GFS_62_5DPS:
gyroSensitivity = 62.5f / 32768.0f;
break;
case GFS_125DPS:
gyroSensitivity = 125.0f / 32768.0f;
break;
case GFS_250DPS:
gyroSensitivity = 250.0f / 32768.0f;
break;
case GFS_500DPS:
gyroSensitivity = 500.0f / 32768.0f;
break;
case GFS_1000DPS:
gyroSensitivity = 1000.0f / 32768.0f;
break;
case GFS_2000DPS:
gyroSensitivity = 2000.0f / 32768.0f;
break;
}
return gyroSensitivity;
}
/*******************************************************************************
* 名 称： bsp_Icm42688RegCfg
* 功 能： Icm42688 寄存器配置
* 入口参数： 无
* 出口参数： 无
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注：
*******************************************************************************/
int8_t bsp_Icm42688RegCfg(void)
{
uint8_t reg_val = 0;
/* 读取 who am i 寄存器 */
reg_val = icm42688_read_reg(ICM42688_WHO_AM_I);
printf("reg_val:%d\n",reg_val);
icm42688_write_reg(ICM42688_REG_BANK_SEL, 0); //设置bank 0区域寄存器
icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x01); //软复位传感器
ICM42688DelayMs(100);
if(reg_val == ICM42688_ID)
{
icm42688_write_reg(ICM42688_REG_BANK_SEL, 1); //设置bank 1区域寄存器
icm42688_write_reg(ICM42688_INTF_CONFIG4, 0x02); //设置为4线SPI通信
icm42688_write_reg(ICM42688_REG_BANK_SEL, 0); //设置bank 0区域寄存器
icm42688_write_reg(ICM42688_FIFO_CONFIG, 0x40); //Stream-to-FIFO Mode(page63)
reg_val = icm42688_read_reg(ICM42688_INT_SOURCE0);
icm42688_write_reg(ICM42688_INT_SOURCE0, 0x00);
icm42688_write_reg(ICM42688_FIFO_CONFIG2, 0x00); // watermark
icm42688_write_reg(ICM42688_FIFO_CONFIG3, 0x02); // watermark
icm42688_write_reg(ICM42688_INT_SOURCE0, reg_val);
icm42688_write_reg(ICM42688_FIFO_CONFIG1, 0x63); // Enable the accel and gyro to the FIFO
icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
icm42688_write_reg(ICM42688_INT_CONFIG, 0x36);
icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
reg_val = icm42688_read_reg(ICM42688_INT_SOURCE0);
reg_val |= (1 << 2); //FIFO_THS_INT1_ENABLE
icm42688_write_reg(ICM42688_INT_SOURCE0, reg_val);
bsp_Icm42688GetAres(AFS_8G);
icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
reg_val = icm42688_read_reg(ICM42688_ACCEL_CONFIG0);//page74
reg_val |= (AFS_8G << 5); //量程 ±8g
reg_val |= (AODR_50Hz); //输出速率 50HZ
icm42688_write_reg(ICM42688_ACCEL_CONFIG0, reg_val);
bsp_Icm42688GetGres(GFS_1000DPS);
icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
reg_val = icm42688_read_reg(ICM42688_GYRO_CONFIG0);//page73
reg_val |= (GFS_1000DPS << 5); //量程 ±1000dps
reg_val |= (GODR_50Hz); //输出速率 50HZ
icm42688_write_reg(ICM42688_GYRO_CONFIG0, reg_val);
icm42688_write_reg(ICM42688_REG_BANK_SEL, 0x00);
reg_val = icm42688_read_reg(ICM42688_PWR_MGMT0); //读取PWR—MGMT0当前寄存器的值(page72)
reg_val &= ~(1 << 5);//使能温度测量
reg_val |= ((3) << 2);//设置GYRO_MODE 0:关闭 1:待机 2:预留 3:低噪声
reg_val |= (3);//设置ACCEL_MODE 0:关闭 1:关闭 2:低功耗 3:低噪声
icm42688_write_reg(ICM42688_PWR_MGMT0, reg_val);
ICM42688DelayMs(1); //操作完PWR—MGMT0寄存器后 200us内不能有任何读写寄存器的操作
return 0;
}
return -1;
}
/*******************************************************************************
* 名 称： bsp_Icm42688Init
* 功 能： Icm42688 传感器初始化
* 入口参数： 无
* 出口参数： 0: 初始化成功 其他值: 初始化失败
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注：
*******************************************************************************/
int8_t bsp_Icm42688Init(void)
{
// bsp_IcmSpixCsInit();
SPI_Init();
return(bsp_Icm42688RegCfg());
}
/*******************************************************************************
* 名 称： bsp_IcmGetTemperature
* 功 能： 读取Icm42688 内部传感器温度
* 入口参数： 无
* 出口参数： 无
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注： datasheet page62
*******************************************************************************/
int8_t bsp_IcmGetTemperature(int16_t* pTemp)
{
uint8_t buffer[2] = {0};
icm42688_read_regs(ICM42688_TEMP_DATA1, buffer, 2);
*pTemp = (int16_t)(((int16_t)((buffer[0] << 8) | buffer[1])) / 132.48 + 25);
return 0;
}
/*******************************************************************************
* 名 称： bsp_IcmGetAccelerometer
* 功 能： 读取Icm42688 加速度的值
* 入口参数： 三轴加速度的值
* 出口参数： 无
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注： datasheet page62
*******************************************************************************/
int8_t bsp_IcmGetAccelerometer(icm42688RawData_t* accData)
{
uint8_t buffer[6] = {0};
icm42688_read_regs(ICM42688_ACCEL_DATA_X1, buffer, 6);
accData->x = ((uint16_t)buffer[0] << 8) | buffer[1];
accData->y = ((uint16_t)buffer[2] << 8) | buffer[3];
accData->z = ((uint16_t)buffer[4] << 8) | buffer[5];
accData->x = (int16_t)(accData->x * accSensitivity);
accData->y = (int16_t)(accData->y * accSensitivity);
accData->z = (int16_t)(accData->z * accSensitivity);
return 0;
}
/*******************************************************************************
* 名 称： bsp_IcmGetGyroscope
* 功 能： 读取Icm42688 陀螺仪的值
* 入口参数： 三轴陀螺仪的值
* 出口参数： 无
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注： datasheet page63
*******************************************************************************/
int8_t bsp_IcmGetGyroscope(icm42688RawData_t* GyroData)
{
uint8_t buffer[6] = {0};
icm42688_read_regs(ICM42688_GYRO_DATA_X1, buffer, 6);
GyroData->x = ((uint16_t)buffer[0] << 8) | buffer[1];
GyroData->y = ((uint16_t)buffer[2] << 8) | buffer[3];
GyroData->z = ((uint16_t)buffer[4] << 8) | buffer[5];
GyroData->x = (int16_t)(GyroData->x * gyroSensitivity);
GyroData->y = (int16_t)(GyroData->y * gyroSensitivity);
GyroData->z = (int16_t)(GyroData->z * gyroSensitivity);
return 0;
}
/*******************************************************************************
* 名 称： bsp_IcmGetRawData
* 功 能： 读取Icm42688加速度陀螺仪数据
* 入口参数： 六轴
* 出口参数： 无
* 作 者： Baxiange
* 创建日期： 2022-07-25
* 修 改：
* 修改日期：
* 备 注： datasheet page62,63
*******************************************************************************/
int8_t bsp_IcmGetRawData(icm42688RawData_t* accData, icm42688RawData_t* GyroData)
{
uint8_t buffer[12] = {0};
icm42688_read_regs(ICM42688_ACCEL_DATA_X1, buffer, 12);
accData->x = ((uint16_t)buffer[0] << 8) | buffer[1];
accData->y = ((uint16_t)buffer[2] << 8) | buffer[3];
accData->z = ((uint16_t)buffer[4] << 8) | buffer[5];
GyroData->x = ((uint16_t)buffer[6] << 8) | buffer[7];
GyroData->y = ((uint16_t)buffer[8] << 8) | buffer[9];
GyroData->z = ((uint16_t)buffer[10] << 8) | buffer[11];
accData->x = (int16_t)(accData->x * accSensitivity);
accData->y = (int16_t)(accData->y * accSensitivity);
accData->z = (int16_t)(accData->z * accSensitivity);
GyroData->x = (int16_t)(GyroData->x * gyroSensitivity);
GyroData->y = (int16_t)(GyroData->y * gyroSensitivity);
GyroData->z = (int16_t)(GyroData->z * gyroSensitivity);
return 0;
}
icm42688.h
#ifndef __BSP_ICM42688_H__
#define __BSP_ICM42688_H__
#include <stdint.h>
#define ICM_USE_HARD_SPI
//#define ICM_USE_HARD_I2C
/* ICM42688 registers
https://store.invensense.com/datasheets/invensense/DS-ICM-42688v1-2.pdf
*/
// Bank 0
#define ICM42688_DEVICE_CONFIG 0x11
#define ICM42688_DRIVE_CONFIG 0x13
#define ICM42688_INT_CONFIG 0x14
#define ICM42688_FIFO_CONFIG 0x16
#define ICM42688_TEMP_DATA1 0x1D
#define ICM42688_TEMP_DATA0 0x1E
#define ICM42688_ACCEL_DATA_X1 0x1F
#define ICM42688_ACCEL_DATA_X0 0x20
#define ICM42688_ACCEL_DATA_Y1 0x21
#define ICM42688_ACCEL_DATA_Y0 0x22
#define ICM42688_ACCEL_DATA_Z1 0x23
#define ICM42688_ACCEL_DATA_Z0 0x24
#define ICM42688_GYRO_DATA_X1 0x25
#define ICM42688_GYRO_DATA_X0 0x26
#define ICM42688_GYRO_DATA_Y1 0x27
#define ICM42688_GYRO_DATA_Y0 0x28
#define ICM42688_GYRO_DATA_Z1 0x29
#define ICM42688_GYRO_DATA_Z0 0x2A
#define ICM42688_TMST_FSYNCH 0x2B
#define ICM42688_TMST_FSYNCL 0x2C
#define ICM42688_INT_STATUS 0x2D
#define ICM42688_FIFO_COUNTH 0x2E
#define ICM42688_FIFO_COUNTL 0x2F
#define ICM42688_FIFO_DATA 0x30
#define ICM42688_APEX_DATA0 0x31
#define ICM42688_APEX_DATA1 0x32
#define ICM42688_APEX_DATA2 0x33
#define ICM42688_APEX_DATA3 0x34
#define ICM42688_APEX_DATA4 0x35
#define ICM42688_APEX_DATA5 0x36
#define ICM42688_INT_STATUS2 0x37
#define ICM42688_INT_STATUS3 0x38
#define ICM42688_SIGNAL_PATH_RESET 0x4B
#define ICM42688_INTF_CONFIG0 0x4C
#define ICM42688_INTF_CONFIG1 0x4D
#define ICM42688_PWR_MGMT0 0x4E
#define ICM42688_GYRO_CONFIG0 0x4F
#define ICM42688_ACCEL_CONFIG0 0x50
#define ICM42688_GYRO_CONFIG1 0x51
#define ICM42688_GYRO_ACCEL_CONFIG0 0x52
#define ICM42688_ACCEL_CONFIG1 0x53
#define ICM42688_TMST_CONFIG 0x54
#define ICM42688_APEX_CONFIG0 0x56
#define ICM42688_SMD_CONFIG 0x57
#define ICM42688_FIFO_CONFIG1 0x5F
#define ICM42688_FIFO_CONFIG2 0x60
#define ICM42688_FIFO_CONFIG3 0x61
#define ICM42688_FSYNC_CONFIG 0x62
#define ICM42688_INT_CONFIG0 0x63
#define ICM42688_INT_CONFIG1 0x64
#define ICM42688_INT_SOURCE0 0x65
#define ICM42688_INT_SOURCE1 0x66
#define ICM42688_INT_SOURCE3 0x68
#define ICM42688_INT_SOURCE4 0x69
#define ICM42688_FIFO_LOST_PKT0 0x6C
#define ICM42688_FIFO_LOST_PKT1 0x6D
#define ICM42688_SELF_TEST_CONFIG 0x70
#define ICM42688_WHO_AM_I 0x75
#define ICM42688_REG_BANK_SEL 0x76
// Bank 1
#define ICM42688_SENSOR_CONFIG0 0x03
#define ICM42688_GYRO_CONFIG_STATIC2 0x0B
#define ICM42688_GYRO_CONFIG_STATIC3 0x0C
#define ICM42688_GYRO_CONFIG_STATIC4 0x0D
#define ICM42688_GYRO_CONFIG_STATIC5 0x0E
#define ICM42688_GYRO_CONFIG_STATIC6 0x0F
#define ICM42688_GYRO_CONFIG_STATIC7 0x10
#define ICM42688_GYRO_CONFIG_STATIC8 0x11
#define ICM42688_GYRO_CONFIG_STATIC9 0x12
#define ICM42688_GYRO_CONFIG_STATIC10 0x13
#define ICM42688_XG_ST_DATA 0x5F
#define ICM42688_YG_ST_DATA 0x60
#define ICM42688_ZG_ST_DATA 0x61
#define ICM42688_TMSTVAL0 0x62
#define ICM42688_TMSTVAL1 0x63
#define ICM42688_TMSTVAL2 0x64
#define ICM42688_INTF_CONFIG4 0x7A
#define ICM42688_INTF_CONFIG5 0x7B
#define ICM42688_INTF_CONFIG6 0x7C
// Bank 2
#define ICM42688_ACCEL_CONFIG_STATIC2 0x03
#define ICM42688_ACCEL_CONFIG_STATIC3 0x04
#define ICM42688_ACCEL_CONFIG_STATIC4 0x05
#define ICM42688_XA_ST_DATA 0x3B
#define ICM42688_YA_ST_DATA 0x3C
#define ICM42688_ZA_ST_DATA 0x3D
// Bank 4
#define ICM42688_GYRO_ON_OFF_CONFIG 0x0E
#define ICM42688_APEX_CONFIG1 0x40
#define ICM42688_APEX_CONFIG2 0x41
#define ICM42688_APEX_CONFIG3 0x42
#define ICM42688_APEX_CONFIG4 0x43
#define ICM42688_APEX_CONFIG5 0x44
#define ICM42688_APEX_CONFIG6 0x45
#define ICM42688_APEX_CONFIG7 0x46
#define ICM42688_APEX_CONFIG8 0x47
#define ICM42688_APEX_CONFIG9 0x48
#define ICM42688_ACCEL_WOM_X_THR 0x4A
#define ICM42688_ACCEL_WOM_Y_THR 0x4B
#define ICM42688_ACCEL_WOM_Z_THR 0x4C
#define ICM42688_INT_SOURCE6 0x4D
#define ICM42688_INT_SOURCE7 0x4E
#define ICM42688_INT_SOURCE8 0x4F
#define ICM42688_INT_SOURCE9 0x50
#define ICM42688_INT_SOURCE10 0x51
#define ICM42688_OFFSET_USER0 0x77
#define ICM42688_OFFSET_USER1 0x78
#define ICM42688_OFFSET_USER2 0x79
#define ICM42688_OFFSET_USER3 0x7A
#define ICM42688_OFFSET_USER4 0x7B
#define ICM42688_OFFSET_USER5 0x7C
#define ICM42688_OFFSET_USER6 0x7D
#define ICM42688_OFFSET_USER7 0x7E
#define ICM42688_OFFSET_USER8 0x7F
#define ICM42688_ADDRESS 0x69 // Address of ICM42688 accel/gyro when ADO = HIGH
#define AFS_2G 0x03
#define AFS_4G 0x02
#define AFS_8G 0x01
#define AFS_16G 0x00 // default
#define GFS_2000DPS 0x00 // default
#define GFS_1000DPS 0x01
#define GFS_500DPS 0x02
#define GFS_250DPS 0x03
#define GFS_125DPS 0x04
#define GFS_62_5DPS 0x05
#define GFS_31_25DPS 0x06
#define GFS_15_125DPS 0x07
#define AODR_8000Hz 0x03
#define AODR_4000Hz 0x04
#define AODR_2000Hz 0x05
#define AODR_1000Hz 0x06 // default
#define AODR_200Hz 0x07
#define AODR_100Hz 0x08
#define AODR_50Hz 0x09
#define AODR_25Hz 0x0A
#define AODR_12_5Hz 0x0B
#define AODR_6_25Hz 0x0C
#define AODR_3_125Hz 0x0D
#define AODR_1_5625Hz 0x0E
#define AODR_500Hz 0x0F
#define GODR_8000Hz 0x03
#define GODR_4000Hz 0x04
#define GODR_2000Hz 0x05
#define GODR_1000Hz 0x06 // default
#define GODR_200Hz 0x07
#define GODR_100Hz 0x08
#define GODR_50Hz 0x09
#define GODR_25Hz 0x0A
#define GODR_12_5Hz 0x0B
#define GODR_500Hz 0x0F
#define ICM42688_ID 0x47
typedef struct {
int16_t x; /**< Raw int16_t value from the x axis */
int16_t y; /**< Raw int16_t value from the y axis */
int16_t z; /**< Raw int16_t value from the z axis */
} icm42688RawData_t;
//--------------------------------------------------------//
int8_t bsp_Icm42688Init(void);
int8_t bsp_IcmGetTemperature(int16_t* pTemp);
int8_t bsp_IcmGetAccelerometer(icm42688RawData_t *accData);
int8_t bsp_IcmGetGyroscope(icm42688RawData_t *GyroData);
int8_t bsp_IcmGetRawData(icm42688RawData_t *accData,icm42688RawData_t *GyroData);
#endif
DWT.c
#include "DWT.h"
/*******************************************************************************
* 名 称： 延时
* 功 能： 延时
* 入口参数： _ulDelayTime：延时的时间
* 出口参数： 无
* 作 者： BAXIANGE.
* 创建日期： 2021-05-21
* 修 改：
* 修改日期：
* 备 注：
*******************************************************************************/
uint8_t times_t;
void bsp_InitDWT(void)
{
DEM_CR |= (unsigned int)DEM_CR_TRCENA; /* Enable Cortex-M4's DWT CYCCNT reg. */
DWT_CYCCNT = (unsigned int)0u;
DWT_CR |= (unsigned int)DWT_CR_CYCCNTENA;
}
void bsp_DelayUS(uint32_t _ulDelayTime)
{
uint32_t tCnt, tDelayCnt;
uint32_t tStart;
tStart = DWT_CYCCNT; /* 刚进入时的计数器值 */
tCnt = 0;
tDelayCnt = _ulDelayTime * (SystemCoreClock / 1000000); /* 需要的节拍数 */
while(tCnt < tDelayCnt)
{
tCnt = DWT_CYCCNT - tStart; /* 求减过程中，如果发生第一次32位计数器重新计数，依然可以正确计算 */
}
}
void bsp_DelayMS(uint32_t _ulDelayTime)
{
bsp_DelayUS(1000*_ulDelayTime);
}
/* Sleep implementation */
void inv_imu_sleep_us(uint32_t us)
{
bsp_DelayUS(us);
}
uint32_t inv_imu_get_time_us(void)
{
return OS_TS_GET()/73 + times_t*58835168;
}
uint32_t OS_TS_GET(void)
{
uint32_t _get_ts;
uint32_t _ts;
static uint32_t _ts_bak; /* 时间戳备份 */
// _get_ts = DWT_CYCCNT / 73;
_get_ts = DWT_CYCCNT;
if(_get_ts < _ts_bak)
{
/* 做溢出修正 */
_ts = 0XFFFFFFFF - _ts_bak + _get_ts;
// _ts = 58835168 - _ts_bak + _get_ts;
/* 加上上次数据 即可求出本次时间差*/
_ts = _ts+_ts_bak;
}
else
{
/* 正常情况 */
_ts = _get_ts;
}
_ts_bak = _ts;
return _ts;
}
DWT.h
#include "stm32f1xx_hal.h"
void bsp_InitDWT(void);
void bsp_DelayUS(uint32_t _ulDelayTime);
void bsp_DelayMS(uint32_t _ulDelayTime);
void inv_imu_sleep_us(uint32_t us);
uint32_t inv_imu_get_time_us(void);
uint32_t OS_TS_GET(void);
#define DWT_CYCCNT *(volatile unsigned int *)0xE0001004
#define DWT_CR *(volatile unsigned int *)0xE0001000
#define DEM_CR *(volatile unsigned int *)0xE000EDFC
#define DBGMCU_CR *(volatile unsigned int *)0xE0042004
#define DEM_CR_TRCENA (1 << 24)
#define DWT_CR_CYCCNTENA (1 << 0)
IO_SPI.c
#include "IO_SPI.h"
#include "DWT.h"
/*
* 函数名：void SPI_Init(void)
* 输入参数：
* 输出参数：无
* 返回值：无
* 函数作用：初始化 SPI 的四根引脚
*/
void SPI_Init(void) {
GPIO_InitTypeDef GPIO_InitStruct;
SPIx_SCK_GPIO_CLK_ENABLE();
SPIx_MISO_GPIO_CLK_ENABLE();
SPIx_MOSI_GPIO_CLK_ENABLE();
CS_GPIO_CLK_ENABLE();
GPIO_InitStruct.Pin = SPIx_SCK_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(SPIx_SCK_GPIO_PORT, &GPIO_InitStruct); // SCK CS MOSI 为输出
GPIO_InitStruct.Pin = SPIx_MOSI_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(SPIx_MOSI_GPIO_PORT, &GPIO_InitStruct);
GPIO_InitStruct.Pin = CS_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(CS_GPIO_PORT, &GPIO_InitStruct);
GPIO_InitStruct.Pin = SPIx_MISO_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(SPIx_MISO_GPIO_PORT, &GPIO_InitStruct); // MISO 为输入
CS(1); // CS 初始化高
SPI_CLK(0); // CLK 初始化低
}
/*
*********************************************************************************************************
* 函 数 名: IIC_Delay
* 功能说明: IIC总线位延迟，最快400KHz
* 形 参：无
* 返 回 值: 无
*********************************************************************************************************
*/
//static void IIC_Delay(void)
//{
// uint8_t i;
// /*
// 下面的时间是通过安富莱AX-Pro逻辑分析仪测试得到的。
// CPU主频72MHz时，在内部Flash运行, MDK工程不优化
// 循环次数为10时，SCL频率 = 205KHz
// 循环次数为7时，SCL频率 = 347KHz， SCL高电平时间1.5us，SCL低电平时间2.87us
// 循环次数为5时，SCL频率 = 421KHz， SCL高电平时间1.25us，SCL低电平时间2.375us
// IAR工程编译效率高，不能设置为7
// */
// for (i = 0; i < 10; i++);
//}
/*
*********************************************************************************************************
* 函 数 名: SPI_Delay
* 功能说明: SPI总线位延迟
* 形 参：无
* 返 回 值: 无
*********************************************************************************************************
*/
static void SPI_Delay(void)
{
bsp_DelayUS(1);
}
/*
* 函数名：void SPI_WriteByte(uint8_t data)
* 输入参数：data -> 要写的数据
* 输出参数：无
* 返回值：无
* 函数作用：模拟 SPI 写一个字节
*/
void SPI_WriteByte(uint8_t data)
{
uint8_t i = 0;
uint8_t temp = 0;
for(i=0; i<8; i++) {
temp = ((data&0x80)==0x80)? 1:0;
data = data<<1;
SPI_CLK(0); //CPOL=0
SPI_MOSI(temp);
SPI_Delay();
SPI_CLK(1); //CPHA=0
SPI_Delay(); }
SPI_CLK(0); }
/*
* 函数名：uint8_t SPI_ReadByte(void)
* 输入参数：
* 输出参数：无
* 返回值：读到的数据
* 函数作用：模拟 SPI 读一个字节
*/
uint8_t SPI_ReadByte(void) {
uint8_t i = 0;
uint8_t read_data = 0xFF;
for(i=0; i<8; i++) {
read_data = read_data << 1;
SPI_CLK(0);
SPI_Delay();
SPI_CLK(1);
SPI_Delay();
if(SPI_MISO()==1) {
read_data = read_data + 1; } }
SPI_CLK(0);
return read_data;
}
/*
* 函数名：uint8_t SPI_WriteReadByte(uint8_t data)
* 输入参数：data -> 要写的一个字节数据
* 输出参数：无
* 返回值：读到的数据
* 函数作用：模拟 SPI 读写一个字节
*/
uint8_t SPI_WriteReadByte(uint8_t data)
{
uint8_t i = 0;
uint8_t temp = 0;
uint8_t read_data = 0xFF;
for(i=0;i<8;i++) {
temp = ((data&0x80)==0x80)? 1:0;
data = data<<1;
read_data = read_data<<1;
SPI_CLK(0);
SPI_MOSI(temp);
SPI_Delay();
SPI_CLK(1);
SPI_Delay();
if(SPI_MISO()==1) {
read_data = read_data + 1;
}
}
SPI_CLK(0);
return read_data;
}
IO_SPI.h
#ifndef __IO_SPI_H__
#define __IO_SPI_H__
#include "stm32f1xx_hal.h"
#include <inttypes.h>
/* 定义SPI总线连接的GPIO端口, 用户只需要修改下面4行代码即可任意改变SCL和SDA的引脚 */
#define SPIx SPI1
#define GPIO_PORT_SPI GPIOB /* GPIO端口 */
#define RCC_SPI_ENABLE __HAL_RCC_GPIOB_CLK_ENABLE() /* GPIO端口时钟 */
#define SPIx_SCK_GPIO_CLK_ENABLE() __HAL_RCC_GPIOB_CLK_ENABLE()
#define SPIx_MOSI_GPIO_CLK_ENABLE() __HAL_RCC_GPIOB_CLK_ENABLE()
#define SPIx_MISO_GPIO_CLK_ENABLE() __HAL_RCC_GPIOB_CLK_ENABLE()
#define CS_GPIO_CLK_ENABLE() __HAL_RCC_GPIOB_CLK_ENABLE()
#define SPIx_SCK_PIN GPIO_PIN_13
#define SPIx_SCK_GPIO_PORT GPIOB
#define SPIx_MOSI_PIN GPIO_PIN_15
#define SPIx_MOSI_GPIO_PORT GPIOB
#define CS_PIN GPIO_PIN_12
#define CS_GPIO_PORT GPIOB
#define SPIx_MISO_PIN GPIO_PIN_14
#define SPIx_MISO_GPIO_PORT GPIOB
/* 定义时钟、数据线、片选信号线的宏，增加代码的可移植性和可阅读性 */
#define SPI_CLK(level) HAL_GPIO_WritePin(SPIx_SCK_GPIO_PORT, SPIx_SCK_PIN, level?GPIO_PIN_SET:GPIO_PIN_RESET)
#define SPI_MOSI(level) HAL_GPIO_WritePin(SPIx_MOSI_GPIO_PORT, SPIx_MOSI_PIN, level?GPIO_PIN_SET:GPIO_PIN_RESET)
#define SPI_MISO() HAL_GPIO_ReadPin(SPIx_MISO_GPIO_PORT, SPIx_MISO_PIN)
#define CS(level) HAL_GPIO_WritePin(CS_GPIO_PORT, CS_PIN, level?GPIO_PIN_SET:GPIO_PIN_RESET)
void SPI_Init(void);
void SPI_WriteByte(uint8_t data);
uint8_t SPI_ReadByte(void);
uint8_t SPI_WriteReadByte(uint8_t data);
static void SPI_Delay(void);
//void bsp_DelayUS(uint32_t _ulDelayTime);
//void bsp_DelayMS(uint32_t _ulDelayTime);
#endif
main.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* <h2><center>© Copyright (c) 2022 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
* opensource.org/licenses/BSD-3-Clause
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "icm42688.h"
#include "stdio.h"
#include "DWT.h"
#include "io_spi.h"
#include "icm_426xx_alg.h"
#include "invn_algo_agm.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
int16_t iTemperature = 0;
icm42688RawData_t stAccData;
icm42688RawData_t stGyroData;
InvnAlgoAGMConfig config;
InvnAlgoAGMInput input;
InvnAlgoAGMOutput output;
extern uint8_t times_t;
static uint32_t test_t;
int32_t sRacc_data [3];
int32_t sRgyr_data [3];
int32_t acc_cal_q16 [3];
int32_t gyr_cal_q16 [3];
int rc = 1;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
int fputc(int ch, FILE *fp)
{
while(!(USART1->SR & (1<<7)));
USART1->DR = ch;
return ch;
}
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_SPI2_Init();
MX_USART1_UART_Init();
MX_TIM2_Init();
/* USER CODE BEGIN 2 */
HAL_TIM_Base_Start_IT(&htim2);
bsp_InitDWT();
printf("wroking...\n");
bsp_Icm42688Init();
init_agm_algo();
rc = invn_algo_agm_init(&config);
printf("sucess is 0,fail is 1,rc = %d\n",rc);
invn_algo_agm_set_config(&config);
// invn_algo_agm_version ();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
// bsp_IcmGetTemperature(&iTemperature);
// printf("1.Temp:%d\n",iTemperature);
//
// bsp_IcmGetAccelerometer(&stAccData);
// printf("2.AccX:%d AccY:%d AccZ:%d\n",stAccData.x,stAccData.y,stAccData.z);
// bsp_IcmGetGyroscope(&stGyroData);
// printf("3.GyroX:%d GyroY:%d GyroZ:%d\n",stGyroData.x,stGyroData.y,stGyroData.z);
// bsp_IcmGetRawData(&stAccData,&stGyroData);
// printf("4.AccX:%d AccY:%d AccZ:%d GyroX:%d GyroY:%d GyroZ:%d\n",stAccData.x,stAccData.y,stAccData.z,stGyroData.x,stGyroData.y,stGyroData.z);
// bsp_IcmGetTemperature(&iTemperature);
// printf("Temp:%d\n",iTemperature);
bsp_IcmGetRawData(&stAccData,&stGyroData);
printf("AccX:%d--AccY:%d--AccZ:%d----GyroX:%d--GyroY:%d--GyroZ:%d\n",stAccData.x,stAccData.y,stAccData.z,stGyroData.x,stGyroData.y,stGyroData.z);
input.sRacc_data[0] = stAccData.x;
input.sRacc_data[1] = stAccData.y;
input.sRacc_data[2] = stAccData.z;
input.sRgyr_data[0] = stGyroData.x;
input.sRgyr_data[1] = stGyroData.y;
input.sRgyr_data[2] = stGyroData.z;
invn_algo_agm_process (&input, &output);
printf("X:%d--Y:%d--Z:%d---temp:%d\n",output.grv_quat_q30[0],output.grv_quat_q30[1],output.grv_quat_q30[2],output.grv_quat_q30[3]);
HAL_Delay(1000);
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
HAL_RCC_MCOConfig(RCC_MCO, RCC_MCO1SOURCE_PLLCLK, RCC_MCODIV_1);
}
/* USER CODE BEGIN 4 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim->Instance == htim2.Instance)
{
if(DWT_CYCCNT < test_t)
{
if(times_t > 72)
{
times_t = 0;
}else{
times_t++;
}
test_t = DWT_CYCCNT;
}else{
test_t = DWT_CYCCNT;
}
}
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
文章来源地址https://www.toymoban.com/news/detail-781547.html
文章来源：https://www.toymoban.com/news/detail-781547.html
到了这里，关于ICM42688笔记的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
