759 lines
21 KiB
C
759 lines
21 KiB
C
#include "icm42688p.h"
|
||
|
||
|
||
#define ICM42xxx_HSPI hspi2
|
||
#define ICM42xxx_BUFFER_LEN 16
|
||
|
||
sGyroConfigStatic9_t gyroConfigStatic9;
|
||
sGyroConfigStatic5_t gyroConfigStatic5;
|
||
sGyroConfigStatic2_t gyroConfigStatic2;
|
||
|
||
sAccelConfigStatic2_t accelConfigStatic2;
|
||
sAccelConfigStatic4_t accelConfigStatic4;
|
||
|
||
sGyroConfig1_t gyroConfig1;
|
||
sGyroAccelConfig0_t gyroAccelConfig0;
|
||
sGyroConfig0_t gyroConfig0;
|
||
sAccelConfig1_t accelConfig1;
|
||
sAccelConfig0_t accelConfig0;
|
||
|
||
|
||
//float _gyroRange;
|
||
//float _accelRange;
|
||
|
||
///\brief Conversion formula to get temperature in Celsius (Sec 4.13)
|
||
static float TEMP_DATA_REG_SCALE = 132.48f;
|
||
static float TEMP_OFFSET = 25.0f;
|
||
|
||
///\brief Accel calibration简短加速校准
|
||
float _accBD[3] = {0};
|
||
float _accB[3] = {0};
|
||
float _accS[3] = {1.0f, 1.0f, 1.0f};
|
||
float _accMax[3] = {0};
|
||
float _accMin[3] = {0};
|
||
|
||
// data buffer
|
||
float _t = 0.0f;
|
||
float _acc[3] = {0};
|
||
float _gyr[3] = {0};
|
||
float _acc1[3] = {0};
|
||
float _gyr1[3] = {0};
|
||
|
||
///\brief Gyro calibration陀螺简易校准
|
||
float _gyroBD[3] = {0};
|
||
float _gyrB[3] = {0};
|
||
|
||
static int NUM_CALIB_SAMPLES = 1000; ///< for gyro/accel bias calib用于陀螺/加速度偏置校准
|
||
|
||
///\brief Full scale resolution factors简短的全比例尺分辨率因子
|
||
float _accelScale = 0.0f;
|
||
float _gyroScale = 0.0f;
|
||
|
||
///\brief Full scale selections简短的全尺寸选择
|
||
enum AccelFS _accelFS;
|
||
enum GyroFS _gyroFS;
|
||
|
||
|
||
uint8_t icm42xxx_spi_tx [ICM42xxx_BUFFER_LEN] ={0xff};
|
||
uint8_t icm42xxx_spi_rx [ICM42xxx_BUFFER_LEN] ={0xff};
|
||
// buffer for reading from sensor
|
||
uint8_t _buffer[15] = {0};
|
||
uint8_t _bank = 0; ///< current user bank
|
||
|
||
|
||
|
||
|
||
/* writes a byte to ICM42688 register given a register address and data */
|
||
int writeRegister(uint8_t subAddress, uint8_t data)
|
||
{
|
||
SPI_CS_Enable(); //将片选线置低，告诉从机开始通信
|
||
|
||
icm42xxx_spi_tx[0] = subAddress & 0x7F; // 写入时，寄存器地址字节的第一位为 0；0x7f <-> 01111111
|
||
icm42xxx_spi_tx[1] = data; // MOSI上第二个字节为写入的数据
|
||
|
||
HAL_SPI_TransmitReceive(&ICM42xxx_HSPI,&icm42xxx_spi_tx[0],&icm42xxx_spi_rx[0], 2,0XFFFF);
|
||
|
||
SPI_CS_Disable(); //将片选线重新置高，告诉从机结束通信
|
||
|
||
ICM42688_readRegisters(subAddress,_buffer,1);
|
||
/* check the read back register against the written register */
|
||
if(_buffer[0] == data)
|
||
{
|
||
return 1;
|
||
}
|
||
else
|
||
{
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
int setBank(uint8_t bank)
|
||
{
|
||
// if we are already on this bank, bail
|
||
if (_bank == bank) return 1;
|
||
|
||
_bank = bank;
|
||
|
||
return writeRegister(REG_BANK_SEL, bank);
|
||
}
|
||
|
||
|
||
void ICM42688Reset() //复位
|
||
{
|
||
setBank(0);
|
||
writeRegister(UB0_REG_DEVICE_CONFIG, 0x01);
|
||
// wait for ICM42688 to come back up
|
||
HAL_Delay(1);
|
||
}
|
||
|
||
/* reads registers from ICM42688 given a starting register address, number of bytes, and a pointer to store data */
|
||
int ICM42688_readRegisters(uint8_t subAddress, uint8_t* dest,uint8_t len)
|
||
{
|
||
if(len>ICM42xxx_BUFFER_LEN-1)
|
||
return -1; // 超出缓存空间！
|
||
|
||
/* write data to device */
|
||
SPI_CS_Enable(); //将片选线置低，告诉从机开始通信
|
||
|
||
icm42xxx_spi_tx[0] = subAddress | 0x80; // 读取时，寄存器地址字节的第一位为 1
|
||
memset(icm42xxx_spi_tx+1,0x00,len); // 接收数据时，注意MOSI上不能再发地址，否则会破坏突发读的地址连续性
|
||
if( HAL_SPI_TransmitReceive(&ICM42xxx_HSPI,&icm42xxx_spi_tx[0],&icm42xxx_spi_rx[0], len+1,0XFF)!= HAL_OK) //因为先发地址后读数据，因此这里长度是len+1
|
||
{
|
||
return -2; // SPI读取操作出错
|
||
}
|
||
|
||
SPI_CS_Disable(); //将片选线重新置高，告诉从机结束通信
|
||
memcpy(dest,icm42xxx_spi_rx+1,len);
|
||
|
||
return 0;
|
||
|
||
}
|
||
|
||
|
||
///* sets the accelerometer full scale range to values other than default *///将加速度计满量程设置为默认值*/以外的值
|
||
int setAccelFS(enum AccelFS fssel)
|
||
{
|
||
|
||
setBank(0);
|
||
|
||
// read current register value
|
||
uint8_t reg;
|
||
if (ICM42688_readRegisters(UB0_REG_ACCEL_CONFIG0,®,1) < 0) return -1;
|
||
|
||
// only change FS_SEL in reg
|
||
reg = (fssel << 5) | (reg & 0x1F);
|
||
|
||
if (writeRegister(UB0_REG_ACCEL_CONFIG0, reg) < 0) return -2;
|
||
|
||
_accelScale = (float)(1 << (4 - fssel)) / 32768.0f;
|
||
_accelFS = fssel;
|
||
|
||
return 1;
|
||
}
|
||
|
||
|
||
/* sets the gyro full scale range to values other than default */
|
||
int setGyroFS(enum GyroFS fssel) {
|
||
|
||
|
||
setBank(0);
|
||
|
||
// read current register value
|
||
uint8_t reg;
|
||
if (ICM42688_readRegisters(UB0_REG_GYRO_CONFIG0,®,1) < 0) return -1;
|
||
|
||
// only change FS_SEL in reg
|
||
reg = (fssel << 5) | (reg & 0x1F);
|
||
|
||
if (writeRegister(UB0_REG_GYRO_CONFIG0, reg) < 0) return -2;
|
||
|
||
_gyroScale = (2000.0f / (float)(1 << fssel)) / 32768.0f;
|
||
_gyroFS = fssel;
|
||
return 1;
|
||
}
|
||
|
||
int setFilters(uint8_t gyroFilters, uint8_t accFilters)
|
||
{
|
||
if (setBank(1) < 0) return -1;
|
||
|
||
if (gyroFilters == 1)
|
||
{
|
||
if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, GYRO_NF_ENABLE | GYRO_AAF_ENABLE) < 0)
|
||
{
|
||
return -2;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, GYRO_NF_DISABLE | GYRO_AAF_DISABLE) < 0)
|
||
{
|
||
return -3;
|
||
}
|
||
}
|
||
|
||
if (setBank(2) < 0) return -4;
|
||
|
||
if (accFilters == 1)
|
||
{
|
||
if (writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, ACCEL_AAF_ENABLE) < 0)
|
||
{
|
||
return -5;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, ACCEL_AAF_DISABLE) < 0)
|
||
{
|
||
return -6;
|
||
}
|
||
}
|
||
if (setBank(0) < 0) return -7;
|
||
return 1;
|
||
}
|
||
|
||
|
||
/* reads the most current data from ICM42688 and stores in buffer */
|
||
int getAGT(void)
|
||
{
|
||
|
||
// grab the data from the ICM42688
|
||
if (ICM42688_readRegisters(UB0_REG_TEMP_DATA1,_buffer,14) < 0) return -1;
|
||
|
||
// combine bytes into 16 bit values
|
||
int16_t rawMeas[7]; // temp, accel xyz, gyro xyz
|
||
for (size_t i=0; i<7; i++) {
|
||
rawMeas[i] = ((int16_t)_buffer[i*2] << 8) | _buffer[i*2+1];
|
||
}
|
||
|
||
_t = ((float)(rawMeas[0]) / TEMP_DATA_REG_SCALE) + TEMP_OFFSET;
|
||
|
||
_acc[0] = ((rawMeas[1] * _accelScale) - _accB[0]) * _accS[0];
|
||
_acc[1] = ((rawMeas[2] * _accelScale) - _accB[1]) * _accS[1];
|
||
_acc[2] = ((rawMeas[3] * _accelScale) - _accB[2]) * _accS[2];
|
||
|
||
_acc1[0] = (rawMeas[1] - _accB[0]) * _accS[0];
|
||
_acc1[1] = (rawMeas[2] - _accB[1]) * _accS[1];
|
||
_acc1[2] = (rawMeas[3] - _accB[2]) * _accS[2];
|
||
|
||
_gyr[0] = (rawMeas[4] * _gyroScale) - _gyrB[0];
|
||
_gyr[1] = (rawMeas[5] * _gyroScale) - _gyrB[1];
|
||
_gyr[2] = (rawMeas[6] * _gyroScale) - _gyrB[2];
|
||
|
||
_gyr1[0] = rawMeas[4] - _gyrB[0];
|
||
_gyr1[1] = rawMeas[5] - _gyrB[1];
|
||
_gyr1[2] = rawMeas[6] - _gyrB[2];
|
||
|
||
return 1;
|
||
}
|
||
|
||
|
||
/* estimates the gyro biases *///估计陀螺偏差
|
||
int calibrateGyro(void) {
|
||
// set at a lower range (more resolution) since IMU not moving设置在较低的范围(更高的分辨率)，因为IMU不移动
|
||
const enum GyroFS current_fssel = _gyroFS;
|
||
if (setGyroFS(dps250) < 0) return -1;
|
||
|
||
// take samples and find bias抽取样本，找出偏差
|
||
_gyroBD[0] = 0;
|
||
_gyroBD[1] = 0;
|
||
_gyroBD[2] = 0;
|
||
for (size_t i=0; i < NUM_CALIB_SAMPLES; i++) {
|
||
getAGT();
|
||
_gyroBD[0] += (gyrX() + _gyrB[0]) / NUM_CALIB_SAMPLES;
|
||
_gyroBD[1] += (gyrY() + _gyrB[1]) / NUM_CALIB_SAMPLES;
|
||
_gyroBD[2] += (gyrZ() + _gyrB[2]) / NUM_CALIB_SAMPLES;
|
||
HAL_Delay(1);
|
||
}
|
||
_gyrB[0] = _gyroBD[0];
|
||
_gyrB[1] = _gyroBD[1];
|
||
_gyrB[2] = _gyroBD[2];
|
||
|
||
// recover the full scale setting
|
||
if (setGyroFS(current_fssel) < 0) return -4;
|
||
return 1;
|
||
}
|
||
|
||
|
||
void setGyroNotchFilterFHz(double freq, uint8_t axis) //設置陷波器
|
||
{
|
||
uint8_t bank = 1;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
double fdesired = freq * 1000;
|
||
double coswz = cos(2 * 3.14 * fdesired / 32);
|
||
int16_t nfCoswz;
|
||
uint8_t nfCoswzSel;
|
||
if (fabs(coswz) <= 0.875) {
|
||
nfCoswz = round(coswz * 256);
|
||
nfCoswzSel = 0;
|
||
} else {
|
||
nfCoswzSel = 1;
|
||
if (coswz > 0.875) {
|
||
nfCoswz = round(8 * (1 - coswz) * 256);
|
||
} else if (coswz < -0.875) {
|
||
nfCoswz = round(-8 * (1 + coswz) * 256);
|
||
}
|
||
}
|
||
uint8_t * _pBuf;
|
||
if (axis == X_AXIS) {
|
||
gyroConfigStatic9.gyroNFCoswzSelX = nfCoswzSel;
|
||
gyroConfigStatic9.gyroNFCoswzX8 = nfCoswz >> 8;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC6, nfCoswz);
|
||
_pBuf = (uint8_t *) &gyroConfigStatic9;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, _pBuf[0]);
|
||
} else if (axis == Y_AXIS) {
|
||
gyroConfigStatic9.gyroNFCoswzSelY = nfCoswzSel;
|
||
gyroConfigStatic9.gyroNFCoswzY8 = nfCoswz >> 8;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC7, nfCoswz);
|
||
_pBuf = (uint8_t *) &gyroConfigStatic9;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, _pBuf[0]);
|
||
} else if (axis == Z_AXIS) {
|
||
gyroConfigStatic9.gyroNFCoswzSelZ = nfCoswzSel;
|
||
gyroConfigStatic9.gyroNFCoswzZ8 = nfCoswz >> 8;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC8, nfCoswz);
|
||
_pBuf = (uint8_t *) &gyroConfigStatic9;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, _pBuf[0]);
|
||
} else if (axis == ALL)
|
||
{
|
||
gyroConfigStatic9.gyroNFCoswzSelX = nfCoswzSel;
|
||
gyroConfigStatic9.gyroNFCoswzX8 = nfCoswz >> 8;
|
||
gyroConfigStatic9.gyroNFCoswzSelY = nfCoswzSel;
|
||
gyroConfigStatic9.gyroNFCoswzY8 = nfCoswz >> 8;
|
||
gyroConfigStatic9.gyroNFCoswzSelZ = nfCoswzSel;
|
||
gyroConfigStatic9.gyroNFCoswzZ8 = nfCoswz >> 8;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC6, nfCoswz);
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC7, nfCoswz);
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC8, nfCoswz);
|
||
_pBuf = (uint8_t *) &gyroConfigStatic9;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, _pBuf[0]);
|
||
}
|
||
bank = 0;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
}
|
||
|
||
void setGyroNFbandwidth(uint8_t bw)
|
||
{
|
||
uint8_t bank = 1;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
uint8_t bandWidth = (bw << 4) | 0x01;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC10, bandWidth );
|
||
bank = 0;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
}
|
||
|
||
|
||
void setGyroNotchFilter(uint8_t mode)
|
||
{
|
||
if (mode==1) {
|
||
gyroConfigStatic2.gyroNFDis = 0;
|
||
} else {
|
||
gyroConfigStatic2.gyroNFDis = 1;
|
||
}
|
||
uint8_t bank = 1;
|
||
uint8_t * _pBuf;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
_pBuf = (uint8_t *) &gyroConfigStatic9;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, _pBuf[0] );
|
||
bank = 0;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
}
|
||
|
||
|
||
|
||
void setAAFBandwidth(uint8_t who, uint8_t BWIndex) //設置二階濾波器帶寬
|
||
{
|
||
uint8_t bank = 0;
|
||
uint8_t * _pBuf;
|
||
uint16_t AAFDeltsqr = BWIndex * BWIndex;
|
||
if (who == GYRO) {
|
||
bank = 1;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC3, BWIndex);
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC4, AAFDeltsqr);
|
||
gyroConfigStatic5.gyroAAFDeltsqr = AAFDeltsqr >> 8;
|
||
if (BWIndex == 1) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 15;
|
||
} else if (BWIndex == 2) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 13;
|
||
} else if (BWIndex == 3) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 12;
|
||
} else if (BWIndex == 4) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 11;
|
||
} else if (BWIndex == 5 || BWIndex == 6) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 10;
|
||
} else if (BWIndex > 6 && BWIndex < 10) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 9;
|
||
} else if (BWIndex > 9 && BWIndex < 14) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 8;
|
||
} else if (BWIndex > 13 && BWIndex < 19) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 7;
|
||
} else if (BWIndex > 18 && BWIndex < 27) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 6;
|
||
} else if (BWIndex > 26 && BWIndex < 37) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 5;
|
||
} else if (BWIndex > 36 && BWIndex < 53) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 4;
|
||
} else if (BWIndex > 53 && BWIndex <= 63) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 3;
|
||
}
|
||
_pBuf = (uint8_t *) &gyroConfigStatic5;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC5, _pBuf[0]);
|
||
bank = 0;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
} else if (who == ACCEL) {
|
||
bank = 2;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
accelConfigStatic2.accelAAFDelt = BWIndex;
|
||
_pBuf = (uint8_t *) &accelConfigStatic2;
|
||
writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, _pBuf[0]);
|
||
writeRegister(UB2_REG_ACCEL_CONFIG_STATIC3, AAFDeltsqr);
|
||
accelConfigStatic4.accelAAFDeltsqr = AAFDeltsqr >> 8;
|
||
if (BWIndex == 1) {
|
||
accelConfigStatic4.accelAAFBitshift = 15;
|
||
} else if (BWIndex == 2) {
|
||
accelConfigStatic4.accelAAFBitshift = 13;
|
||
} else if (BWIndex == 3) {
|
||
accelConfigStatic4.accelAAFBitshift = 12;
|
||
} else if (BWIndex == 4) {
|
||
accelConfigStatic4.accelAAFBitshift = 11;
|
||
} else if (BWIndex == 5 || BWIndex == 6) {
|
||
accelConfigStatic4.accelAAFBitshift = 10;
|
||
} else if (BWIndex > 6 && BWIndex < 10) {
|
||
accelConfigStatic4.accelAAFBitshift = 9;
|
||
} else if (BWIndex > 9 && BWIndex < 14) {
|
||
accelConfigStatic4.accelAAFBitshift = 8;
|
||
} else if (BWIndex > 13 && BWIndex < 19) {
|
||
accelConfigStatic4.accelAAFBitshift = 7;
|
||
} else if (BWIndex > 18 && BWIndex < 27) {
|
||
accelConfigStatic4.accelAAFBitshift = 6;
|
||
} else if (BWIndex > 26 && BWIndex < 37) {
|
||
accelConfigStatic4.accelAAFBitshift = 5;
|
||
} else if (BWIndex > 36 && BWIndex < 53) {
|
||
accelConfigStatic4.accelAAFBitshift = 4;
|
||
} else if (BWIndex > 53 && BWIndex <= 63) {
|
||
accelConfigStatic4.accelAAFBitshift = 3;
|
||
}
|
||
_pBuf = (uint8_t *) &accelConfigStatic4;
|
||
writeRegister(UB2_REG_ACCEL_CONFIG_STATIC4, _pBuf[0]);
|
||
|
||
bank = 0;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
} else if (who == ALL) {
|
||
bank = 1;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC3, BWIndex );
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC4, AAFDeltsqr );
|
||
gyroConfigStatic5.gyroAAFDeltsqr = AAFDeltsqr >> 8;
|
||
if (BWIndex == 1) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 15;
|
||
} else if (BWIndex == 2) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 13;
|
||
} else if (BWIndex == 3) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 12;
|
||
} else if (BWIndex == 4) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 11;
|
||
} else if (BWIndex == 5 || BWIndex == 6) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 10;
|
||
} else if (BWIndex > 6 && BWIndex < 10) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 9;
|
||
} else if (BWIndex > 9 && BWIndex < 14) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 8;
|
||
} else if (BWIndex > 13 && BWIndex < 19) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 7;
|
||
} else if (BWIndex > 18 && BWIndex < 27) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 6;
|
||
} else if (BWIndex > 26 && BWIndex < 37) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 5;
|
||
} else if (BWIndex > 36 && BWIndex < 53) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 4;
|
||
} else if (BWIndex > 53 && BWIndex <= 63) {
|
||
gyroConfigStatic5.gyroAAFBitshift = 3;
|
||
}
|
||
_pBuf = (uint8_t *) &gyroConfigStatic5;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC5, _pBuf[0]);
|
||
bank = 2;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
accelConfigStatic2.accelAAFDelt = BWIndex;
|
||
_pBuf = (uint8_t *) &accelConfigStatic2;
|
||
writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, _pBuf[0] );
|
||
writeRegister(UB2_REG_ACCEL_CONFIG_STATIC3, AAFDeltsqr );
|
||
accelConfigStatic4.accelAAFDeltsqr = AAFDeltsqr >> 8;
|
||
if (BWIndex == 1) {
|
||
accelConfigStatic4.accelAAFBitshift = 15;
|
||
} else if (BWIndex == 2) {
|
||
accelConfigStatic4.accelAAFBitshift = 13;
|
||
} else if (BWIndex == 3) {
|
||
accelConfigStatic4.accelAAFBitshift = 12;
|
||
} else if (BWIndex == 4) {
|
||
accelConfigStatic4.accelAAFBitshift = 11;
|
||
} else if (BWIndex == 5 || BWIndex == 6) {
|
||
accelConfigStatic4.accelAAFBitshift = 10;
|
||
} else if (BWIndex > 6 && BWIndex < 10) {
|
||
accelConfigStatic4.accelAAFBitshift = 9;
|
||
} else if (BWIndex > 9 && BWIndex < 14) {
|
||
accelConfigStatic4.accelAAFBitshift = 8;
|
||
} else if (BWIndex > 13 && BWIndex < 19) {
|
||
accelConfigStatic4.accelAAFBitshift = 7;
|
||
} else if (BWIndex > 18 && BWIndex < 27) {
|
||
accelConfigStatic4.accelAAFBitshift = 6;
|
||
} else if (BWIndex > 26 && BWIndex < 37) {
|
||
accelConfigStatic4.accelAAFBitshift = 5;
|
||
} else if (BWIndex > 36 && BWIndex < 53) {
|
||
accelConfigStatic4.accelAAFBitshift = 4;
|
||
} else if (BWIndex > 53 && BWIndex <= 63) {
|
||
accelConfigStatic4.accelAAFBitshift = 3;
|
||
}
|
||
_pBuf = (uint8_t *) &accelConfigStatic4;
|
||
writeRegister(UB2_REG_ACCEL_CONFIG_STATIC4, _pBuf[0] );
|
||
bank = 0;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void setAAF(uint8_t who, uint8_t mode)
|
||
{
|
||
uint8_t bank = 0;
|
||
uint8_t * _pBuf;
|
||
if (who == GYRO) {
|
||
if (mode==1) {
|
||
gyroConfigStatic2.gyroAAFDis = 0;
|
||
} else {
|
||
gyroConfigStatic2.gyroAAFDis = 1;
|
||
}
|
||
bank = 1;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
_pBuf = (uint8_t *) &gyroConfigStatic2;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, _pBuf[0] );
|
||
} else if (who == ACCEL) {
|
||
if (mode==1) {
|
||
accelConfigStatic2.accelAAFDis = 0;
|
||
} else {
|
||
accelConfigStatic2.accelAAFDis = 1;
|
||
}
|
||
bank = 2;
|
||
writeRegister(REG_BANK_SEL, bank );
|
||
_pBuf = (uint8_t *) &accelConfigStatic2;
|
||
writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, _pBuf[0] );
|
||
} else if (who == ALL) {
|
||
if (mode==1) {
|
||
gyroConfigStatic2.gyroAAFDis = 0;
|
||
accelConfigStatic2.accelAAFDis = 0;
|
||
} else {
|
||
gyroConfigStatic2.gyroAAFDis = 1;
|
||
accelConfigStatic2.accelAAFDis = 1;
|
||
}
|
||
bank = 1;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
_pBuf = (uint8_t *) &gyroConfigStatic2;
|
||
writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, _pBuf[0]);
|
||
bank = 2;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
_pBuf = (uint8_t *) &accelConfigStatic2;
|
||
writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, _pBuf[0]);
|
||
}
|
||
bank = 0;
|
||
writeRegister(REG_BANK_SEL, bank);
|
||
}
|
||
|
||
|
||
int setUIFilter(uint8_t who, uint8_t filterOrder , uint8_t UIFilterIndex)
|
||
{
|
||
int ret = 0;
|
||
uint8_t * _pBuf;
|
||
setBank(0);
|
||
if (filterOrder > 3 || UIFilterIndex > 15) {
|
||
ret = -1;
|
||
} else {
|
||
if (who == GYRO) {
|
||
gyroConfig1.gyroUIFiltODR = filterOrder;
|
||
_pBuf = (uint8_t *) &gyroConfig1;
|
||
writeRegister(UB0_REG_GYRO_CONFIG1, _pBuf[0]);
|
||
gyroAccelConfig0.gyroUIFiltBW = UIFilterIndex;
|
||
_pBuf = (uint8_t *) &gyroAccelConfig0;
|
||
writeRegister(UB0_REG_GYRO_ACCEL_CONFIG0, _pBuf[0]);
|
||
} else if (who == ACCEL) {
|
||
accelConfig1.accelUIFiltORD = filterOrder;
|
||
_pBuf = (uint8_t *) &accelConfig1;
|
||
writeRegister(UB0_REG_ACCEL_CONFIG1, _pBuf[0]);
|
||
gyroAccelConfig0.accelUIFiltBW = UIFilterIndex;
|
||
_pBuf = (uint8_t *) &gyroAccelConfig0;
|
||
writeRegister(UB0_REG_GYRO_ACCEL_CONFIG0, _pBuf[0]);
|
||
} else if (who == ALL) {
|
||
gyroConfig1.gyroUIFiltODR = filterOrder;
|
||
_pBuf = (uint8_t *) &gyroConfig1;
|
||
writeRegister(UB0_REG_GYRO_CONFIG1, _pBuf[0]);
|
||
accelConfig1.accelUIFiltORD = filterOrder;
|
||
_pBuf = (uint8_t *) &accelConfig1;
|
||
writeRegister(UB0_REG_ACCEL_CONFIG1, _pBuf[0]);
|
||
gyroAccelConfig0.gyroUIFiltBW = UIFilterIndex;
|
||
gyroAccelConfig0.accelUIFiltBW = UIFilterIndex;
|
||
_pBuf = (uint8_t *) &gyroAccelConfig0;
|
||
writeRegister(UB0_REG_GYRO_ACCEL_CONFIG0, _pBuf[0]);
|
||
}
|
||
}
|
||
return ret;
|
||
}
|
||
|
||
|
||
int setODRAndFSR(uint8_t who, uint8_t ODR, uint8_t FSR)
|
||
{
|
||
int ret = 0;
|
||
uint8_t * _pBuf;
|
||
|
||
setBank(0);
|
||
|
||
if (who == GYRO) {
|
||
if (ODR > 15 || FSR > FSR_7)
|
||
{
|
||
ret = -1;
|
||
}
|
||
else
|
||
{
|
||
gyroConfig0.gyroODR = ODR;
|
||
gyroConfig0.gyroFsSel = FSR;
|
||
_pBuf = (uint8_t *) &gyroConfig0;
|
||
writeRegister(UB0_REG_GYRO_CONFIG0, _pBuf[0]);
|
||
|
||
switch (FSR)
|
||
{
|
||
case FSR_0:
|
||
break;
|
||
case FSR_1:
|
||
setGyroFS(dps2000);
|
||
break;
|
||
case FSR_2:
|
||
setGyroFS(dps1000);
|
||
break;
|
||
case FSR_3:
|
||
setGyroFS(dps500);
|
||
break;
|
||
case FSR_4:
|
||
setGyroFS(dps250);
|
||
break;
|
||
case FSR_5:
|
||
setGyroFS(dps125);
|
||
break;
|
||
case FSR_6:
|
||
setGyroFS(dps62_5);
|
||
break;
|
||
case FSR_7:
|
||
setGyroFS(dps31_25);
|
||
break;
|
||
}
|
||
|
||
}
|
||
}
|
||
else if (who == ACCEL)
|
||
{
|
||
if (ODR > 15 || FSR > FSR_3)
|
||
{
|
||
ret = -2;
|
||
} else {
|
||
accelConfig0.accelODR = ODR;
|
||
accelConfig0.accelFsSel = FSR;
|
||
_pBuf = (uint8_t *) &accelConfig0;
|
||
writeRegister(UB0_REG_ACCEL_CONFIG0, _pBuf[0] );
|
||
|
||
switch (FSR) {
|
||
case FSR_0:
|
||
setAccelFS(gpm16);
|
||
break;
|
||
case FSR_1:
|
||
setAccelFS(gpm8);
|
||
break;
|
||
case FSR_2:
|
||
setAccelFS(gpm4);
|
||
break;
|
||
case FSR_3:
|
||
setAccelFS(gpm2);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
return ret;
|
||
}
|
||
|
||
|
||
|
||
|
||
/* starts communication with the ICM42688P */
|
||
int ICM42688Begin(void)
|
||
{
|
||
|
||
// reset the ICM42688
|
||
ICM42688Reset();
|
||
|
||
// check the WHO AM I byte
|
||
setBank(0);
|
||
ICM42688_readRegisters(UB0_REG_WHO_AM_I,_buffer,1);
|
||
if(_buffer[0] != WHO_AM_I)
|
||
{
|
||
printf("ID = %x \n\r", _buffer[0]);
|
||
return -3;// 表示读取身份错误
|
||
}
|
||
|
||
|
||
// turn on accel and gyro in Low Noise (LN) Mode在低噪声(LN)模式下打开加速和陀螺
|
||
if(writeRegister(UB0_REG_PWR_MGMT0, 0x0F) < 0)
|
||
{
|
||
return -4;
|
||
}
|
||
|
||
// 16G is default -- do this to set up accel resolution scaling
|
||
int ret = setAccelFS(gpm16);
|
||
if (ret < 0) return ret;
|
||
|
||
// 2000DPS is default -- do this to set up gyro resolution scaling
|
||
ret = setGyroFS(dps2000);
|
||
if (ret < 0) return ret;
|
||
|
||
// disable inner filters (Notch filter, Anti-alias filter, UI filter block)
|
||
// if (setFilters(0, 0) < 0) {
|
||
// return -7;
|
||
// }
|
||
|
||
// // estimate gyro bias
|
||
// if (calibrateGyro() < 0) {
|
||
// return -8;
|
||
// }
|
||
// // successful init, return 1
|
||
return 1;
|
||
}
|
||
|
||
|
||
void ICM42688_Init(void)
|
||
{
|
||
// start communication with IMU
|
||
int status = ICM42688Begin();
|
||
if (status < 0) {
|
||
printf("IMU初始化不成功 \r\n ");
|
||
printf("检查IMU接线或尝试循环供电 \r\n ");
|
||
printf("错误状态:%d ",status);
|
||
printf(" \r\n ");
|
||
while(1) {
|
||
status = ICM42688Begin();
|
||
}
|
||
}
|
||
printf("\r\n ICM42688_Init 完成 \r\n");
|
||
|
||
|
||
setGyroNotchFilterFHz(1,ALL);//設置陷波頻率 1KH
|
||
setGyroNFbandwidth(1);//帶寬
|
||
setGyroNotchFilter(1);//使能了NF
|
||
setAAFBandwidth(GYRO,6);//設置二階濾波器帶寬6=258hz 2=84
|
||
setAAF(GYRO,1);//使能了AAF
|
||
setUIFilter(GYRO,2 ,1);// 3阶 Accel LPF的带宽=ODR/4 dps2000
|
||
setODRAndFSR(/* who= */GYRO,/* ODR= */ODR_500HZ, /* FSR = */FSR_1);
|
||
|
||
|
||
|
||
setAAFBandwidth(ACCEL,6);//設置二階濾波器帶寬258hz
|
||
setAAF(ACCEL,true);//使能了AAF
|
||
setUIFilter(ACCEL,2 ,7);// 3阶 Accel LPF的带宽=OODR/40 gpm8
|
||
setODRAndFSR(/* who= */ACCEL,/* ODR= */ODR_500HZ, /* FSR = */FSR_1);
|
||
|
||
}
|
||
|
||
|