motor/Common/spi/Imu_ICM20600.c

/*
 * 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;
}