串口ok
This commit is contained in:
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
* Acc_ICM20600.c
|
||||
*
|
||||
* Created on: July 12, 2020
|
||||
* Author: lqh
|
||||
*/
|
||||
#include "Imu_ICM20600.h"
|
||||
#include "main.h"
|
||||
|
||||
/* ICM20600 registers addresses */
|
||||
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：
|
||||
#define CONFIG 0x1A //低通滤波频率，典型值：
|
||||
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：
|
||||
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值
|
||||
#define ACCEL_CONFIG1 0x1D //加速计自检、测量范围及高通滤波频率，典型值:
|
||||
#define FIFO_EN 0x23
|
||||
#define FIFO_WM_TH1 0x60
|
||||
#define FIFO_WM_TH2 0x61
|
||||
#define FIFO_COUNTH 0x72
|
||||
#define FIFO_COUNTL 0x73
|
||||
#define FIFO_R_W 0x74
|
||||
|
||||
#define ACCEL_XOUT_H 0x3B //加速度计 x轴加速度 高8位
|
||||
#define ACCEL_XOUT_L 0x3C //加速度计 x轴加速度 低8位
|
||||
#define ACCEL_YOUT_H 0x3D //加速度计 y轴加速度 高8位
|
||||
#define ACCEL_YOUT_L 0x3E //加速度计 y轴加速度 低8位
|
||||
#define ACCEL_ZOUT_H 0x3F //加速度计 z轴加速度 高8位
|
||||
#define ACCEL_ZOUT_L 0x40 //加速度计 z轴加速度 低8位
|
||||
#define TEMP_OUT_H 0x41 //温度 高8位
|
||||
#define TEMP_OUT_L 0x42 //温度 低8位
|
||||
#define GYRO_XOUT_H 0x43 //陀螺仪 x轴陀螺仪 高8位
|
||||
#define GYRO_XOUT_L 0x44 //陀螺仪 x轴陀螺仪 低8位
|
||||
#define GYRO_YOUT_H 0x45 //陀螺仪 y轴陀螺仪 高8位
|
||||
#define GYRO_YOUT_L 0x46 //陀螺仪 y轴陀螺仪 低8位
|
||||
#define GYRO_ZOUT_H 0x47 //陀螺仪 z轴陀螺仪 高8位
|
||||
#define GYRO_ZOUT_L 0x48 //陀螺仪 z轴陀螺仪 低8位
|
||||
|
||||
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x11，只读)
|
||||
#define USER_CONTROL 0x6A //0x10 I2C_IF_DIS ENABLE
|
||||
#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
|
||||
|
||||
// WHOAMI values
|
||||
#define MPU_WHOAMI_20600 0x11
|
||||
|
||||
|
||||
/** 初始化ADXL加速度计*/
|
||||
int Imu_ICM20600_init(Imu_ICM20600_t *Imu, const char *name, SPI_DEV_t *dev,
|
||||
GPIO_EXIT_t *exti) {
|
||||
|
||||
uint8_t ChipId; //Read ICM20600 Chip Infomation
|
||||
int rslt;
|
||||
uint8_t TxdBuf[4];
|
||||
uint8_t RxdBuf[4];
|
||||
|
||||
rslt = 0;
|
||||
|
||||
Imu->name = name;
|
||||
Imu->_dev = dev;
|
||||
Imu->exti = exti;
|
||||
|
||||
Imu->Accx = 0;
|
||||
Imu->Accy = 0;
|
||||
Imu->Accz = 0;
|
||||
|
||||
Imu->Gyrox = 0;
|
||||
Imu->Gyroy = 0;
|
||||
Imu->Gyroz = 0;
|
||||
|
||||
Imu->At = 0;
|
||||
Imu->read_cnt = 0;
|
||||
Imu->exti->cnt = 0;
|
||||
|
||||
Imu->cnt = 0;
|
||||
Imu->last_cnt = 0;
|
||||
Imu->pps = 0;
|
||||
|
||||
Imu->acc_lsb_per_g = 2048.0f;
|
||||
Imu->gyro_lsb_per_deg_sec = 16.38;
|
||||
|
||||
SPI_DEV_begin(Imu->_dev, -1);
|
||||
|
||||
/* Quick verification test for boards */
|
||||
TxdBuf[0] = 0x80 | WHO_AM_I; //Read ChipId
|
||||
TxdBuf[1] = 0x00; //Read ChipId
|
||||
SPI_DEV_select(dev);
|
||||
SPI_DEV_transfer(dev, TxdBuf, 2, RxdBuf, 2, 2);
|
||||
SPI_DEV_unselect(dev);
|
||||
|
||||
ChipId = RxdBuf[1];
|
||||
|
||||
//If Target Chip is ICM20600, Set Chip Range & Turn on Accelerometer Mode
|
||||
if (ChipId == MPU_WHOAMI_20600) {
|
||||
|
||||
//开始配置
|
||||
TxdBuf[0] = PWR_MGMT_1; //Write ICM20600 PWR_MGMT_1
|
||||
TxdBuf[1] = 0x00; //reset imu
|
||||
SPI_DEV_select(dev);
|
||||
SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
|
||||
SPI_DEV_unselect(dev);
|
||||
|
||||
TxdBuf[0] = ACCEL_CONFIG; //Write ICM20600 ACCEL_CONFIG
|
||||
TxdBuf[1] = 0x18; //ACC ±16G
|
||||
SPI_DEV_select(dev);
|
||||
SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
|
||||
SPI_DEV_unselect(dev);
|
||||
|
||||
TxdBuf[0] = CONFIG; //Write ICM20600 CONFIG
|
||||
TxdBuf[1] = 0x07; //Gyroscope Rate 8k hz
|
||||
SPI_DEV_select(dev);
|
||||
SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
|
||||
SPI_DEV_unselect(dev);
|
||||
|
||||
TxdBuf[0] = SMPLRT_DIV; //Write ICM20600 SMPLRT_DIV
|
||||
TxdBuf[1] = 0x00; //INTERNAL_SAMPLE_RATE = 1kHz
|
||||
SPI_DEV_select(dev);
|
||||
SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
|
||||
SPI_DEV_unselect(dev);
|
||||
|
||||
TxdBuf[0] = GYRO_CONFIG; //Write ICM20600 GYRO_CONFIG
|
||||
TxdBuf[1] = 0x18; //Gyro ±2000 dps
|
||||
SPI_DEV_select(dev);
|
||||
SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
|
||||
SPI_DEV_unselect(dev);
|
||||
|
||||
TxdBuf[0] = ACCEL_CONFIG1; //Write ICM20600 ACCEL_CONFIG1
|
||||
TxdBuf[1] = 0x08; //Accelerometer rate 1k hz
|
||||
SPI_DEV_select(dev);
|
||||
SPI_DEV_transfer(dev, TxdBuf, 2, NULL, 0, 2);
|
||||
SPI_DEV_unselect(dev);
|
||||
|
||||
} else {
|
||||
rslt = -1;
|
||||
}
|
||||
SPI_DEV_end(Imu->_dev);
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/** 读取ICM-20600值 */
|
||||
bool Imu_ICM20600_update(Imu_ICM20600_t *Imu) {
|
||||
uint8_t TxdBuf[15];
|
||||
uint8_t RxdBuf[15];
|
||||
int i;
|
||||
int16_t TempAcceleratorData;
|
||||
bool rslt;
|
||||
|
||||
if (SPI_DEV_begin(Imu->_dev, 0)) //进入临界区
|
||||
{
|
||||
TxdBuf[0] = ((ACCEL_XOUT_H | 0x80)); //Read
|
||||
for (i = 1; i < 15; i++) {
|
||||
TxdBuf[i] = 0x00; //Dummy Byte 虚拟字节
|
||||
}
|
||||
SPI_DEV_select(Imu->_dev);
|
||||
rslt = SPI_DEV_transfer(Imu->_dev, TxdBuf, 15, RxdBuf, 15, 3);
|
||||
SPI_DEV_unselect(Imu->_dev);
|
||||
|
||||
SPI_DEV_end(Imu->_dev); //退出临界区
|
||||
|
||||
Imu->exti->cnt = 0; //中断计数清零
|
||||
|
||||
if (rslt) {
|
||||
Imu->cnt++;
|
||||
taskENTER_CRITICAL();
|
||||
Imu->read_cnt++;
|
||||
|
||||
/** Data combination for Acc Raw Data */
|
||||
TempAcceleratorData = ((RxdBuf[1] << 8) | (RxdBuf[2]));
|
||||
if ((TempAcceleratorData & 0x0800) != 0) //Sign-extending for raw data
|
||||
{
|
||||
TempAcceleratorData |= 0xF000;
|
||||
}
|
||||
Imu->Accx = (float)TempAcceleratorData/Imu->acc_lsb_per_g; //Accumulated Value of ax x轴
|
||||
|
||||
//Data combination forAy Raw Data
|
||||
TempAcceleratorData = ((RxdBuf[3] << 8) | (RxdBuf[4]));
|
||||
if ((TempAcceleratorData & 0x0800) != 0) //Sign-extending for raw data
|
||||
{
|
||||
TempAcceleratorData |= 0xF000;
|
||||
}
|
||||
Imu->Accy = (float)TempAcceleratorData/Imu->acc_lsb_per_g; //Accumulated Value of ay y轴
|
||||
|
||||
//Data combination for Az Raw Data
|
||||
TempAcceleratorData = ((RxdBuf[5] << 8) | (RxdBuf[6]));
|
||||
if ((TempAcceleratorData & 0x0800) != 0) //Sign-extending for raw data
|
||||
{
|
||||
TempAcceleratorData |= 0xF000;
|
||||
}
|
||||
Imu->Accz = (float)TempAcceleratorData/Imu->acc_lsb_per_g; //Accumulated Value of az z轴
|
||||
|
||||
//Data combination for Temperature Raw Data
|
||||
TempAcceleratorData = ((RxdBuf[7] << 8) | RxdBuf[8]);
|
||||
if ((TempAcceleratorData & 0x0800) != 0) //Sign-extending for raw data
|
||||
{
|
||||
TempAcceleratorData |= 0xF000;
|
||||
}
|
||||
Imu->At = (float)((double)TempAcceleratorData/333.87+21); // IMU Value of Temperature 温度
|
||||
|
||||
|
||||
/** Data combination for Gyro Raw Data */
|
||||
TempAcceleratorData = ((RxdBuf[9] << 8) | (RxdBuf[10]));
|
||||
if ((TempAcceleratorData & 0x0800) != 0) //Sign-extending for raw data
|
||||
{
|
||||
TempAcceleratorData |= 0xF000;
|
||||
}
|
||||
Imu->Gyrox = (float)TempAcceleratorData/Imu->gyro_lsb_per_deg_sec; //Gyro Value of ax x轴
|
||||
|
||||
//Data combination forAy Raw Data
|
||||
TempAcceleratorData = ((RxdBuf[11] << 8) | (RxdBuf[12]));
|
||||
if ((TempAcceleratorData & 0x0800) != 0) //Sign-extending for raw data
|
||||
{
|
||||
TempAcceleratorData |= 0xF000;
|
||||
}
|
||||
Imu->Gyroy = (float)TempAcceleratorData/Imu->gyro_lsb_per_deg_sec; //Gyro Value of ay y轴
|
||||
|
||||
//Data combination for Az Raw Data
|
||||
TempAcceleratorData = ((RxdBuf[13] << 8) | (RxdBuf[14]));
|
||||
if ((TempAcceleratorData & 0x0800) != 0) //Sign-extending for raw data
|
||||
{
|
||||
TempAcceleratorData |= 0xF000;
|
||||
}
|
||||
Imu->Gyroz = (float)TempAcceleratorData/Imu->gyro_lsb_per_deg_sec; //Gyro Value of az z轴
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
} else {
|
||||
rslt = false;
|
||||
}
|
||||
|
||||
return rslt;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Imu_ICM20600_stats(Imu_ICM20600_t *Imu) {
|
||||
Imu->pps = Imu->cnt - Imu->last_cnt;
|
||||
Imu->last_cnt = Imu->cnt;
|
||||
}
|
||||
Reference in New Issue
Block a user