#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); }