840 lines
23 KiB
C
840 lines
23 KiB
C
#include "stdio.h"
|
||
#include <stdint.h>
|
||
#include "stdlib.h"
|
||
#include <math.h>
|
||
#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;
|
||
}
|
||
|
||
|