lijie/foc_Gimbal
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
lijie
2024-01-24 15:56:10 +08:00
commit 2c58355e63
40 changed files with 5524 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
IMU/alldata.h Normal file
View File

@ -0,0 +1,25 @@
#ifndef __ALLDATA_H
#define __ALLDATA_H
#include "main.h"
typedef struct{
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
}_st_Mpu;
typedef struct{
float roll;
float pitch;
float yaw;
}_st_AngE;
extern _st_Mpu ICM42688;
extern _st_AngE Angle;
#endif

443
IMU/icm42688.c Normal file
View File

@ -0,0 +1,443 @@
#include "icm42688.h"
#include "kalman.h"
#include "alldata.h"
// ICM42688<38><38><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>
float icm42688_acc_x, icm42688_acc_y, icm42688_acc_z ;
// ICM42688<38>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
float icm42688_gyro_x, icm42688_gyro_y, icm42688_gyro_z ;
static float icm42688_acc_inv = 1, icm42688_gyro_inv = 1;
void Spi_GPIO_Init()
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pins : PBPin PB12 */
GPIO_InitStruct.Pin = GPIO_PIN_12 | GPIO_PIN_13 |GPIO_PIN_15;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/*Configure GPIO pins : PBPin PB14 */
GPIO_InitStruct.Pin = GPIO_PIN_14;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}
void Init_ICM42688(void)
{
unsigned char time;
unsigned char model;
Spi_GPIO_Init();
// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD>
model = 0xff;
while(1)
{
Read_Datas_ICM42688(ICM42688_WHO_AM_I, &model, 1);
if(model == 0x47)
{
// ICM42688
break;
}
ICM42688_DELAY_MS(10);
}
Write_Data_ICM42688(ICM42688_PWR_MGMT0,0x00); // <20><>λ<EFBFBD>豸
ICM42688_DELAY_MS(10); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWR<57><52>MGMT0<54>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD>200us<75>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κζ<CEBA>д<EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
// <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD>ٶȼƺ<C8BC><C6BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD><EFBFBD>̺<EFBFBD><CCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Set_LowpassFilter_Range_ICM42688(ICM42688_AFS_16G, ICM42688_AODR_32000HZ, ICM42688_GFS_2000DPS, ICM42688_GODR_32000HZ);
Write_Data_ICM42688(ICM42688_PWR_MGMT0, 0x0f); // <20><><EFBFBD><EFBFBD>GYRO_MODE,ACCEL_MODEΪ<45><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
ICM42688_DELAY_MS(10);
}
/**
*
* @brief <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD>
* @param
* @return void
* @notes <20><>λ:g(m/s^2),<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
* Example: Get_Acc_ICM42688();
*
**/
void Get_Acc_ICM42688(void)
{
unsigned char dat[6];
Read_Datas_ICM42688(ICM42688_ACCEL_DATA_X1, dat, 6);
icm42688_acc_x = icm42688_acc_inv * (short int)(((short int)dat[0] << 8) | dat[1]);
icm42688_acc_y = icm42688_acc_inv * (short int)(((short int)dat[2] << 8) | dat[3]);
icm42688_acc_z = icm42688_acc_inv * (short int)(((short int)dat[4] << 8) | dat[5]);
}
/**
*
* @brief <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>ǽǼ<C7BD><C7BC>ٶ<EFBFBD>
* @param
* @return void
* @notes <20><>λΪ:<3A><>/s,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
* Example: Get_Gyro_ICM42688();
*
**/
void Get_Gyro_ICM42688(void)
{
unsigned char dat[6];
Read_Datas_ICM42688(ICM42688_GYRO_DATA_X1, dat, 6);
icm42688_gyro_x = icm42688_gyro_inv * (short int)(((short int)dat[0] << 8) | dat[1]);
icm42688_gyro_y = icm42688_gyro_inv * (short int)(((short int)dat[2] << 8) | dat[3]);
icm42688_gyro_z = icm42688_gyro_inv * (short int)(((short int)dat[4] << 8) | dat[5]);
}
/**
*
* @brief <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param afs // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param aodr // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param gfs // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param godr // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* Example: Set_LowpassFilter_Range_ICM42688(ICM42688_AFS_16G,ICM42688_AODR_32000HZ,ICM42688_GFS_2000DPS,ICM42688_GODR_32000HZ);
*
**/
void Set_LowpassFilter_Range_ICM42688(enum icm42688_afs afs, enum icm42688_aodr aodr, enum icm42688_gfs gfs, enum icm42688_godr godr)
{
Write_Data_ICM42688(ICM42688_ACCEL_CONFIG0, (afs << 5) | (aodr + 1)); // <20><>ʼ<EFBFBD><CABC>ACCEL<45><4C><EFBFBD>̺<EFBFBD><CCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(p77)
Write_Data_ICM42688(ICM42688_GYRO_CONFIG0, (gfs << 5) | (godr + 1)); // <20><>ʼ<EFBFBD><CABC>GYRO<52><4F><EFBFBD>̺<EFBFBD><CCBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(p76)
switch(afs)
{
case ICM42688_AFS_2G:
icm42688_acc_inv = 2000 / 32768.0f; // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>2g
break;
case ICM42688_AFS_4G:
icm42688_acc_inv = 4000 / 32768.0f; // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>4g
break;
case ICM42688_AFS_8G:
icm42688_acc_inv = 8000 / 32768.0f; // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>8g
break;
case ICM42688_AFS_16G:
icm42688_acc_inv = 16000 / 32768.0f; // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>16g
break;
default:
icm42688_acc_inv = 1; // <20><>ת<EFBFBD><D7AA>Ϊʵ<CEAA><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
switch(gfs)
{
case ICM42688_GFS_15_625DPS:
icm42688_gyro_inv = 15.625f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>15.625dps
break;
case ICM42688_GFS_31_25DPS:
icm42688_gyro_inv = 31.25f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>31.25dps
break;
case ICM42688_GFS_62_5DPS:
icm42688_gyro_inv = 62.5f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>62.5dps
break;
case ICM42688_GFS_125DPS:
icm42688_gyro_inv = 125.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>125dps
break;
case ICM42688_GFS_250DPS:
icm42688_gyro_inv = 250.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>250dps
break;
case ICM42688_GFS_500DPS:
icm42688_gyro_inv = 500.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>500dps
break;
case ICM42688_GFS_1000DPS:
icm42688_gyro_inv = 1000.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>1000dps
break;
case ICM42688_GFS_2000DPS:
icm42688_gyro_inv = 2000.0f / 32768.0f; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ:<3A><>2000dps
break;
default:
icm42688_gyro_inv = 1; // <20><>ת<EFBFBD><D7AA>Ϊʵ<CEAA><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
break;
}
}
/**
*
* @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д8bit<69><74><EFBFBD><EFBFBD>
* @param data <20><><EFBFBD><EFBFBD>
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* Example: Write_8bit_ICM42688(0x00);
*
**/
static void Write_8bit_ICM42688(unsigned char dat)
{
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
if(0x80 & dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
}
else
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
dat <<= 1;
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
if(0x80 & dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
}
else
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
dat <<= 1;
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
if(0x80 & dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
}
else
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
dat <<= 1;
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
if(0x80 & dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
}
else
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
dat <<= 1;
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
if(0x80 & dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
}
else
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
dat <<= 1;
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
if(0x80 & dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
}
else
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
dat <<= 1;
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
if(0x80 & dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
}
else
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
dat <<= 1;
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
if(0x80 & dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,1);
}
else
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_15,0);
}
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
dat <<= 1;
}
/**
*
* @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>8bit<69><74><EFBFBD><EFBFBD>
* @param data <20><><EFBFBD><EFBFBD>
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* Example: Read_8bit_ICM42688(dat);
*
**/
static void Read_8bit_ICM42688(unsigned char *dat)
{
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
*dat = *dat << 1;
*dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
*dat = *dat << 1;
*dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
*dat = *dat << 1;
*dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
*dat = *dat << 1;
*dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
*dat = *dat << 1;
*dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
*dat = *dat << 1;
*dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
*dat = *dat << 1;
*dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
*dat = *dat << 1;
*dat |=sys_gpio_pin_get(GPIOB,GPIO_PIN_14);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
}
/**
*
* @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
* @param reg <20>Ĵ<EFBFBD><C4B4><EFBFBD>
* @param data <20><>Ҫд<D2AA><D0B4><EFBFBD>üĴ<C3BC><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* Example: Write_Data_ICM42688(0x00,0x00);
*
**/
static void Write_Data_ICM42688(unsigned char reg, unsigned char dat)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_12,0);
Write_8bit_ICM42688(reg);
Write_8bit_ICM42688(dat);
sys_gpio_pin_set(GPIOB,GPIO_PIN_12,1);
}
/**
*
* @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
* @param reg <20>Ĵ<EFBFBD><C4B4><EFBFBD>
* @param data <20>Ѷ<EFBFBD><D1B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>data
* @param num <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* Example: Read_Datas_ICM42688(0x00,0x00,1);
*
**/
static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num)
{
sys_gpio_pin_set(GPIOB,GPIO_PIN_12,0);
reg |= 0x80;
Write_8bit_ICM42688(reg);
while(num--) Read_8bit_ICM42688(dat++);
sys_gpio_pin_set(GPIOB,GPIO_PIN_12,1);
}
/* <20><>ȡMPU6050<35><30><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>˲<EFBFBD> */
//<2F><><EFBFBD><EFBFBD>ֵ:0,<2C><><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
_st_Mpu ICM42688;
static volatile int16_t *pMpu = (int16_t *)&ICM42688;
int16_t MpuOffset[6] = {0}; //MPU6050<35><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
uint8_t MpuGetData(void)
{
uint8_t i;
int res;
unsigned char buffer[12];
Read_Datas_ICM42688(ICM42688_ACCEL_DATA_X1,buffer,12);
for(i=0;i<6;i++)
{
pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i]; /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16bit<69><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥˮƽУ׼ֵ */
if(i < 3) /* <20>Ǽ<EFBFBD><C7BC>ٶȿ<D9B6><C8BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD> */
{
{
static struct KalmanFilter EKF[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};
kalmanfiter(&EKF[i],(float)pMpu[i]);
pMpu[i] = (int16_t)EKF[i].Out;
}
}
if(i > 2) /* <20><><EFBFBD>ٶ<EFBFBD>һ<EFBFBD>׻<EFBFBD><D7BB><EFBFBD><EFBFBD>˲<EFBFBD> */
{
uint8_t k=i-3;
const float factor = 0.15f;
static float tBuff[3];
pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
}
}
return 0;
}
void get_IMU(float *pitch,float *roll)
{
*roll = atan2(ICM42688.accY, ICM42688.accZ) * 180.0 / _PI;
*pitch = atan2(-ICM42688.accX, sqrt(ICM42688.accX * ICM42688.accY + ICM42688.accZ * ICM42688.accZ)) * 180.0 / _PI;
// *roll = atan2(ICM42688.accY, ICM42688.accZ);
// *pitch = atan2(-ICM42688.accX, sqrt(ICM42688.accX * ICM42688.accY + ICM42688.accZ * ICM42688.accZ));
}
/**
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŵ<EFBFBD>ƽ
*/
void sys_gpio_pin_set(GPIO_TypeDef *p_gpiox, uint16_t pinx, uint8_t status)
{
if (status & 0X01)
{
p_gpiox->BSRR |= pinx;
}
else
{
p_gpiox->BSRR |= (uint32_t)pinx << 16;
}
}
/**
* <20><>ȡ<EFBFBD><C8A1><EFBFBD>ŵ<EFBFBD>ƽ
*/
uint8_t sys_gpio_pin_get(GPIO_TypeDef *p_gpiox, uint16_t pinx)
{
if (p_gpiox->IDR & pinx)
{
return 1;
}
else
{
return 0;
}
}

