#include "stdio.h" #include #include "stdlib.h" #include #include "delay.h" #include "string.h" #include "spi.h" #include "sensors.h" #include "icm42688p.h" #define IMU_DEBUG 1 #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; // Ref from ICM-42688P datasheet 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}; // 陀螺简易校准 float _gyroBD[3] = {0}; float _gyrB[3] = {0}; // 校准采样数量 static int NUM_CALIB_SAMPLES = 1000; // 比例尺分辨率因子 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) { //1.写寄存器操作 MEMS_CS_Enable(); // 发送寄存器地址（最高位置0表示写操作） uint8_t addr_byte = subAddress; uint8_t rx_byte = SPI2_ReadWriteByte(addr_byte); // 发送数据字节 rx_byte = SPI2_ReadWriteByte(data); //2.写后读校验 uint8_t readback_data; if(ICM42688_readRegisters(subAddress, &readback_data, 1) != 0) { //printf("读取失败 \r\n"); //return -1; } // 关闭片选 MEMS_CS_Disable(); // 3.数据一致性校验 return (readback_data == data) ? 1 : -2; } 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); printf("ICM-42688P Reset.\r\n"); int status = writeRegister(UB0_REG_DEVICE_CONFIG, 0x01); // wait for ICM42688 to come back up Delay_Ms(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; // 错误码-1：缓冲区溢出 // 片选使能 MEMS_CS_Enable(); // 步骤1：发送寄存器地址（带读标志位）并丢弃首字节回传 uint8_t rx_byte; // 发送地址，接收回传（丢弃） SPI2_ReadWriteByte(subAddress | 0x80); //连续读取len个字节 for (int i = 0; i < len; i++) { // 发送空数据触发接收 rx_byte = SPI2_ReadWriteByte(0x00); // 存储有效数据 icm42xxx_spi_rx[i] = rx_byte; } // 片选禁用 MEMS_CS_Disable(); // 无需偏移，因无地址回传 memcpy(dest, icm42xxx_spi_rx, len); return 0; // 成功 } ///* sets the accelerometer full scale range to values other than default *///将加速度计满量程设置为默认值*/以外的值 int setAccelFS(enum AccelFS fssel) { uint8_t reg; setBank(0); // read current register value 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; #if IMU_DEBUG printf("set setAccelFS sucess.\n"); #endif return 1; } /* sets the gyro full scale range to values other than default */ int setGyroFS(enum GyroFS fssel) { uint8_t reg; setBank(0); // read current register value 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) { char i = 0; int16_t rawMeas[7]; // grab the data from the ICM42688 if (ICM42688_readRegisters(UB0_REG_TEMP_DATA1,_buffer,14) < 0) return -1; // combine bytes into 16 bit values // temp, accel xyz, gyro xyz for (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; } /**---------------------------------------------------------------------- * Function : ICM42688_GetData * Description : 读取MPU6500原始数据，这里按照逐字节读取方式，以后可以优化为 DMA直接读取提高速度 * Author : zhanli&719901725@qq.com * Date : 2025/02/20 zhanli *---------------------------------------------------------------------**/ int ICM42688_GetData(IMU_Data *mIMU_Data) { // grab the data from the ICM42688 if (ICM42688_readRegisters(UB0_REG_TEMP_DATA1,_buffer,14) < 0) { return -1; }; // 转换bytes到16 bit : temp, accel xyz, gyro xyz int16_t rawMeas[7]; for (uint8_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] - _acc[0]; _gyr1[1] = rawMeas[5] - _acc[1]; _gyr1[2] = rawMeas[6] - _acc[2]; for(uint8_t i = 0; i < 3; i++){ mIMU_Data->Accel[i] = _acc[i]; mIMU_Data->Gyro[i] = _gyr[i]; } mIMU_Data -> Temp = _t; return 1; } /* estimates the gyro biases *///估计陀螺偏差 int calibrateGyro(void) { uint8_t i = 0; // 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 (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; Delay_Ms(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; uint8_t * _pBuf; double fdesired = 0; double coswz = 0; int16_t nfCoswz; uint8_t nfCoswzSel; writeRegister(REG_BANK_SEL, bank); fdesired = freq * 1000; coswz = cos(2 * 3.14 * fdesired / 32); 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); } } 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; uint8_t bandWidth; writeRegister(REG_BANK_SEL, bank ); bandWidth = (bw << 4) | 0x01; writeRegister(UB1_REG_GYRO_CONFIG_STATIC10, bandWidth ); bank = 0; writeRegister(REG_BANK_SEL, bank ); } void setGyroNotchFilter(uint8_t mode) { uint8_t bank = 1; uint8_t * _pBuf; if (mode==1) { gyroConfigStatic2.gyroNFDis = 0; } else { gyroConfigStatic2.gyroNFDis = 1; } 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) { int ret = 0; // 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("Read ICM42688 error, Read ID = %x \n\r", _buffer[0]); return -3;// 表示读取身份错误 } printf("Read ICM42688 ID sucess, ICM42688 ID = 0x%x \n\r", _buffer[0]); // 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 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; } int8_t 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 "); return 0; } #if IMU_DEBUG printf("ICM-42688 init sucess. \r\n"); #endif 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, 1);//使能了AAF setUIFilter(ACCEL,2 ,7);// 3阶 Accel LPF的带宽=OODR/40 gpm8 setODRAndFSR(/* who= */ACCEL,/* ODR= */ODR_500HZ, /* FSR = */FSR_1); return 1; }