BMI088芯片IMU

已于 2024-07-07 00:37:16 修改
阅读量14 收藏
点赞数
文章标签： 网络
于 2023-01-04 10:10:08 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_44749785/article/details/128543469
版权 
/*bmi088.h*/
#ifndef __bmi088_h
#define __bmi088_h

#include "gd32f1x0.h"
#include "public.h"


typedef struct{
	uint8_t acc_chip_id;
	uint8_t gyro_chip_id;
	uint8_t acc_raw_buff[6];
	uint8_t gyro_raw_buff[6];
	
	float acc_adc_x;
	float acc_adc_y;
	float acc_adc_z;
	float gyro_adc_x;
	float gyro_adc_y;
	float gyro_adc_z;
	
	float acc_raw_x;
	float acc_raw_y;
	float acc_raw_z;
	float gyro_raw_x;
	float gyro_raw_y;
	float gyro_raw_z;
	
	s16 out_accx;
	s16 out_accy;
	s16 out_accz;
	s16 out_gyrox;
	s16 out_gyroy;
	s16 out_gyroz;
	
	float kalman_myax;
	float kalman_myay;
	float kalman_myaz;
			
	float kalman_mygx;
	float kalman_mygy;
	float kalman_mygz;
	
	
	float fyaw_angle;
	float fpitch_angle;
	float froll_angle;
	
	s16 hyaw_angle;
	s16 hpitch_angle;
	s16 hroll_angle;

	
}BMI088_DATA;


typedef struct 
{
	float q0; //四元数
	float q1; 
	float q2;
	float q3;		
  float exInt;//叉积计算误差的累计积分
	float	eyInt;
	float ezInt;		
	
	float q0q0;
	float q0q1;
	float q0q2;
	float q0q3;
	float q1q1;
	float q1q2;
	float q1q3;
	float q2q2;
	float q2q3;
	float q3q3;

	float myq0a;
	float myq1a;
	float myq2a;
	float myq3a;

}Q_DATA;

typedef struct 
{
    float Last_P;//上次估算协方差 不可以为0 ! ! ! ! !  一般Last_P=1
    float Now_P;//当前估算协方差
	
    float out;//卡尔曼滤波器输出
	
    float Kg;//卡尔曼增益
    float Q;//过程噪声协方差
    float R;//观测噪声协方差
}KALMAN;
extern KALMAN kalman_ax;
extern KALMAN kalman_ay;
extern KALMAN kalman_az;

extern KALMAN kalman_gx;
extern KALMAN kalman_gy;
extern KALMAN kalman_gz;

extern Q_DATA q_data;

#define BMI088_SELECT_MODE_PROT	            GPIOB 	
#define BMI088_SELECT_MODE_PIN              GPIO_PIN_4
#define BMI088_SELECT_MODE_IIC_ENABLE       GPIO_SetBits(BMI088_SELECT_MODE_PROT,BMI088_SELECT_MODE_PIN)
#define BMI088_SELECT_MODE_SPI_ENABLE       GPIO_ResetBits(BMI088_SELECT_MODE_PROT,BMI088_SELECT_MODE_PIN)


#define LED_WORK_PROT    GPIOA
#define LED_WORK_PIN     GPIO_PIN_4
#define LED_WORK_ON      GPIO_SetBits(LED_WORK_PROT,LED_WORK_PIN)
#define LED_WORK_OFF     GPIO_ResetBits(LED_WORK_PROT,LED_WORK_PIN)

#define BMI088_SPI_MOSI_PROT	      GPIOA  //MOSI  	
#define BMI088_SPI_MOSI_PIN         GPIO_PIN_7//GPIO_PIN_7
#define BMI088_SPI_SDA_H            GPIO_SetBits(BMI088_SPI_MOSI_PROT,BMI088_SPI_MOSI_PIN)
#define BMI088_SPI_SDA_L            GPIO_ResetBits(BMI088_SPI_MOSI_PROT,BMI088_SPI_MOSI_PIN)

#define BMI088_SPI_MISO_PROT	      GPIOA   //MISO 	
#define BMI088_SPI_MISO_PIN         GPIO_PIN_6//GPIO_PIN_6
#define BMI088_SPI_READ             GPIO_ReadInputBit(BMI088_SPI_MISO_PROT,BMI088_SPI_MISO_PIN)


#define BMI088_SPI_CLK_PROT	        GPIOA //SCLK    	
#define BMI088_SPI_CLK_PIN          GPIO_PIN_5
#define BMI088_SPI_CLK_H            GPIO_SetBits(BMI088_SPI_CLK_PROT,BMI088_SPI_CLK_PIN)
#define BMI088_SPI_CLK_L            GPIO_ResetBits(BMI088_SPI_CLK_PROT,BMI088_SPI_CLK_PIN)

#define BMI088_ACC_SPI_CS_PROT	    GPIOB  //CS  	
#define BMI088_ACC_SPI_CS_PIN       GPIO_PIN_8
#define BMI088_ACC_SPI_CS_H         GPIO_SetBits(BMI088_ACC_SPI_CS_PROT,BMI088_ACC_SPI_CS_PIN)
#define BMI088_ACC_SPI_CS_L         GPIO_ResetBits(BMI088_ACC_SPI_CS_PROT,BMI088_ACC_SPI_CS_PIN)