204
IMU/icm42688.h Normal file
View File

@ -0,0 +1,204 @@
#ifndef _DMX_ICM42688_H_
#define _DMX_ICM42688_H_
#include "stm32h7xx_hal.h"
#include "mymain.h"
#define ICM42688_DELAY_MS(time) (HAL_Delay(time))
extern float icm42688_acc_x, icm42688_acc_y, icm42688_acc_z ; // <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>
extern float icm42688_gyro_x, icm42688_gyro_y, icm42688_gyro_z ; // <20><><EFBFBD><EFBFBD>ICM42688<38>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
static void Read_Datas_ICM42688(unsigned char reg, unsigned char *dat, unsigned int num);
static void Write_Data_ICM42688(unsigned char reg, unsigned char dat);
enum icm42688_afs
{
ICM42688_AFS_16G,// default
ICM42688_AFS_8G,
ICM42688_AFS_4G,
ICM42688_AFS_2G,
NUM_ICM42688__AFS
};
enum icm42688_aodr
{
ICM42688_AODR_32000HZ,
ICM42688_AODR_16000HZ,
ICM42688_AODR_8000HZ,
ICM42688_AODR_4000HZ,
ICM42688_AODR_2000HZ,
ICM42688_AODR_1000HZ,// default
ICM42688_AODR_200HZ,
ICM42688_AODR_100HZ,
ICM42688_AODR_50HZ,
ICM42688_AODR_25HZ,
ICM42688_AODR_12_5HZ,
ICM42688_AODR_6_25HZ,
ICM42688_AODR_3_125HZ,
ICM42688_AODR_1_5625HZ,
ICM42688_AODR_500HZ,
NUM_ICM42688_AODR
};
enum icm42688_gfs
{
ICM42688_GFS_2000DPS,// default
ICM42688_GFS_1000DPS,
ICM42688_GFS_500DPS,
ICM42688_GFS_250DPS,
ICM42688_GFS_125DPS,
ICM42688_GFS_62_5DPS,
ICM42688_GFS_31_25DPS,
ICM42688_GFS_15_625DPS,
NUM_ICM42688_GFS
};
enum icm42688_godr
{
ICM42688_GODR_32000HZ,
ICM42688_GODR_16000HZ,
ICM42688_GODR_8000HZ,
ICM42688_GODR_4000HZ,
ICM42688_GODR_2000HZ,
ICM42688_GODR_1000HZ,// default
ICM42688_GODR_200HZ,
ICM42688_GODR_100HZ,
ICM42688_GODR_50HZ,
ICM42688_GODR_25HZ,
ICM42688_GODR_12_5HZ,
ICM42688_GODR_X0HZ,
ICM42688_GODR_X1HZ,
ICM42688_GODR_X2HZ,
ICM42688_GODR_500HZ,
NUM_ICM42688_GODR
};
void gpio_init(void);
/**
*
* @brief ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>dz<EFBFBD>ʼ<EFBFBD><CABC>
* @param
* @return void
* @notes <20>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
* Example: Init_ICM42688();
*
**/
void Init_ICM42688(void);
//void get_IMU(float accx,float accy,float accz,float *pitch,float *roll);
void get_IMU(float *pitch,float *roll);
uint8_t sys_gpio_pin_get(GPIO_TypeDef *p_gpiox, uint16_t pinx);
void sys_gpio_pin_set(GPIO_TypeDef *p_gpiox, uint16_t pinx, uint8_t status);
/**
*
* @brief <20><><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><C7BC>ٶ<EFBFBD>
* @param
* @return void
* @notes <20><>λ:g(m/s^2),<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
* Example: Get_Acc_ICM42688();
*
**/
void Get_Acc_ICM42688(void);
/**
*
* @brief <20><><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>ǽǼ<C7BD><C7BC>ٶ<EFBFBD>
* @param
* @return void
* @notes <20><>λΪ:<3A><>/s,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
* Example: Get_Gyro_ICM42688();
*
**/
void Get_Gyro_ICM42688(void);
/**
*
* @brief <20><><EFBFBD><EFBFBD>ICM42688<38><38><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param afs // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param aodr // <20><><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param gfs // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @param godr // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>dmx_icm42688.h<>ļ<EFBFBD><C4BC><EFBFBD>ö<EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD>в鿴
* @return void
* @notes ICM42688.c<>ļ<EFBFBD><C4BC>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>,<2C>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><C3B3><EFBFBD>
* Example: Set_LowpassFilter_Range_ICM42688(ICM42688_AFS_16G,ICM42688_AODR_32000HZ,ICM42688_GFS_2000DPS,ICM42688_GODR_32000HZ);
*
**/
void Set_LowpassFilter_Range_ICM42688(enum icm42688_afs afs, enum icm42688_aodr aodr, enum icm42688_gfs gfs, enum icm42688_godr godr);
uint8_t MpuGetData(void);
// ICM42688Bank0<6B>ڲ<EFBFBD><DAB2><EFBFBD>ַ
#define ICM42688_DEVICE_CONFIG 0x11
#define ICM42688_DRIVE_CONFIG 0x13
#define ICM42688_INT_CONFIG 0x14
#define ICM42688_FIFO_CONFIG 0x16
#define ICM42688_TEMP_DATA1 0x1D
#define ICM42688_TEMP_DATA0 0x1E
#define ICM42688_ACCEL_DATA_X1 0x1F
#define ICM42688_ACCEL_DATA_X0 0x20
#define ICM42688_ACCEL_DATA_Y1 0x21
#define ICM42688_ACCEL_DATA_Y0 0x22
#define ICM42688_ACCEL_DATA_Z1 0x23
#define ICM42688_ACCEL_DATA_Z0 0x24
#define ICM42688_GYRO_DATA_X1 0x25
#define ICM42688_GYRO_DATA_X0 0x26
#define ICM42688_GYRO_DATA_Y1 0x27
#define ICM42688_GYRO_DATA_Y0 0x28
#define ICM42688_GYRO_DATA_Z1 0x29
#define ICM42688_GYRO_DATA_Z0 0x2A
#define ICM42688_TMST_FSYNCH 0x2B
#define ICM42688_TMST_FSYNCL 0x2C
#define ICM42688_INT_STATUS 0x2D
#define ICM42688_FIFO_COUNTH 0x2E
#define ICM42688_FIFO_COUNTL 0x2F
#define ICM42688_FIFO_DATA 0x30
#define ICM42688_APEX_DATA0 0x31
#define ICM42688_APEX_DATA1 0x32
#define ICM42688_APEX_DATA2 0x33
#define ICM42688_APEX_DATA3 0x34
#define ICM42688_APEX_DATA4 0x35
#define ICM42688_APEX_DATA5 0x36
#define ICM42688_INT_STATUS2 0x37
#define ICM42688_INT_STATUS3 0x38
#define ICM42688_SIGNAL_PATH_RESET 0x4B
#define ICM42688_INTF_CONFIG0 0x4C
#define ICM42688_INTF_CONFIG1 0x4D
#define ICM42688_PWR_MGMT0 0x4E
#define ICM42688_GYRO_CONFIG0 0x4F
#define ICM42688_ACCEL_CONFIG0 0x50
#define ICM42688_GYRO_CONFIG1 0x51
#define ICM42688_GYRO_ACCEL_CONFIG0 0x52
#define ICM42688_ACCEL_CONFIG1 0x53
#define ICM42688_TMST_CONFIG 0x54
#define ICM42688_APEX_CONFIG0 0x56
#define ICM42688_SMD_CONFIG 0x57
#define ICM42688_FIFO_CONFIG1 0x5F
#define ICM42688_FIFO_CONFIG2 0x60
#define ICM42688_FIFO_CONFIG3 0x61
#define ICM42688_FSYNC_CONFIG 0x62
#define ICM42688_INT_CONFIG0 0x63
#define ICM42688_INT_CONFIG1 0x64
#define ICM42688_INT_SOURCE0 0x65
#define ICM42688_INT_SOURCE1 0x66
#define ICM42688_INT_SOURCE3 0x68
#define ICM42688_INT_SOURCE4 0x69
#define ICM42688_FIFO_LOST_PKT0 0x6C
#define ICM42688_FIFO_LOST_PKT1 0x6D
#define ICM42688_SELF_TEST_CONFIG 0x70
#define ICM42688_WHO_AM_I 0x75
#define ICM42688_REG_BANK_SEL 0x76 // Banks
#define ICM42688_SENSOR_CONFIG0 0x03
#define ICM42688_GYRO_CONFIG_STATIC2 0x0B
#define ICM42688_GYRO_CONFIG_STATIC3 0x0C
#define ICM42688_GYRO_CONFIG_STATIC4 0x0D
#define ICM42688_GYRO_CONFIG_STATIC5 0x0E
#define ICM42688_GYRO_CONFIG_STATIC6 0x0F
#define ICM42688_GYRO_CONFIG_STATIC7 0x10
#define ICM42688_GYRO_CONFIG_STATIC8 0x11
#define ICM42688_GYRO_CONFIG_STATIC9 0x12
#define ICM42688_GYRO_CONFIG_STATIC10 0x13
#define ICM42688_XG_ST_DATA 0x5F
#define ICM42688_YG_ST_DATA 0x60
#define ICM42688_ZG_ST_DATA 0x61
#define ICM42688_TMSTVAL0 0x62
#define ICM42688_TMSTVAL1 0x63
#define ICM42688_TMSTVAL2 0x64
#define ICM42688_INTF_CONFIG4 0x7A
#define ICM42688_INTF_CONFIG5 0x7B
#define ICM42688_INTF_CONFIG6 0x7C
#endif

