ICM42670陀螺仪可以使用iic驱动，单片机平台使用GD32f305VE。
iic初始化代码如下 .c文件
#include "M_IIC.h"
#include <stdio.h>
#include "systick.h"
/*!
\brief configure the GPIO ports
\param[in] none
\param[out] none
\retval none
*/
void iic_gpio_config(void)
{
/* enable GPIOB clock */
// gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
rcu_periph_clock_enable(RCU_GPIO_I2C);
// rcu_periph_clock_enable(RCU_AF);
/* connect PB6 to I2C_SCL */
/* connect PB7 to I2C_SDA */
gpio_init(I2C_SCL_PORT, GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ, I2C_SCL_PIN);
gpio_init(I2C_SDA_PORT, GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ, I2C_SDA_PIN);
}
/*!
\brief configure the I2C interfaces
\param[in] none
\param[out] none
\retval none
*/
void i2c_config(void)
{
iic_gpio_config();
/* enable I2C clock */
rcu_periph_clock_enable(RCU_I2C);
/* configure I2C clock */
i2c_clock_config(I2CX, I2C_SPEED, I2C_DTCY_2);
/* configure I2C address */
i2c_mode_addr_config(I2CX, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, I2CX_SLAVE_ADDRESS7);
/* enable I2CX */
i2c_enable(I2CX);
/* enable acknowledge */
i2c_ack_config(I2CX, I2C_ACK_ENABLE);
}
/*!
\brief reset i2c bus
\param[in] none
\param[out] none
\retval none
*/
void i2c_bus_reset()
{
i2c_deinit(I2CX);
/* configure SDA/SCL for GPIO */
GPIO_BC(I2C_SCL_PORT) |= I2C_SCL_PIN;
GPIO_BC(I2C_SDA_PORT) |= I2C_SDA_PIN;
gpio_init(I2C_SCL_PORT, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, I2C_SCL_PIN);
gpio_init(I2C_SDA_PORT, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, I2C_SDA_PIN);
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
GPIO_BOP(I2C_SCL_PORT) |= I2C_SCL_PIN;
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
GPIO_BOP(I2C_SDA_PORT) |= I2C_SDA_PIN;
/* connect I2C_SCL_PIN to I2C_SCL */
/* connect I2C_SDA_PIN to I2C_SDA */
gpio_init(I2C_SCL_PORT, GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ, I2C_SCL_PIN);
gpio_init(I2C_SDA_PORT, GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ, I2C_SDA_PIN);
/* configure the I2CX interface */
/* enable I2C clock */
rcu_periph_clock_enable(RCU_I2C);
/* configure I2C clock */
i2c_clock_config(I2CX, I2C_SPEED, I2C_DTCY_2);
/* configure I2C address */
i2c_mode_addr_config(I2CX, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, I2CX_SLAVE_ADDRESS7);
/* enable I2CX */
i2c_enable(I2CX);
/* enable acknowledge */
i2c_ack_config(I2CX, I2C_ACK_ENABLE);
}
.h文件
#ifndef _CAN
#define _CAN
#include "gd32f30x.h"
#define I2C_SPEED 400000
#define I2CX_SLAVE_ADDRESS7 0xD0
#define I2C_PAGE_SIZE 8
#define I2CX I2C1
#define RCU_GPIO_I2C RCU_GPIOB
#define RCU_I2C RCU_I2C1
#define I2C_SCL_PORT GPIOB
#define I2C_SDA_PORT GPIOB
#define I2C_SCL_PIN GPIO_PIN_10
#define I2C_SDA_PIN GPIO_PIN_11
/* configure the GPIO ports */
//void gpio_config(void);
/* configure the I2C interfaces */
void i2c_config(void);
/* reset i2c bus */
void i2c_bus_reset(void);
#endif
驱动ICM42670的iic时序如下：
所以开始写ICM42670的驱动
配置的寄存器主要有
配置好量程，频率和低噪声模式就可在寄存器中查到实时的数据，读取数据前首先要读取INT_STATUS_DRDY寄存器
确认数据已经准备好，然后是 .c如下：
#include "ICM42670.h"
#include <stdio.h>
#include "systick.h"
//在iic总线上发送起始位
uint8_t iic_start(void){
uint16_t timeout=0;
while(i2c_flag_get(I2CX, I2C_FLAG_I2CBSY) && (timeout < I2C_TIME_OUT)) {
timeout++;
}
if(timeout < I2C_TIME_OUT) {
i2c_start_on_bus(I2CX);
return 1;
} else {
return 0;
}
}
//在iic总线上发送从机地址并清除ADDSEND
uint8_t iic_slave_address(uint8_t addr,uint32_t trandirection){
uint16_t timeout=0;
while((!i2c_flag_get(I2CX, I2C_FLAG_SBSEND)) && (timeout < I2C_TIME_OUT)) {
timeout++;
}
if(timeout < I2C_TIME_OUT) {
i2c_master_addressing(I2CX,addr, trandirection);
timeout=0;
while((!i2c_flag_get(I2CX, I2C_FLAG_ADDSEND)) && (timeout < I2C_TIME_OUT)) {
timeout++;
}
if(timeout < I2C_TIME_OUT) {
i2c_flag_clear(I2CX, I2C_FLAG_ADDSEND);//根据要求清除ADDSEND位
return 1;
} else {
return 0;
}
} else {
return 0;
}
}
//查询TBE的方式写入数据，适用与前n-2个数据
uint8_t iic_datasendTBE(uint8_t iic_data){
uint16_t timeout=0;
while((! i2c_flag_get(I2CX, I2C_FLAG_TBE)) && (timeout < I2C_TIME_OUT)) {
timeout++;
}
if(timeout < I2C_TIME_OUT) {
i2c_data_transmit(I2CX, iic_data);
return 1;
} else {
return 0;
}
}
//检测TBC发送
uint8_t iic_datasendTBC(uint8_t iic_data){
uint16_t timeout=0;
/* wait until BTC bit is set */
while((!i2c_flag_get(I2CX, I2C_FLAG_BTC)) && (timeout < I2C_TIME_OUT)) {
timeout++;
}
if(timeout < I2C_TIME_OUT) {
i2c_data_transmit(I2CX, iic_data);
return 1;
} else return 0;
}
//最后一位数据读取时调用
uint8_t iic_dataread_N_2(uint8_t* p_buffer){
uint16_t timeout=0;
/* wait until BTC bit is set */
// while(!i2c_flag_get(I2CX, I2C_FLAG_BTC));
/* disable acknowledge */
i2c_ack_config(I2CX, I2C_ACK_DISABLE);
/* wait until BTC bit is set */
// while(!i2c_flag_get(I2CX, I2C_FLAG_BTC));
/* send a stop condition to I2C bus */
i2c_stop_on_bus(I2CX);
while(timeout<I2C_TIME_OUT) {
timeout++;
/* wait until RBNE bit is set */
if(i2c_flag_get(I2CX, I2C_FLAG_RBNE)) {
/* read a byte from the EEPROM */
*p_buffer = i2c_data_receive(I2CX);
return 1;
/* point to the next location where the byte read will be saved */
}
}
return 0;
}
//前0-N-2个数据调用
uint8_t iic_dataread_N(uint8_t* p_buffer){
uint16_t timeout=0;
while(timeout<I2C_TIME_OUT) {
timeout++;
/* wait until RBNE bit is set */
if(i2c_flag_get(I2CX, I2C_FLAG_RBNE)) {
/* read a byte from the EEPROM */
*p_buffer = i2c_data_receive(I2CX);
return 1;
/* point to the next location where the byte read will be saved */
}
}
return 0;
}
uint8_t iic_stop(void){
uint16_t timeout=0;
while((!i2c_flag_get(I2CX, I2C_FLAG_BTC)) && (timeout < I2C_TIME_OUT)) {//等待最后一个字节发送完成，随后产生停止信号
timeout++;
}
if(timeout < I2C_TIME_OUT) {
i2c_stop_on_bus(I2CX);
timeout=0;
while((I2C_CTL0(I2CX) & I2C_CTL0_STOP) && (timeout < I2C_TIME_OUT)) {//等待停止信号发送完成
timeout++;
}
if(timeout < I2C_TIME_OUT) {
return 1;
} else {
return 0;
}
} else {
return 0;
}
}
//ICM42670单字节写入
uint8_t ICM42670_writeonebyte(uint16_t addr,uint8_t data){
//i2c_bus_reset();
if(!iic_start()) return 0; //发送起始信号
if(!iic_slave_address(EERPOM_ADD,I2C_TRANSMITTER)) return 0; //发送设备地址
if(!iic_datasendTBE(((uint8_t)(addr>>8)))) return 0; //发送写入地址高8位
if(!iic_datasendTBE(((uint8_t)(addr)))) return 0; //发送写入地址低8位
if(!iic_datasendTBE(((uint8_t)(data)))) return 0; //发送写入的数据
if(!iic_stop()) return 0; //发送停止信号
return 1;
}
//ICM42670单字节读取
uint8_t ICM42670_readonebyte(uint8_t* data){
uint16_t timeout=0;
//i2c_bus_reset();
i2c_ack_config(I2CX, I2C_ACK_ENABLE);//使能回复应答
if(!iic_start()) return 0; //发送起始信号
if(!iic_slave_address(EERPOM_ADD,I2C_RECEIVER)) return 0; //发送设备地址
if(!iic_dataread_N_2(data)) return 0; //读取一字节数据
while((I2C_CTL0(I2CX) & I2C_CTL0_STOP) && (timeout < I2C_TIME_OUT)) {//等待停止位发送完成
timeout++;
}
if(timeout < I2C_TIME_OUT) {
return 1;
} else {
return 0;
}
}
//随机写入
uint8_t ICM42670_writeRandombyte(uint8_t addr,uint8_t* data,uint16_t length){
//OPENWRITE;
//delay_1ms(10);
if(!iic_start()) return 0; //发送起始信号
if(!iic_slave_address(EERPOM_ADD,I2C_TRANSMITTER)) return 0; //发送设备地址
// if(!iic_datasendTBE(((uint8_t)(addr>>8)))) return 0; //发送写入地址高8位
if(!iic_datasendTBE(((uint8_t)(addr)))) return 0; //发送写入地址低8位
while(length){
if(!iic_datasendTBC(((uint8_t)(*data)))) return 0; //发送写入的数据
length--;
data++;
}
if(!iic_stop()) return 0; //发送停止信号
// CLOSEWRITE;
return 1;
}
//随机读取
uint8_t ICM42670_readRandombyte(uint8_t addr,uint8_t* data,uint16_t length){
if(!iic_start()) return 0; //发送起始信号
if(!iic_slave_address(EERPOM_ADD,I2C_TRANSMITTER)) return 0; //发送设备地址
// if(!iic_datasendTBE(((uint8_t)(addr>>8)))) return 0; //发送写入地址高8位
if(!iic_datasendTBE(((uint8_t)(addr)))) return 0; //发送写入地址低8位
while(!i2c_flag_get(I2CX, I2C_FLAG_BTC));
i2c_bus_reset();
i2c_ack_config(I2CX, I2C_ACK_ENABLE);//使能回复应答
if(!iic_start()) return 0; //发送起始信号
if(!iic_slave_address(EERPOM_ADD,I2C_RECEIVER)) return 0; //发送设备地址
while(length){
if(length<2){
iic_dataread_N_2(data);
}
else{
iic_dataread_N(data);
}
data++;
length--;
}
return 1;
}
void ICM42670_test(void){
uint8_t buf[10]={0},buf2[10]={0x12,0x34,3,4,5,6,7,8,9,10};
ICM42670_readRandombyte(0x00,buf,3);
// uint8_t ad=0;
buf[0]=0x04;
ICM42670_writeRandombyte(0x02,&buf[0],1);
for(uint16_t i=0;i<0xFFFF;i++){};
ICM42670_readRandombyte(0x00,buf,3);
ICM42670_readRandombyte(0x09,buf,4);
ICM42670_readRandombyte(0x3F,buf,8);
//
}
extern void Delay_Nms(uint16_t Nms);
uint8_t buf1[30]={0};
float data2[6]={0};
int16_t data1[6]={0};
void ICM42670_Init(void){
uint8_t data=0;
ICM42670_writeRandombyte(BLK_SEL_R,&data,0);
ICM42670_writeRandombyte(BLK_SEL_W,&data,0);
ICM42670_readRandombyte(INTF_CONFIG1,&data,1);// 读取 ACCEL_CONFIG0寄存器的值并配置
data=0;//设置iic模式和只使用内部时钟
ICM42670_writeRandombyte(INTF_CONFIG1,&data,1);
ICM42670_readRandombyte(SIGNAL_PATH_RESET,&data,1);
data&=0xEF;
data|=0x10;
ICM42670_writeRandombyte(SIGNAL_PATH_RESET,&data,1);//软件重启
Delay_Nms(20);
ICM42670_writeRandombyte(BLK_SEL_R,&data,0);
ICM42670_writeRandombyte(BLK_SEL_W,&data,0);
ICM42670_readRandombyte(INTF_CONFIG1,&data,1);// 读取 ACCEL_CONFIG0寄存器的值并配置
data=0;//设置iic模式和只使用内部时钟
ICM42670_writeRandombyte(INTF_CONFIG1,&data,1);
// data=0x00;
// ICM42670_writeRandombyte(INTF_CONFIG1,&data,1);
// uint8_t zdata;
// ICM42670_readRandombyte(INT_SOURCE0,&zdata,1);
//
// data=0x00;
// ICM42670_writeRandombyte(INT_SOURCE0,&data,1);
// data=0x00;
// ICM42670_writeRandombyte(FIFO_CONFIG2,&data,1);
// data=0x02;
// ICM42670_writeRandombyte(FIFO_CONFIG3,&data,1);
// ICM42670_writeRandombyte(INT_SOURCE0,&zdata,1);
// data=0x03;
// ICM42670_writeRandombyte(INTF_CONFIG1,&data,1);//使能FIFO
// data=0x36;
// ICM42670_writeRandombyte(INT_CONFIG,&data,1);
// ICM42670_readRandombyte(INT_SOURCE0,&data,1);
// data|= (1 << 2);
// ICM42670_writeRandombyte(INT_SOURCE0,&data,1);
//ICM42670_readRandombyte(SIGNAL_PATH_RESET,&data,1);
//===========设置ACCEL_CONFIG0=============
ICM42670_readRandombyte(ACCEL_CONFIG0,&data,1);// 读取 ACCEL_CONFIG0寄存器的值并配置
data&=0x9F; // 10011111b 将第5-6位清零
//data|=0x20; // 00100000b 将第5-6位设置为01 -> ±8g
data|=0x40; // 01000000b 将第5-6位设置为01 -> ±4g
data&=0xf0; // 11110000b 将第0-3位清零
//data|=0x09; // 00001001b 将第0-3位设置为1001 -> 100Hz
data|=0x0A; // 00001010b 将第0-3位设置为1001 -> 50Hz
ICM42670_writeRandombyte(ACCEL_CONFIG0,&data,1);
//===========设置GYRO_CONFIG0=============
ICM42670_readRandombyte(GYRO_CONFIG0,&data,1);// 读取GYRO_CONFIG0寄存器的值并配置
data&=0xF0; // 11110000b 将第0-3位清零
//data|=0x09; // 00001001b 将第0-3位设置为1001 -> 100Hz
data|=0x0A; // 00001010b 将第0-3位设置为1001 -> 50Hz
data&=0x9F; // 10011111b 将第5-6位清零
data|=0x09; // 01100000b 将第5-6位设置为11 -> 250dps
ICM42670_writeRandombyte(GYRO_CONFIG0,&data,1);
//===========设置PWR_MGMT0==========
ICM42670_readRandombyte(PWR_MGMT0,&data,1);// 读取PWR_MGMT0寄存器的值并配置
data&=0xFC; // 11111100b 将第0-1位清零
data|=0x03; // 00000011b 将第0-1位设置为11 -> Places gyroscope in Low Noise (LN) Mode
data&=0xF3; // 11110011b 将第2-3位清零
data|=0x0C; // 00001100b 将第2-3位设置为11 -> Places gyroscope in Low Noise (LN) Mode
ICM42670_writeRandombyte(PWR_MGMT0,&data,1);//这一步之后要延迟至少200us
}
//此函数50ms定时调用（去掉延时）
void read_42670data(void){
float krm[6]={0};
uint8_t data=0;
//===============读取数据测试==================
//Delay_Nms(20);
while(!data){
ICM42670_readRandombyte(INT_STATUS_DRDY,&data,1);
Delay_Nms(2);
}
ICM42670_readRandombyte(ACCEL_DATA_X1,(uint8_t *)buf1,12);
//ICM42670_readRandombyte(FIFO_DATA,(uint8_t *)buf1,12);
for(int i=0;i<6;i++){
data1[i]=(buf1[i*2]<<8)|buf1[i*2+1];
if(i<3)
data2[i]=data1[i]/32767.0f*4.0f;
else if(i>=3)
data2[i]=data1[i]/32767.0f*250.0f;
}
// krm[0]=kalmanFilter(&KFP_accelX,data2[0]);//卡尔曼滤波
// krm[1]=kalmanFilter(&KFP_accelY,data2[1]);
// krm[2]=kalmanFilter(&KFP_accelZ,data2[2]);
// krm[3]=kalmanFilter(&KFP_gyroX,data2[3]);
// krm[4]=kalmanFilter(&KFP_gyroY,data2[4]);
// krm[5]=kalmanFilter(&KFP_gyroZ,data2[5]);
// algorithm(krm[0],krm[1],krm[2],krm[3],krm[4],krm[5]);//采用卡尔曼滤波
algorithm(data2[0],data2[1],data2[2],data2[3],data2[4],data2[5]);//四元素姿态解算
//printf("%d,%d,%d,%d,%d,%d-------->%f,%f,%f,%f,%f,%f\n",data1[0],data1[1],data1[2],data1[3],data1[4],data1[5],krm[0],krm[1],krm[2],krm[3],krm[4],krm[5]);
//printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",data2[0],data2[1],data2[2],data2[3],data2[4],data2[5],krm[0],krm[1],krm[2],krm[3],krm[4],krm[5],pitch,roll,yaw);
Delay_Nms(40);
}
.h文件如下：
#ifndef AT24CXX_H
#define AT24CXX_H
#include "M_IIC.h"
#include "gd32f30x.h"
#include "Angle.h"
#define I2C_TIME_OUT (uint16_t)(5000)
#define EERPOM_ADD (uint8_t)0xD0 //0xA0写入 0xA1读取
/* BANK0 */
#define MCLK_RDY 0x00
#define DEVICE_CONFIG 0x01
#define SIGNAL_PATH_RESET 0x02
#define DRIVE_CONFIG1 0x03
#define DRIVE_CONFIG2 0x04
#define DRIVE_CONFIG3 0x05
#define INT_CONFIG 0x06
#define TEMP_DATA1 0x09
#define TEMP_DATA0 0x0a
#define ACCEL_DATA_X1 0x0b
#define ACCEL_DATA_X0 0x0c
#define ACCEL_DATA_Y1 0x0d
#define ACCEL_DATA_Y0 0x0e
#define ACCEL_DATA_Z1 0x0f
#define ACCEL_DATA_Z0 0x10
#define GYRO_DATA_X1 0x11
#define GYRO_DATA_X0 0x12
#define GYRO_DATA_Y1 0x13
#define GYRO_DATA_Y0 0x14
#define GYRO_DATA_Z1 0x15
#define GYRO_DATA_Z0 0x16
#define TMST_FSYNCH 0x17
#define TMST_FSYNCL 0x18
#define APEX_DATA4 0x1d
#define APEX_DATA5 0x1e
#define PWR_MGMT0 0x1f
#define GYRO_CONFIG0 0x20
#define ACCEL_CONFIG0 0x21
#define TEMP_CONFIG0 0x22
#define GYRO_CONFIG1 0x23
#define ACCEL_CONFIG1 0x24
#define APEX_CONFIG0 0x25
#define APEX_CONFIG1 0x26
#define WOM_CONFIG 0x27
#define FIFO_CONFIG1 0x28
#define FIFO_CONFIG2 0x29
#define FIFO_CONFIG3 0x2a
#define INT_SOURCE0 0x2b
#define INT_SOURCE1 0x2c
#define INT_SOURCE3 0x2d
#define INT_SOURCE4 0x2e
#define FIFO_LOST_PKT0 0x2f
#define FIFO_LOST_PKT1 0x30
#define APEX_DATA0 0x31
#define APEX_DATA1 0x32
#define APEX_DATA2 0x33
#define APEX_DATA3 0x34
#define INTF_CONFIG0 0x35
#define INTF_CONFIG1 0x36
#define INT_STATUS_DRDY 0x39
#define INT_STATUS 0x3a
#define INT_STATUS2 0x3b
#define INT_STATUS3 0x3c
#define FIFO_COUNTH 0x3d
#define FIFO_COUNTL 0x3e
#define FIFO_DATA 0x3f
#define WHO_AM_I 0x75
#define BLK_SEL_W 0x79
#define MADDR_W 0x7a
#define M_W 0x7b
#define BLK_SEL_R 0x7c
#define MADDR_R 0x7d
#define M_R 0x7e
/* MREG1 */
#define TMST_CONFIG1_MREG1 0x00
#define FIFO_CONFIG5_MREG1 0x01
#define FIFO_CONFIG6_MREG1 0x02
#define FSYNC_CONFIG_MREG1 0x03
#define INT_CONFIG0_MREG1 0x04
#define INT_CONFIG1_MREG1 0x05
#define SENSOR_CONFIG3_MREG1 0x06
#define ST_CONFIG_MREG1 0x13
#define SELFTEST_MREG1 0x14
#define INTF_CONFIG6_MREG1 0x23
#define INTF_CONFIG10_MREG1 0x25
#define INTF_CONFIG7_MREG1 0x28
#define OTP_CONFIG_MREG1 0x2b
#define INT_SOURCE6_MREG1 0x2f
#define INT_SOURCE7_MREG1 0x30
#define INT_SOURCE8_MREG1 0x31
#define INT_SOURCE9_MREG1 0x32
#define INT_SOURCE10_MREG1 0x33
#define APEX_CONFIG2_MREG1 0x44
#define APEX_CONFIG3_MREG1 0x45
#define APEX_CONFIG4_MREG1 0x46
#define APEX_CONFIG5_MREG1 0x47
#define APEX_CONFIG9_MREG1 0x48
#define APEX_CONFIG10_MREG1 0x49
#define APEX_CONFIG11_MREG1 0x4a
#define ACCEL_WOM_X_THR_MREG1 0x4b
#define ACCEL_WOM_Y_THR_MREG1 0x4c
#define ACCEL_WOM_Z_THR_MREG1 0x4d
#define OFFSET_USER0_MREG1 0x4e
#define OFFSET_USER1_MREG1 0x4f
#define OFFSET_USER2_MREG1 0x50
#define OFFSET_USER3_MREG1 0x51
#define OFFSET_USER4_MREG1 0x52
#define OFFSET_USER5_MREG1 0x53
#define OFFSET_USER6_MREG1 0x54
#define OFFSET_USER7_MREG1 0x55
#define OFFSET_USER8_MREG1 0x56
#define ST_STATUS1_MREG1 0x63
#define ST_STATUS2_MREG1 0x64
#define FDR_CONFIG_MREG1 0x66
#define APEX_CONFIG12_MREG1 0x67
/* MREG3 */
#define XA_ST_DATA_MREG3 0x5000
#define YA_ST_DATA_MREG3 0x5001
#define ZA_ST_DATA_MREG3 0x5002
#define XG_ST_DATA_MREG3 0x5003
#define YG_ST_DATA_MREG3 0x5004
#define ZG_ST_DATA_MREG3 0x5005
/* MREG2 */
#define OTP_CTRL7_MREG2 0x2806
void ICM42670_Init(void);
void ICM42670_test(void);
void read_42670data(void);
#endif /* AT24CXX_H */
然后是关于姿态解算和滤波的算法同样是.c如下：
#include "Angle.h"
#define Kp 10.0f // 这里的KpKi是用于调整加速度计修正陀螺仪的速度
#define Ki 0.000f
#define dt 0.005
float pitch,roll,yaw;
//算根号倒数-----快一些
static float invSqrt(float x){
float halfx=0.5f*x;
float y=x;
long i=*(long*)&y;
i=0x5f3759df-(i>>1);
y=*(float*)&i;
y=y*(1.5f-(halfx*y*y));
return y;
}
//
KFP KFP_accelX={0.02,0,0,0,0.001,0.543};
KFP KFP_accelY={0.02,0,0,0,0.001,0.543};
KFP KFP_accelZ={0.02,0,0,0,0.001,0.543};
KFP KFP_gyroX={0.02,0,0,0,0.001,0.543};
KFP KFP_gyroY={0.02,0,0,0,0.001,0.543};
KFP KFP_gyroZ={0.02,0,0,0,0.001,0.543};
/**
*卡尔曼滤波器
*@param KFP *kfp 卡尔曼结构体参数
* float input 需要滤波的参数的测量值（即传感器的采集值）
*@return 滤波后的参数（最优值）
*/
float kalmanFilter(KFP *kfp,float input)
{
//预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
kfp->Now_P = kfp->LastP + kfp->Q;
//卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
//更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
//更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
return kfp->out;
}
#define halfT 0.01f // 采样周期的一半，用于求解四元数微分方程时计算角增量
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 初始位置姿态角为：0、0、0，对应四元数为：1、0、0、0
float exInt = 0, eyInt = 0, ezInt = 0;
//重力加速度在三轴上的分量与用当前姿态计算得来的重力在三轴上的分量的误差的积分
float Q_ANGLE_X= 0, Q_ANGLE_Y = 0, Q_ANGLE_Z = 0;
//互补滤波函数
//输入参数：g表陀螺仪角速度(弧度/s)，a表加计（m/s2或g都可以，会归一化）
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float q0temp,q1temp,q2temp,q3temp;//四元数暂存变量，求解微分方程时要用
float norm; //矢量的模或四元数的范数
float vx, vy, vz;//当前姿态计算得来的重力在三轴上的分量
float ex, ey, ez;//当前加计测得的重力加速度在三轴上的分量
//与用当前姿态计算得来的重力在三轴上的分量的误差
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)//加计处于自由落体状态时不进行姿态解算，因为会产生分母无穷大的情况
return;
norm = sqrt(ax*ax + ay*ay + az*az);//单位化加速度计，
ax = ax / norm;// 这样变更了量程也不需要修改KP参数，因为这里归一化了
ay = ay / norm;
az = az / norm;
//用当前姿态计算出重力在三个轴上的分量，重力在n系下是[0,0,g]，乘以转换矩阵就转到b系
//参考坐标n系转化到载体坐标b系，用四元数表示的方向余弦矩阵第三行即是
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
//计算测得的重力与计算得重力间的误差，这个误差是通过向量外积（也就是叉乘）求出来的
ex = (ay*vz - az*vy) ;
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;
exInt = exInt + ex * Ki; //对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
gx = gx + Kp*ex + exInt; //将误差PI（比例和积分项）补偿到陀螺仪角速度
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //没有磁力计的话就无法修正偏航角
//下面进行姿态的更新，也就是四元数微分方程的求解
q0temp=q0;
q1temp=q1;
q2temp=q2;
q3temp=q3;
//采用一阶毕卡解法，相关知识可参见《惯性器件与惯性导航系统》P212
q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;
//单位化四元数在空间旋转时不会拉伸，仅有旋转角度，这类似线性代数里的正交变换
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//四元数到欧拉角的转换
//其中YAW航向角由于加速度计对其没有修正作用，因此此处直接用陀螺仪积分代替
Q_ANGLE_Z = Q_ANGLE_Z + gz*halfT*2*57.3; // yaw
//Q_ANGLE_Z = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
Q_ANGLE_Y = asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // pitch
Q_ANGLE_X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
//a加速度 g角速度 a——>g g->dps
void algorithm(float ax,float ay,float az,float gx,float gy,float gz){
gx=gx*0.0174f;//将dps转换为弧度
gy=gy*0.0174f;
gz=gz*0.0174f;
float recip;//平方根倒数
recip=invSqrt(ax*ax+ay*ay+az*az);
//提取姿态矩阵的重力分量
float Vx,Vy,Vz;
Vx=2*(q1*q3-q0*q2);
Vy=2*(q0*q1+q2*q3);
Vz=1-2*q1*q1-2*q2*q2;
//求姿态误差
float ex,ey,ez;
ex=ay*Vz-az*Vy;
ey=az*Vx-ax*Vz;
ez=ax*Vy-ay*Vx;
//积分误差
float accex=0,accey=0,accez=0;
accex=accex+ex*Ki*dt;
accey=accey+ey*Ki*dt;
accez=accez+ez*Ki*dt;
//互补滤波，使用pid进行误差修正
gx=gx+Kp*ex+accex;
gy=gy+Kp*ey+accey;
gz=gz+Kp*ez+accez;
//解四元数微分方程如下
gx=gx*(0.5f*dt);
gy=gy*(0.5f*dt);
gz=gz*(0.5f*dt);
q0=q0+(-q1*gx-q2*gy-q3*gz);
q1=q1+(q0*gx+q2*gz-q3*gy);
q2=q2+(q0*gy-q1*gz+q3*gx);
q3=q3+(q0*gz+q1*gy-q2*gx);
//归一化
recip=invSqrt(q0*q0+q1*q1+q2*q2+q3*q3);
q0=q0*recip;
q1=q1*recip;
q2=q2*recip;
q3=q3*recip;
//计算姿态角
float g1,g2,g3,g4,g5;
g1=2.0f*(q1*q3-q0*q2);
g2=2.0f*(q0*q1+q2*q3);
g3=q0*q0-q1*q1-q2*q2+q3*q3;
g4=2.0f*(q1*q2+q0*q3);
g5=q0*q0+q1*q1-q2*q2-q3*q3;
pitch=-asinf(g1)*57.3f;
roll=atanf(g2/g3)*57.3f;
yaw=atanf(g4/g5)*57.3f;
}
.h如下
#ifndef ANGLE_H
#define ANGLE_H
#include "math.h"
typedef struct
{
float LastP;//上次估算协方差 初始化值为0.02
float Now_P;//当前估算协方差 初始化值为0
float out;//卡尔曼滤波器输出 初始化值为0
float Kg;//卡尔曼增益 初始化值为0
float Q;//过程噪声协方差 初始化值为0.001
float R;//观测噪声协方差 初始化值为0.543
}KFP;//Kalman Filter parameter
extern KFP KFP_accelX;
extern KFP KFP_accelY;
extern KFP KFP_accelZ;
extern KFP KFP_gyroX;
extern KFP KFP_gyroY;
extern KFP KFP_gyroZ;
extern float Q_ANGLE_X, Q_ANGLE_Y, Q_ANGLE_Z;
float kalmanFilter(KFP *kfp,float input);
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az);
extern float pitch,roll,yaw;
void algorithm(float ax,float ay,float az,float gx,float gy,float gz);
#endif
经过测试功能正常，但是偏航角没有磁力计矫正所以数据比较差。
参考资料：
AT32F421C8T7利用SPI读取ICM42670原始数据并得到姿态角
arduino.ICM42670P----------github
哔站关于姿态解算的教程
卡尔曼滤波算法c代码实现
1428
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