#define BMI088_GYRO_SPI_CS_PROT	    GPIOB  //CS  	
#define BMI088_GYRO_SPI_CS_PIN      GPIO_PIN_3
#define BMI088_GYRO_SPI_CS_H        GPIO_SetBits(BMI088_GYRO_SPI_CS_PROT,BMI088_GYRO_SPI_CS_PIN)
#define BMI088_GYRO_SPI_CS_L        GPIO_ResetBits(BMI088_GYRO_SPI_CS_PROT,BMI088_GYRO_SPI_CS_PIN)


#define BMI088_ACC_SPI_CS_ENABLE   do{BMI088_GYRO_SPI_CS_H;BMI088_ACC_SPI_CS_L;}while(0)
#define BMI088_ACC_SPI_CS_DISABLE  do{BMI088_GYRO_SPI_CS_H;BMI088_ACC_SPI_CS_H;}while(0)

#define BMI088_GYRO_SPI_CS_ENABLE   do{BMI088_ACC_SPI_CS_H;BMI088_GYRO_SPI_CS_L;}while(0)
#define BMI088_GYRO_SPI_CS_DISABLE  do{BMI088_ACC_SPI_CS_H;BMI088_GYRO_SPI_CS_H;}while(0)


/*寄存器 0位:0-->写入数据到IMU  0位:1-->读取IMU数据*/
#define  BMI088_READ_CODE   0x80
#define  BMI088_WRITE_CODE  0x7f

#define  BMI088_READ_ACC_CHIP_ID    0x00//id=0x1E
#define  BMI088_READ_ACC            0x12
#define  BMI088_ACC_SOFTRESET       0x7E
#define  BMI088_ACC_PWR_CTRL        0x7D
#define  BMI088_ACC_PWR_CONF				0x7C//0:工作模式，3:睡眠模式
#define  BMI088_ACC_CONF            0x40//带宽0x8A=400hz
#define  BMI088_ACC_RANGE           0x41//0=3g,1=6g,2=12g,3=24g


#define  BMI088_READ_GYRO_CHIP_ID   0x00//id=0x0F
#define  BMI088_READ_GYRO           0x02
#define  BMI088_GYRO_RANGE          0x0F//0=2000°/s,1=1000°/s,2=500°/s,3=250°/s,4=125°/s  
#define  BMI088_GYRO_BANDWIDTH      0x10//带宽 3=400hz
#define  BMI088_GYRO_LPM1           0x11//模式 0=正常模式 80=睡眠模式 20=深度睡眠模式
#define  BMI088_GYRO_SOFTRESET      0x14



#define Acc_Gain 0.00018310546875f  //加速度转换单位(初始化加速度计量程+-4g，由于mpu6050的数据寄存器是16位的，LSBa = 2*6 / 65535.0)
#define Gyro_Gain 0.0609756f	      //角速度转换为角度(LSBg = 2000*2 / 65535=0.0609756f)
#define Gyro_Gr 0.0010641f		      //角速度转换成弧度(3.1415 / 180 * LSBg)
#define G 9.80665f					        // m/s^2



extern BMI088_DATA g_bmi088_data;


void bmi088_init(void);
void bmi088_hard_spi_init(void);
uint8_t bmi088_spi_read_write_byte(uint8_t data_in);
void bmi088_read_chip_id(void);

void bmi088_set_acc(uint8_t addr, uint8_t data);
void bmi088_set_gyro(uint8_t addr, uint8_t data);

void bmi088_read_acc(uint8_t addr, uint8_t readsize, uint8_t *readdata);
void bmi088_read_gyro(uint8_t addr, uint8_t readsize, uint8_t *readdata);
void imu_prepare_data(float gx,float gy,float gz,float ax,float ay,float az);
void imu_updata(float gx,float gy,float gz,float ax,float ay,float az,Q_DATA *q_data);
float KalmanFilter(KALMAN *kfp,float input);



#endif






















/*bmi08.c*/
#include "bmi088.h"



BMI088_DATA g_bmi088_data={0};
Q_DATA q_data={1,0};
KALMAN kalman_ax;
KALMAN kalman_ay;
KALMAN kalman_az;

KALMAN kalman_gx;
KALMAN kalman_gy;
KALMAN kalman_gz;





/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  scl3300_Init                
**@author:       
**@data:   
**@param:void
**@param:void
**@return:void        
**@descriptions:  scl3300初始化  
**                         
**--------------------------------------------------------------------------------------------------------**/
void bmi088_init(void)
{
	  bmi088_hard_spi_init();
	
    delay_ms(10);
	
	  //加速度计
		bmi088_set_acc(BMI088_ACC_SOFTRESET,0xB6);//复位
		
		bmi088_set_acc(BMI088_ACC_PWR_CTRL,0x04);//打开加速度计
		
		bmi088_set_acc(BMI088_ACC_CONF,0x8C);//配置加速度计带宽为：1600hz
		
		bmi088_set_acc(BMI088_ACC_RANGE,0x01);//配置加速度计量程为：6g
		
		bmi088_set_acc(BMI088_ACC_PWR_CONF,0x00);//进入正常模式
		
		
		//陀螺仪
		bmi088_set_acc(BMI088_GYRO_SOFTRESET,0xB6);//复位
		
		bmi088_set_acc(BMI088_GYRO_RANGE,0x00);//配置加速度计量程为：0x00=2000°/s,02=500°/s
		
		bmi088_set_acc(BMI088_GYRO_BANDWIDTH,0x00);//配置加速度带宽为：2000hz
		
		bmi088_set_acc(BMI088_GYRO_LPM1,0x00);//进入正常模式
		
		delay_ms(100);
}