135
IMU/imu.c Normal file
View File

@ -0,0 +1,135 @@
#include "imu.h"
#include <math.h>
//const float M_PI = 3.1415926535;
const float RtA = 57.2957795f;
const float AtR = 0.0174532925f;
const float Gyro_G = 0.03051756f*2; //<2F><><EFBFBD><EFBFBD><EFBFBD>dz<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+-2000<30><30>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>1 / (65536 / 4000) = 0.03051756*2
const float Gyro_Gr = 0.0005326f*2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>,ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD> 2*0.03051756 * 0.0174533f = 0.0005326*2
static float NormAcc;
/* <20><>Ԫ<EFBFBD><D4AA>ϵ<EFBFBD><CFB5> */
typedef volatile struct {
float q0;
float q1;
float q2;
float q3;
} Quaternion;
Quaternion NumQ = {1, 0, 0, 0};
/* <20><><EFBFBD><EFBFBD><EFBFBD>ǻ<EFBFBD><C7BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
struct V{
float x;
float y;
float z;
};
volatile struct V GyroIntegError = {0};
_st_AngE Angle; //<2F><>ǰ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>ֵ̬
/* <20><>Ԫ<EFBFBD><D4AA><EFBFBD>ⷨ<EFBFBD><E2B7A8>ʼ<EFBFBD><CABC> */
void imu_rest(void)
{
NumQ.q0 =1;
NumQ.q1 = 0;
NumQ.q2 = 0;
NumQ.q3 = 0;
GyroIntegError.x = 0;
GyroIntegError.y = 0;
GyroIntegError.z = 0;
Angle.pitch = 0;
Angle.roll = 0;
}
double Time_Prev;
void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE)
{
unsigned long Time_Now;
double Ts;
Time_Now = __HAL_TIM_GET_COUNTER(&htim5); //100ns
if(Time_Now > Time_Prev)Ts = (float)(Time_Now - Time_Prev)*1e-7f;
else
Ts = (float)(0xFFFFFFFF - Time_Prev + Time_Now)*1e-7f;
Time_Prev = Time_Now;
if(Ts == 0 || Ts > 400) Ts = 1e-3f;
volatile struct V Gravity,Acc,Gyro,AccGravity;
static float KpDef = 0.5f ;
static float KiDef = 0.0001f;
//static float KiDef = 0.00001f;
float q0_t,q1_t,q2_t,q3_t;
//float NormAcc;
float NormQuat;
float HalfTime = Ts * 0.5f;
//<2F><>ȡ<EFBFBD><C8A1>Ч<EFBFBD><D0A7>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Gravity.x = 2*(NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
Gravity.y = 2*(NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
Gravity.z = 1-2*(NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);
// <20><><EFBFBD>ٶȹ<D9B6>һ<EFBFBD><D2BB>
NormAcc = 1/sqrt(squa(ICM42688.accX)+ squa(ICM42688.accY) +squa(ICM42688.accZ));
Acc.x = pMpu->accX * NormAcc;
Acc.y = pMpu->accY * NormAcc;
Acc.z = pMpu->accZ * NormAcc;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵ó<CBB5><C3B3><EFBFBD>ֵ
AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);
AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);
AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȻ<D9B6><C8BB>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȵIJ<C8B5><C4B2><EFBFBD>ֵ
GyroIntegError.x += AccGravity.x * KiDef;
GyroIntegError.y += AccGravity.y * KiDef;
GyroIntegError.z += AccGravity.z * KiDef;
//<2F><><EFBFBD>ٶ<EFBFBD><D9B6>ںϼ<DABA><CFBC>ٶȻ<D9B6><C8BB>ֲ<EFBFBD><D6B2><EFBFBD>ֵ
Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;
// һ<><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
q0_t = (-NumQ.q1*Gyro.x - NumQ.q2*Gyro.y - NumQ.q3*Gyro.z) * HalfTime;
q1_t = ( NumQ.q0*Gyro.x - NumQ.q3*Gyro.y + NumQ.q2*Gyro.z) * HalfTime;
q2_t = ( NumQ.q3*Gyro.x + NumQ.q0*Gyro.y - NumQ.q1*Gyro.z) * HalfTime;
q3_t = (-NumQ.q2*Gyro.x + NumQ.q1*Gyro.y + NumQ.q0*Gyro.z) * HalfTime;
NumQ.q0 += q0_t;
NumQ.q1 += q1_t;
NumQ.q2 += q2_t;
NumQ.q3 += q3_t;
// <20><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
NormQuat = 1/sqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));
NumQ.q0 *= NormQuat;
NumQ.q1 *= NormQuat;
NumQ.q2 *= NormQuat;
NumQ.q3 *= NormQuat;
// <20><>Ԫ<EFBFBD><D4AA>תŷ<D7AA><C5B7><EFBFBD><EFBFBD>
{
#ifdef YAW_GYRO
*(
float *)pAngE = atan2f(2 * NumQ.q1 *NumQ.q2 + 2 * NumQ.q0 * NumQ.q3, 1 - 2 * NumQ.q2 *NumQ.q2 - 2 * NumQ.q3 * NumQ.q3) * RtA; //yaw
#else
float yaw_G = pMpu->gyroZ * Gyro_G;
if((yaw_G > 1.0f) || (yaw_G < -1.0f)) //<2F><><EFBFBD><EFBFBD>̫С<CCAB><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>Ǹ<EFBFBD><C7B8>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
pAngE->yaw += yaw_G * Ts;
}
#endif
pAngE->pitch = asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3) * RtA;
pAngE->roll = atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2) * RtA;
// pAngE->pitch = asin(2 * NumQ.q0 *NumQ.q2 - 2 * NumQ.q1 * NumQ.q3);
// pAngE->roll = atan2(2 * NumQ.q2 *NumQ.q3 + 2 * NumQ.q0 * NumQ.q1, 1 - 2 * NumQ.q1 *NumQ.q1 - 2 * NumQ.q2 * NumQ.q2);
}
}

