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<6F><6E><EFBFBD>̼<EFBFBD><CCBC><EFBFBD>У
|
|||
|
|
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<6F><6E><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD>У
|
|||
|
|
float _gyroBD[3] = {0};
|
|||
|
|
float _gyrB[3] = {0};
|
|||
|
|
|
|||
|
|
static int NUM_CALIB_SAMPLES = 1000; ///< for gyro/accel bias calib<69><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>/<2F><><EFBFBD>ٶ<EFBFBD>ƫ<EFBFBD><C6AB>У
|
|||
|
|
|
|||
|
|
///\brief Full scale resolution factors<72><73><EFBFBD>̵<EFBFBD>ȫ<EFBFBD><C8AB><EFBFBD><EFBFBD><EFBFBD>߷ֱ<DFB7><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float _accelScale = 0.0f;
|
|||
|
|
float _gyroScale = 0.0f;
|
|||
|
|
|
|||
|
|
///\brief Full scale selections<6E><73><EFBFBD>̵<EFBFBD>ȫ<EFBFBD>ߴ<EFBFBD>ѡ<EFBFBD><D1A1>
|
|||
|
|
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(); //<2F><>Ƭѡ<C6AC><D1A1><EFBFBD>õͣ<C3B5><CDA3><EFBFBD><EFBFBD>ߴӻ<DFB4><D3BB><EFBFBD>ʼͨ<CABC><CDA8>
|
|||
|
|
|
|||
|
|
icm42xxx_spi_tx[0] = subAddress & 0x7F; // д<><D0B4>ʱ<EFBFBD><CAB1><EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD>ֽڵĵ<DAB5>һλΪ 0<><30>0x7f <-> 01111111
|
|||
|
|
icm42xxx_spi_tx[1] = data; // MOSI<53>ϵڶ<CFB5><DAB6><EFBFBD><EFBFBD>ֽ<EFBFBD>Ϊд<CEAA><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
HAL_SPI_TransmitReceive(&ICM42xxx_HSPI,&icm42xxx_spi_tx[0],&icm42xxx_spi_rx[0], 2,0XFFFF);
|
|||
|
|
|
|||
|
|
SPI_CS_Disable(); //<2F><>Ƭѡ<C6AC><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>øߣ<C3B8><DFA3><EFBFBD><EFBFBD>ߴӻ<DFB4><D3BB><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8>
|
|||
|
|
|
|||
|
|
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() //<2F><>λ
|
|||
|
|
{
|
|||
|
|
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; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ䣡
|
|||
|
|
|
|||
|
|
/* write data to device */
|
|||
|
|
SPI_CS_Enable(); //<2F><>Ƭѡ<C6AC><D1A1><EFBFBD>õͣ<C3B5><CDA3><EFBFBD><EFBFBD>ߴӻ<DFB4><D3BB><EFBFBD>ʼͨ<CABC><CDA8>
|
|||
|
|
|
|||
|
|
icm42xxx_spi_tx[0] = subAddress | 0x80; // <20><>ȡʱ<C8A1><CAB1><EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD>ֽڵĵ<DAB5>һλΪ 1
|
|||
|
|
memset(icm42xxx_spi_tx+1,0x00,len); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>ע<EFBFBD><D7A2>MOSI<53>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD>ٷ<EFBFBD><D9B7><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƻ<EFBFBD>ͻ<EFBFBD><CDBB><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if( HAL_SPI_TransmitReceive(&ICM42xxx_HSPI,&icm42xxx_spi_tx[0],&icm42xxx_spi_rx[0], len+1,0XFF)!= HAL_OK) //<2F><>Ϊ<EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ﳤ<EFBFBD><EFB3A4><EFBFBD><EFBFBD>len+1
|
|||
|
|
{
|
|||
|
|
return -2; // SPI<50><49>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
SPI_CS_Disable(); //<2F><>Ƭѡ<C6AC><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>øߣ<C3B8><DFA3><EFBFBD><EFBFBD>ߴӻ<DFB4><D3BB><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8>
|
|||
|
|
memcpy(dest,icm42xxx_spi_rx+1,len);
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
///* sets the accelerometer full scale range to values other than default *///<2F><><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪĬ<CEAA><C4AC>ֵ*/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
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 *///<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
|
|||
|
|
int calibrateGyro(void) {
|
|||
|
|
// set at a lower range (more resolution) since IMU not moving<6E><67><EFBFBD><EFBFBD><EFBFBD>ڽϵ͵ķ<CDB5>Χ(<28><><EFBFBD>ߵķֱ<C4B7><D6B1><EFBFBD>)<29><><EFBFBD><EFBFBD>ΪIMU<4D><55><EFBFBD>ƶ<EFBFBD>
|
|||
|
|
const enum GyroFS current_fssel = _gyroFS;
|
|||
|
|
if (setGyroFS(dps250) < 0) return -1;
|
|||
|
|
|
|||
|
|
// take samples and find bias<61><73>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҳ<EFBFBD>ƫ<EFBFBD><C6AB>
|
|||
|
|
_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) //<2F>O<EFBFBD><4F><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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) //<2F>O<EFBFBD>ö<EFBFBD><C3B6>A<EFBFBD>V<EFBFBD><56><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
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;// <20><>ʾ<EFBFBD><CABE>ȡ<EFBFBD><C8A1><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// turn on accel and gyro in Low Noise (LN) Mode<64>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD>(LN)ģʽ<C4A3>´<C2B4><F2BFAABC>ٺ<EFBFBD><D9BA><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD> \r\n ");
|
|||
|
|
printf("<EFBFBD><EFBFBD><EFBFBD><EFBFBD>IMU<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> \r\n ");
|
|||
|
|
printf("<EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬:%d ",status);
|
|||
|
|
printf(" \r\n ");
|
|||
|
|
while(1) {
|
|||
|
|
status = ICM42688Begin();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
printf("\r\n ICM42688_Init <20><><EFBFBD><EFBFBD> \r\n");
|
|||
|
|
|
|||
|
|
|
|||
|
|
setGyroNotchFilterFHz(1,ALL);//<2F>O<EFBFBD><4F><EFBFBD>ݲ<EFBFBD><DDB2>l<EFBFBD><6C> 1KH
|
|||
|
|
setGyroNFbandwidth(1);//<2F><><EFBFBD><EFBFBD>
|
|||
|
|
setGyroNotchFilter(1);//ʹ<><CAB9><EFBFBD><EFBFBD>NF
|
|||
|
|
setAAFBandwidth(GYRO,6);//<2F>O<EFBFBD>ö<EFBFBD><C3B6>A<EFBFBD>V<EFBFBD><56><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>6=258hz 2=84
|
|||
|
|
setAAF(GYRO,1);//ʹ<><CAB9><EFBFBD><EFBFBD>AAF
|
|||
|
|
setUIFilter(GYRO,2 ,1);// 3<><33> Accel LPF<50>Ĵ<EFBFBD><C4B4><EFBFBD>=ODR/4 dps2000
|
|||
|
|
setODRAndFSR(/* who= */GYRO,/* ODR= */ODR_500HZ, /* FSR = */FSR_1);
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
setAAFBandwidth(ACCEL,6);//<2F>O<EFBFBD>ö<EFBFBD><C3B6>A<EFBFBD>V<EFBFBD><56><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>258hz
|
|||
|
|
setAAF(ACCEL,true);//ʹ<><CAB9><EFBFBD><EFBFBD>AAF
|
|||
|
|
setUIFilter(ACCEL,2 ,7);// 3<><33> Accel LPF<50>Ĵ<EFBFBD><C4B4><EFBFBD>=OODR/40 gpm8
|
|||
|
|
setODRAndFSR(/* who= */ACCEL,/* ODR= */ODR_500HZ, /* FSR = */FSR_1);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|