/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  bmi088_hard_spi_init                
**@author:       
**@data:   
**@param:void
**@param:void
**@return:void        
**@descriptions:  硬件初始化  
**                         
**--------------------------------------------------------------------------------------------------------**/
void bmi088_hard_spi_init(void)
{
 		GPIO_InitPara  GPIO_InitStructure;

	  SPI_InitPara  SPI_InitParaStruct;
	
		RCC_AHBPeriphClock_Enable(RCC_AHBPERIPH_GPIOA  | RCC_AHBPERIPH_GPIOB |RCC_AHBPERIPH_GPIOC | 
														RCC_AHBPERIPH_GPIOD | RCC_AHBPERIPH_GPIOF, ENABLE);	
	
		RCC_APB2PeriphClock_Enable(RCC_APB2PERIPH_CFG,ENABLE);
	
	  RCC_APB2PeriphClock_Enable(RCC_APB2PERIPH_SPI1,ENABLE);
	
	  //LED
		GPIO_InitStructure.GPIO_Pin = LED_WORK_PIN;
		GPIO_InitStructure.GPIO_Mode = GPIO_MODE_OUT;
		GPIO_InitStructure.GPIO_Speed = GPIO_SPEED_50MHZ;
		GPIO_InitStructure.GPIO_OType = GPIO_OTYPE_PP;
		GPIO_InitStructure.GPIO_PuPd = GPIO_PUPD_NOPULL;
		GPIO_Init(LED_WORK_PROT, &GPIO_InitStructure);
		LED_WORK_ON;
	
	
	  //SELECT MODE 
		GPIO_InitStructure.GPIO_Pin = BMI088_SELECT_MODE_PIN;
		GPIO_InitStructure.GPIO_Mode = GPIO_MODE_OUT;
		GPIO_InitStructure.GPIO_Speed = GPIO_SPEED_50MHZ;
		GPIO_InitStructure.GPIO_OType = GPIO_OTYPE_PP;
		GPIO_InitStructure.GPIO_PuPd = GPIO_PUPD_NOPULL;
		GPIO_Init(BMI088_SELECT_MODE_PROT, &GPIO_InitStructure);
		BMI088_SELECT_MODE_SPI_ENABLE;

	  //MOSI
		GPIO_InitStructure.GPIO_Pin = BMI088_SPI_MOSI_PIN;
		GPIO_InitStructure.GPIO_Mode = GPIO_MODE_AF;
		GPIO_InitStructure.GPIO_Speed = GPIO_SPEED_50MHZ;
		GPIO_InitStructure.GPIO_OType = GPIO_OTYPE_PP;
		GPIO_InitStructure.GPIO_PuPd = GPIO_PUPD_PULLUP;
		GPIO_Init(BMI088_SPI_MOSI_PROT, &GPIO_InitStructure); 

	
	  //MISO
		GPIO_InitStructure.GPIO_Pin = BMI088_SPI_MISO_PIN;
		GPIO_InitStructure.GPIO_Mode = GPIO_MODE_AF;
		GPIO_InitStructure.GPIO_Speed = GPIO_SPEED_50MHZ;
		GPIO_InitStructure.GPIO_OType = GPIO_OTYPE_PP;
		GPIO_InitStructure.GPIO_PuPd = GPIO_PUPD_PULLUP;
		GPIO_Init(BMI088_SPI_MISO_PROT, &GPIO_InitStructure); 
	
	  //SCLK
		GPIO_InitStructure.GPIO_Pin = BMI088_SPI_CLK_PIN;
		GPIO_InitStructure.GPIO_Mode = GPIO_MODE_AF;
		GPIO_InitStructure.GPIO_Speed = GPIO_SPEED_50MHZ;
		GPIO_InitStructure.GPIO_OType = GPIO_OTYPE_PP;
		GPIO_InitStructure.GPIO_PuPd = GPIO_PUPD_PULLUP;
		GPIO_Init(BMI088_SPI_CLK_PROT, &GPIO_InitStructure);
		
		
		GPIO_PinAFConfig(GPIOA,GPIO_PINSOURCE5,GPIO_AF_0);
		GPIO_PinAFConfig(GPIOA,GPIO_PINSOURCE6,GPIO_AF_0);
		GPIO_PinAFConfig(GPIOA,GPIO_PINSOURCE7,GPIO_AF_0);


		//ACC_CS
		GPIO_InitStructure.GPIO_Pin = BMI088_ACC_SPI_CS_PIN;
		GPIO_InitStructure.GPIO_Mode = GPIO_MODE_OUT;
		GPIO_InitStructure.GPIO_Speed = GPIO_SPEED_50MHZ;
		GPIO_InitStructure.GPIO_OType = GPIO_OTYPE_PP;
		GPIO_InitStructure.GPIO_PuPd = GPIO_PUPD_NOPULL;
		GPIO_Init(BMI088_ACC_SPI_CS_PROT, &GPIO_InitStructure);
		BMI088_ACC_SPI_CS_H;
		
		
		//GYRO_CS
		GPIO_InitStructure.GPIO_Pin = BMI088_GYRO_SPI_CS_PIN;
		GPIO_InitStructure.GPIO_Mode = GPIO_MODE_OUT;
		GPIO_InitStructure.GPIO_Speed = GPIO_SPEED_50MHZ;
		GPIO_InitStructure.GPIO_OType = GPIO_OTYPE_PP;
		GPIO_InitStructure.GPIO_PuPd = GPIO_PUPD_NOPULL;
		GPIO_Init(BMI088_GYRO_SPI_CS_PROT, &GPIO_InitStructure);
		BMI088_GYRO_SPI_CS_H;
		

		//SPI
		SPI_InitParaStruct.SPI_CRCPOL=7;
		SPI_InitParaStruct.SPI_FirstBit=SPI_FIRSTBIT_MSB;
		SPI_InitParaStruct.SPI_FrameFormat=SPI_FRAMEFORMAT_8BIT;
		SPI_InitParaStruct.SPI_Mode=SPI_MODE_MASTER;
		SPI_InitParaStruct.SPI_PSC=SPI_PSC_256;//SPI_PSC_16
		SPI_InitParaStruct.SPI_SCKPH=SPI_SCKPH_1EDGE;
		SPI_InitParaStruct.SPI_SCKPL=SPI_SCKPL_LOW;
		SPI_InitParaStruct.SPI_SWNSSEN=SPI_SWNSS_SOFT;
		SPI_InitParaStruct.SPI_TransType=SPI_TRANSTYPE_FULLDUPLEX;
		
		SPI_Init(SPI1, &SPI_InitParaStruct);
		
		SPI_Enable(SPI1,ENABLE);
}









