BMI088芯片IMU
BMI088芯片IMU
·
/*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)
{
}
}
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献1条内容
所有评论(0)