16
IMU/imu.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef __IMU_H
#define __IMU_H
#include "mymain.h"
#include "alldata.h"
#define squa( Sq ) (((float)Sq)*((float)Sq))
extern _st_AngE Angle;//<2F><>ǰ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD>ֵ̬
void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE);
// extern void GetAngle(const _st_Mpu *pMpu,_st_AngE *pAngE, float dt);
extern void imu_rest(void);
#endif

12
IMU/kalman.c Normal file
View File

@ -0,0 +1,12 @@
#include "kalman.h"
//һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>
void kalmanfiter(struct KalmanFilter *EKF,float input)
{
EKF->NewP = EKF->LastP + EKF->Q;
EKF->Kg = EKF->NewP / (EKF->NewP + EKF->R);
EKF->Out = EKF->Out + EKF->Kg * (input - EKF->Out);
EKF->LastP = (1 - EKF->Kg) * EKF->NewP;
}

18
IMU/kalman.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef __KALMAN_H
#define __KALMAN_H
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
struct KalmanFilter{
float LastP; //<2F><>һ<EFBFBD><D2BB>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float NewP; //<2F><><EFBFBD>µ<EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float Out; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Kg; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float Q; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
float R; //<2F>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>
};
extern void kalmanfiter(struct KalmanFilter *EKF,float input); //һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>
#endif