/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  hard_spi_init                
**@author:       
**@data:   
**@param:void
**@param:void
**@return:void        
**@descriptions:  
**                         
**--------------------------------------------------------------------------------------------------------**/
uint8_t bmi088_spi_read_write_byte(uint8_t data_in)
{		
	uint8_t retry=0;				 	
	while (SPI_I2S_GetBitState(SPI1, SPI_FLAG_TBE) == RESET) //检查指定的SPI标志位设置与否:发送缓存空标志位
		{
		retry++;
		if(retry>100)return 0;
		}			  
		SPI_I2S_SendData(SPI1, data_in); //通过外设SPIx发送一个数据
		retry=0;

	while (SPI_I2S_GetBitState(SPI1, SPI_FLAG_RBNE) == RESET) //检查指定的SPI标志位设置与否:接受缓存非空标志位
		{
		retry++;
		if(retry>100)return 0;
		}	  
		
	return SPI_I2S_ReceiveData(SPI1); //返回通过SPIx最近接收的数据					    
}







/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  bmi088_read_chip_id                
**@author:       
**@data:   
**@param:void
**@param:void
**@return:void        
**@descriptions:  读取芯片加速度计id 和读取芯片陀螺仪id BMI字节是小端在前大端在后
**                         
**--------------------------------------------------------------------------------------------------------**/
void bmi088_read_chip_id(void)
{
	BMI088_ACC_SPI_CS_ENABLE;
	bmi088_spi_read_write_byte(BMI088_READ_ACC_CHIP_ID | BMI088_READ_CODE);//
	bmi088_spi_read_write_byte(0xFF);/*需抛弃不可用，下一个字节是可用*/
	g_bmi088_data.acc_chip_id=bmi088_spi_read_write_byte(0xFF);
	BMI088_ACC_SPI_CS_DISABLE;
			
			
	BMI088_GYRO_SPI_CS_ENABLE;
	bmi088_spi_read_write_byte(BMI088_READ_GYRO_CHIP_ID | BMI088_READ_CODE);//
	g_bmi088_data.gyro_chip_id=bmi088_spi_read_write_byte(0xFF);
	BMI088_GYRO_SPI_CS_DISABLE;

}


/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  bmi088_read_chip_id                
**@author:       
**@data:   
**@param:void
**@param:void
**@return:void        
**@descriptions:  往芯片加速度计写数据
**                         
**--------------------------------------------------------------------------------------------------------**/
void bmi088_set_acc(uint8_t addr, uint8_t data)
{
	BMI088_ACC_SPI_CS_ENABLE;
	bmi088_spi_read_write_byte(addr & BMI088_WRITE_CODE);//BMI088_READ_ACC_CHIP_ID
	bmi088_spi_read_write_byte(data);
	delay_ms(50);
	BMI088_ACC_SPI_CS_DISABLE;
}



/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  bmi088_read_chip_id                
**@author:       
**@data:   
**@param:void
**@param:void
**@return:void        
**@descriptions:  往芯片陀螺仪写数据
**                         
**--------------------------------------------------------------------------------------------------------**/
void bmi088_set_gyro(uint8_t addr, uint8_t data)
{
	BMI088_GYRO_SPI_CS_ENABLE;
	bmi088_spi_read_write_byte(addr & BMI088_WRITE_CODE);//BMI088_READ_ACC_CHIP_ID
	bmi088_spi_read_write_byte(data);
	delay_ms(50);
	BMI088_GYRO_SPI_CS_DISABLE;
}





/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  bmi088_read_chip_id                
**@author:       
**@data:   
**@param:void
**@param:void
**@return:void        
**@descriptions:  读取芯片加速度计
**                         
**--------------------------------------------------------------------------------------------------------**/
void bmi088_read_acc(uint8_t addr, uint8_t readsize, uint8_t *readdata)
{
	  uint8_t i=0;
    BMI088_ACC_SPI_CS_ENABLE;
		
	  bmi088_spi_read_write_byte(addr | BMI088_READ_CODE);
	
    bmi088_spi_read_write_byte(0XFF);/*抛弃不可用，下一个字节是可用*/
	
    for ( i = 0; i < readsize; i++) /*芯片内部寄存器地址会自动增加*/
	  {
			readdata[i]=bmi088_spi_read_write_byte(0XFF);
    }
		
    BMI088_ACC_SPI_CS_DISABLE;
		
    g_bmi088_data.acc_adc_x = ((int16_t)((g_bmi088_data.acc_raw_buff[1]) << 8) | g_bmi088_data.acc_raw_buff[0]);
    g_bmi088_data.acc_adc_y = ((int16_t)((g_bmi088_data.acc_raw_buff[3]) << 8) | g_bmi088_data.acc_raw_buff[2]);
    g_bmi088_data.acc_adc_z = ((int16_t)((g_bmi088_data.acc_raw_buff[5]) << 8) | g_bmi088_data.acc_raw_buff[4]);
		
//		g_bmi088_data.acc_raw_x= g_bmi088_data.acc_adc_x/(32768.0/6.0);
//		g_bmi088_data.acc_raw_y= g_bmi088_data.acc_adc_y/(32768.0/6.0);
//		g_bmi088_data.acc_raw_z= g_bmi088_data.acc_adc_z/(32768.0/6.0);
}



			
			


/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  bmi088_read_chip_id                
**@author:       
**@data:   
**@param:void
**@param:void
**@return:void        
**@descriptions:  读取芯片陀螺仪
**                         
**--------------------------------------------------------------------------------------------------------**/
void bmi088_read_gyro(uint8_t addr, uint8_t readsize, uint8_t *readdata)
{
	  uint8_t i=0;
    BMI088_GYRO_SPI_CS_ENABLE;
		
	  bmi088_spi_read_write_byte(addr | BMI088_READ_CODE);
		
    for ( i = 0; i < readsize; i++) /*芯片内部寄存器地址会自动增加*/
	  {
			readdata[i]=bmi088_spi_read_write_byte(0XFF);
    }
		
    BMI088_GYRO_SPI_CS_DISABLE;
		
    g_bmi088_data.gyro_adc_x = ((int16_t)((g_bmi088_data.gyro_raw_buff[1]) << 8) | g_bmi088_data.gyro_raw_buff[0]);
    g_bmi088_data.gyro_adc_y = ((int16_t)((g_bmi088_data.gyro_raw_buff[3]) << 8) | g_bmi088_data.gyro_raw_buff[2]);
    g_bmi088_data.gyro_adc_z = ((int16_t)((g_bmi088_data.gyro_raw_buff[5]) << 8) | g_bmi088_data.gyro_raw_buff[4]);
		
//		g_bmi088_data.gyro_raw_x= g_bmi088_data.gyro_adc_x/(32768.0/2000.0);
//		g_bmi088_data.gyro_raw_y= g_bmi088_data.gyro_adc_y/(32768.0/2000.0);
//		g_bmi088_data.gyro_raw_z= g_bmi088_data.gyro_adc_z/(32768.0/2000.0);
}










static float invSqrt(float x) 		//快速计算 1/Sqrt(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;
}






#define Q_KP 1.50f
#define Q_KI 0.001f
#define Q_HALFT  0.005//采样周期的一半，如500h采集 则是250hz 单位s 1000hz=0.0005f 500hz=0.001
void imu_updata(float gx,float gy,float gz,float ax,float ay,float az,Q_DATA *q_data)
{

			float vx,vy,vz=0;							//实际重力加速度
			float ex,ey,ez=0;							//叉积计算的误差
			float norm=0;
			
			q_data->q0q0 = q_data->q0 * q_data->q0;
			q_data->q0q1 = q_data->q0 * q_data->q1;
			q_data->q0q2 = q_data->q0 * q_data->q2;
			q_data->q0q3 = q_data->q0 * q_data->q3;
			q_data->q1q1 = q_data->q1 * q_data->q1;
			q_data->q1q2 = q_data->q1 * q_data->q2;
			q_data->q1q3 = q_data->q1 * q_data->q3;
			q_data->q2q2 = q_data->q2 * q_data->q2;
			q_data->q2q3 = q_data->q2 * q_data->q3;
			q_data->q3q3 = q_data->q3 * q_data->q3;
	
			if(ax*ay*az == 0)return;
			
			//加速度计测量的重力方向(机体坐标系)
			norm = invSqrt(ax*ax + ay*ay + az*az);			
			ax = ax * norm;
			ay = ay * norm;
			az = az * norm;
			
			//四元数推出的实际重力方向(机体坐标系)
			vx = 2*(q_data->q1q3 - q_data->q0q2);												
			vy = 2*(q_data->q0q1 + q_data->q2q3);
			vz = q_data->q0q0 - q_data->q1q1 - q_data->q2q2 + q_data->q3q3;
			
			//叉积误差
			ex = (ay*vz - az*vy);
			ey = (az*vx - ax*vz);
			ez = (ax*vy - ay*vx);
			
			//叉积误差积分为角速度
			q_data->exInt = q_data->exInt + ex * Q_KI;
			q_data->eyInt = q_data->eyInt + ey * Q_KI;
			q_data->ezInt = q_data->ezInt + ez * Q_KI;
			
			//角速度补偿
			gx = gx + Q_KP*ex + q_data->exInt;
			gy = gy + Q_KP*ey + q_data->eyInt;
			gz = gz + Q_KP*ez + q_data->ezInt;
			
			//更新四元数
//			q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
//			q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
//			q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
//			q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
			
			q_data->myq0a=(-q_data->q1*gx - q_data->q2*gy - q_data->q3*gz);
			if(fabs(q_data->myq0a)<0.01){q_data->myq0a=0;}
			if(fabs(q_data->q0)<0.001){q_data->q0=0;}
			q_data->q0 = q_data->q0 + q_data->myq0a * Q_HALFT;
			
			q_data->myq1a=(q_data->q0*gx + q_data->q2*gz - q_data->q3*gy);
			if(fabs(q_data->myq1a)<0.01){q_data->myq1a=0;}
			if(fabs(q_data->q1)<0.001){q_data->q1=0;}
			q_data->q1 = q_data->q1 + q_data->myq1a*Q_HALFT;
			
			q_data->myq2a=(q_data->q0*gy - q_data->q1*gz + q_data->q3*gx);
			if(fabs(q_data->myq2a)<0.01){q_data->myq2a=0;}
			if(fabs(q_data->q2)<0.001){q_data->q2=0;}
			q_data->q2 = q_data->q2 + q_data->myq2a *Q_HALFT;
			
			q_data->myq3a=(q_data->q0*gz + q_data->q1*gy - q_data->q2*gx);
			if(fabs(q_data->myq3a)<0.01){q_data->myq3a=0;}
			if(fabs(q_data->q3)<0.001){q_data->q3=0;}//0.0005
			q_data->q3 = q_data->q3 + (q_data->myq3a)*Q_HALFT;	
			
			
			
			//单位化四元数
			norm = invSqrt(q_data->q0 * q_data->q0 + q_data->q1 * q_data->q1 + q_data->q2 * q_data->q2 + q_data->q3 * q_data->q3);
			q_data->q0 = q_data->q0 * norm;
			q_data->q1 = q_data->q1 * norm;
			q_data->q2 = q_data->q2 * norm;  
			q_data->q3 = q_data->q3 * norm;
			
			//四元数反解欧拉角
			g_bmi088_data.fyaw_angle = (float)(atan2(2.f * (q_data->q1q2 + q_data->q0q3), q_data->q0q0 + q_data->q1q1 - q_data->q2q2 - q_data->q3q3)* 57.3f);
			g_bmi088_data.hyaw_angle = g_bmi088_data.fyaw_angle*100;
			
			g_bmi088_data.fpitch_angle = (float)(-asin(2.f * (q_data->q1q3 - q_data->q0q2))* 57.3f);
			g_bmi088_data.hpitch_angle=g_bmi088_data.fpitch_angle;
			
			g_bmi088_data.froll_angle = (float)(atan2(2.f * q_data->q2q3 + 2.f * q_data->q0q1, q_data->q0q0 - q_data->q1q1 - q_data->q2q2 + q_data->q3q3)* 57.3f);
			g_bmi088_data.hroll_angle=g_bmi088_data.froll_angle;
}




/**--------------functiion Info--------------------------------------------------------------------------------- 
**@functiion name:  KalmanFilter                
**@author:       
**@data:   
**@param:卡尔曼结构图
**@param:滤波值 (即传感器的采集值)
**@return:滤波后的值(最优值)       
**@descriptions:  一维卡尔曼滤波器
**                         
**--------------------------------------------------------------------------------------------------------**/
float KalmanFilter(KALMAN *kfp,float input)
{
   float temp0=0;

   //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
   kfp->Now_P = kfp->Last_P + kfp->Q;
   
   //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
	 //   kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
   temp0=(kfp->Now_P + kfp->R);
   if(temp0==0){temp0=0.00001;}
   kfp->Kg = kfp->Now_P / temp0;

   //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
   kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
   
   //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
   kfp->Last_P = (1-kfp->Kg) * kfp->Now_P;
   
   return kfp->out;
}




void imu_prepare_data(float gx,float gy,float gz,float ax,float ay,float az)
{
     float kalman_myax=0;
	   float kalman_myay=0;
	   float kalman_myaz=0;
	   float kalman_mygx=0;
	   float kalman_mygy=0;
	   float kalman_mygz=0;
	
	
		
			//卡尔曼滤波
			kalman_myax=KalmanFilter(&kalman_ax,ax);
			kalman_myay=KalmanFilter(&kalman_ay,ay);
			kalman_myaz=KalmanFilter(&kalman_az,az);
			
			kalman_mygx=KalmanFilter(&kalman_gx,gx);
			kalman_mygy=KalmanFilter(&kalman_gy,gy);
			kalman_mygz=KalmanFilter(&kalman_gz,gz);
			
			//获取输出的数据 与计算角度无关
			g_bmi088_data.out_accx=(float)(kalman_myax * Acc_Gain *1000);  //g *1000
			g_bmi088_data.out_accy=(float)(kalman_myay * Acc_Gain *1000);
			g_bmi088_data.out_accz=(float)(kalman_myaz * Acc_Gain *1000);
			
			g_bmi088_data.out_gyroz=(float)(kalman_mygz * Gyro_Gain *50); //deg/s *50
	
	
	   //单位转换 将加速度原始AD值转换为m/s^2
			g_bmi088_data.kalman_myax = (float)kalman_myax * Acc_Gain * G;
			g_bmi088_data.kalman_myay = (float)kalman_myay * Acc_Gain * G;
			g_bmi088_data.kalman_myaz = (float)kalman_myaz * Acc_Gain * G;
			
			//单位转换 将陀螺仪AD值转换为 弧度/s
			g_bmi088_data.kalman_mygx = (float)kalman_mygx * Gyro_Gr;
			g_bmi088_data.kalman_mygy = (float)kalman_mygy * Gyro_Gr;
			g_bmi088_data.kalman_mygz = (float)kalman_mygz * Gyro_Gr;
			
}








/*main.c*/
int main(void)
{
 	  //速度卡尔曼
		kalman_ax.Kg=0;
		kalman_ax.Now_P=0;	
		kalman_ax.Last_P=1;
		kalman_ax.Q=0.01;
		kalman_ax.R=10;
		kalman_ax.R=0;
		
		kalman_ay.Kg=0;
		kalman_ay.Now_P=0;	
		kalman_ay.Last_P=1;
		kalman_ay.Q=0.01;
		kalman_ay.R=10;
		kalman_ay.R=0;
		
		kalman_az.Kg=0;
		kalman_az.Now_P=0;	
		kalman_az.Last_P=1;
		kalman_az.Q=0.01;
		kalman_az.R=10;
		kalman_az.R=0;
		
		kalman_gx.Kg=0;
		kalman_gx.Now_P=0;	
		kalman_gx.Last_P=1;
		kalman_gx.Q=0.01;
		kalman_gx.R=10;
		kalman_gx.R=0;
		
		kalman_gy.Kg=0;
		kalman_gy.Now_P=0;	
		kalman_gy.Last_P=1;
		kalman_gy.Q=0.01;
		kalman_gy.R=10;
		kalman_gy.R=0;
		
		kalman_gz.Kg=0;
		kalman_gz.Now_P=0;	
		kalman_gz.Last_P=1;
		kalman_gz.Q=0.01;
		kalman_gz.R=10;
		kalman_gz.R=0;
		
		bmi088_init();//初始化mpu
		
   while(1)
   {

   }


}
"六月的雨"
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
基于NUCLEO-F767ZI开发板使用BMI088六轴加速度传感器1
08-03
本文将探讨如何基于STMicroelectronics的NUCLEO-F767ZI开发板和Bosch BMI088六轴加速度传感器进行数据采集。BMI088是一款高性能的传感器，它集成了三轴加速度计和三轴陀螺仪，适用于需要高精度和快速响应的设备，如...
BMI088PDF手册
09-17
BMI088是一款六轴运动跟踪惯性测量单元（IMU），用于检测六自由度（6DoF）的运动和旋转。它结合了两个惯性传感器的功能：高级三轴16位陀螺仪和先进的三轴16位加速度计。 2. 设计和功能 BMI088旨在满足高性能消费者...
BMI088加速度传感器Arduino库使用方法简介1
08-03
BMI088加速度传感器是博世Sensortec推出的一款高性能六轴惯性测量单元（IMU），专为无人机和机器人应用设计，具备高振动稳定性。这款传感器能有效地抑制由于PCB共振或系统结构引起的振动，同时拥有卓越的温度稳定性...
Intel Curie芯片内置IMU传感器：BMI160手册
03-03
Intel Curie芯片内置IMU传感器：BMI160手册
SpringBoot整合SSE，实现后端主动推送DEMO
zjy660358的专栏
07-17 345
说起服务端主动推送，大家第一个想到的一定是WEBSOCKET。作为软件工程师，不能无脑使用一种技术，要结合实际情况，择优选取。SSE（Server-Sent Events）相比于WEBSOCKET1、轻量化、兼容性 基于传统的HTTP协议，所以浏览器兼容性比较好2、 只支持单向通讯。（服务器->客户端）
27_MobileNetV3网络详解
https://github.com/foxpup11?tab=repositories
07-17 811
https://www.bilibili.com/video/BV1GK4y1p7uE/?spm_id_from=333.999.0.0&vd_source=7dace3632125a1ef7fd32c285eb2fbac
RTI DDS大数据碎片
qq_33089547的博客
07-17 974
PublicationBuiltingTopicData或SubscriptionBuiltnTopicData样本较大的最常见原因是序列化的TypeCode或TypeObject，但您也可能发送了大量属性（通过7.5.19 PROPERTY QosPolicy（DDS扩展））或具有较大的ContentFilteredTopic筛选器表达式，以及其他大小可变的字段，这可能会导致样本大小较大。如果您尝试发送一个大小大于MTU的DDS样本，但尚未设置DDS级碎片，您将看到IP级碎片。
Internet 控制报文协议 —— ICMPv4 和 ICMPv6 详解
u013669912的博客
07-17 884
…
计算机网络技术期末复习
CWPIN21的博客
07-17 1093
本文深入探讨了计算机网络的核心概念与关键技术。涵盖了网络拓扑结构，如星型、总线型和环形等，分析了它们的特点与适用场景。详细阐述了 TCP/IP 协议栈的工作原理，包括 IP 地址分配、子网掩码的作用以及路由选择算法。还探讨了网络安全方面的常见威胁，如病毒、黑客攻击等，并介绍了相应的防护措施。通过实例，让读者对计算机网络有更全面、深入的理解。
使用tcpdump 和 Wireshark进行简单TCP抓包分析【图文教程】
最新发布
qq_42037383的博客
07-19 752
和都是网络分析工具，用于捕获和分析网络数据包，但它们在功能和使用上有所不同。所以，这两者实际上是搭配使用的，先用 tcpdump 命令在 Linux 服务器上抓包，接着把抓包的文件拖出到 Windows 电脑后，用 Wireshark 可视化分析。如果你是在 Windows 上抓包，只需要用 Wireshark 工具就可以。
网络抓包工具tcpdump的使用
fengzhan12138的博客
07-17 898
tcpdump命令是基于unix系统的命令行的数据报嗅探工具，可以抓取流动在网卡上的数据包，熟悉 tcpdump 的使用能够帮助你分析调试网络数据。
UDP协议
bushibrnxiaohaij的博客
07-17 1073
所以UDP在发送报文前只需要把把头的结构化字段填好，然后防止报文的前面就可以像服务器进行请求了，但是不管是服务器还是客服端，都一定存在大量的UDP报文，所以OS也一定要对这么多的报文进行管理，所以OS也会存在对报文描述的结构体，报文本质就是数据，所以就是一段缓冲区，所以描述报文的结构体里面一定会存在指针。UDP的报头非常简单，存在16位源端口和16为目的端口，16位UDP长度, 表示整个数据报(UDP首部+UDP数据)的最大长度，如果校验和出错, 就会直接丢弃。UDP协议是传输层的协议，是在应用层之下的。
深入理解Linux网络（一）：内核如何接收网络包
又见南风的博客
07-18 800
在上⾯的代码中跟踪函数调⽤， __igb_open => igb_request_irq => igb_request_msix , 在 igb_request_msix 中我们看到了，对于多队列的⽹卡，为每⼀个队列都注册了中断，其对应的中断处理函数是 igb_msix_ring（该函数也在 drivers/net/ethernet/intel/igb/igb_main.c 下）。如协议注册⼩节看到 inet_protos 中保存着 tcp_v4_rcv() 和 udp_rcv() 的函数地址。
linux学习笔记整理: 关于linux系统介绍 2024/7/16；
gzx233的博客
07-16 1148
linux基础介绍和指令
响应“一机两用”政策号召 提高政务内外网安全
Canon_YK的博客
07-17 994
随着一机两用政策的实施，即政府部门内部网络终端在特定情况下需同时连接政务内外网，如何在保证工作效率的同时，确保网络安全和数据安全，成为摆在面前的重要课题。未来，随着技术的不断发展和完善，沙箱技术将在政务内外网安全中发挥更加重要的作用。深信达的SDC沙箱技术已经和多个政企单位合作，高效便捷的沙箱技术已经大大提升了政务机关的内网安全，为政务机关业务的开展提供了强有力的安全保障。SDC沙箱技术能够对隔离环境中的网络行为进行实时监控和审计，包括网络连接的建立、数据的传输、应用程序的运行等。
TCP/IP协议，以及对等网络通信原理！
g188_2657的博客
07-16 410
补充：那两个不通的包发给了B，B扔的是两个包（原因：两边发会更快，机器会采取更快的方式），所以，不同的物理网段中应该配置不同的逻辑网段。重点核心：源IP和目标IP是始终不变的，而MAC是需要每过一个逻辑网段就要重新拆包，发包。：超文本传输协议（网站访问WEB）（Apache、nginx）(IIS)电脑A----S1----R1-----R2-----S2---电脑B。一、对等网络通信原理：公有----公有 私有----私有。防火墙：（IP、MAC、底层协议、端口、：国际（互联网）协议（IP地址）
堆叠和集群
2301_79810514的博客
07-18 316
堆叠 ID：设备的槽位号，在堆叠系统中管理和唯一标识一台成员交换机，不能冲突交换机的。1. 主交换机退出：备交换机升为主交换机，原备交换机重新计算拓扑并同步给成员交换机，2. 备交换机：作为主的备份，主交换机故障后备交换机接替业务，每个堆叠系统只有一台。3. 从交换机：堆叠系统中剩下的就是从交换机，可以多太，负责流量转发，备交换机故障。堆叠系统：多台设备堆叠后组成一台逻辑交换机称作堆叠系统，堆叠系统中的单台交换机教。成员退出：某台交换机离开堆叠系统，根据离开交换机的角色不同，对堆叠系统影响不同。
5 webSocket
ever__ever的博客
07-19 386
webSockets 是一种先进的技术;它可以在用户的浏览器和服务器之间打开交互式通信会话;使用此 API,您可以向服务器发送消息并接收事件驱动的响应,而无需通过轮询服务器的方式以获得响应
BMI088六轴运动追踪传感器数据手册
BMI088是一款专为高精度运动和旋转检测设计的六自由度（6DoF）惯性测量单元（IMU）。它将先进的16位三轴陀螺仪和具有创新性的16位三轴加速度计的功能集于一身。这款传感器旨在满足在严酷振动环境下运行的高性能消费...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值